[Core] add Debug/DeviceCmd for sent cmd display

Robocup-ssl-China · Apr 15, 2024 · 1571e08 · 1571e08
1 parent 93eaa98
commit 1571e08
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 3 deletions.
diff --git a/Core/src/Simulator/CommandInterface.cpp b/Core/src/Simulator/CommandInterface.cpp
@@ -19,6 +19,7 @@ int SIM_PORT = 0;
 int SELF_PORT = 0;
 int CHIP_ANGLE = 1;
 int TEAM;
+bool DEBUG_CMD = false;
 ZSS::Protocol::Robots_Command robots_command;
 std::thread *_thread = nullptr;
 }
@@ -31,6 +32,8 @@ CCommandInterface::CCommandInterface(const COptionModule *pOption, QObject *pare
     ZSS::ZParamManager::instance()->loadParam(SELF_PORT, "Ports/SimSelfPort", 30015);
     ZSS::ZParamManager::instance()->loadParam(CHIP_ANGLE, "Simulator/ChipAngle", 45);
     ZSS::ZParamManager::instance()->loadParam(isYellow, "ZAlert/IsYellow", false);
+
+    ZSS::ZParamManager::instance()->loadParam(DEBUG_CMD,"Debug/DeviceCmd",false);
     TEAM = isYellow ? PARAM::YELLOW : PARAM::BLUE;
     command_socket = new QUdpSocket();
     receiveSocket = new QUdpSocket();
@@ -102,6 +105,41 @@ void CCommandInterface::sendCommands() {
             robot_command->set_power(commands[i].chip_kick);
         }
     }
+    if(DEBUG_CMD){
+        static const int DATA_SIZE = 7;
+        static const std::array<std::string, DATA_SIZE> titles{"BotID","Vel_X","Vel_Y","Vel_R","Dribb","KickM","Power"};
+        static const int FONT_SIZE = 50;
+        static const double DEBUG_TEXT_LENGTH = FONT_SIZE*3;
+        static const double DEBUG_X = PARAM::Field::PITCH_LENGTH / 2 - 3 * FONT_SIZE * (PARAM::Field::MAX_PLAYER) - DEBUG_TEXT_LENGTH;
+        static const double DEBUG_X_STEP = PARAM::Field::PITCH_LENGTH / 30;
+        static const double DEBUG_Y = PARAM::Field::PITCH_WIDTH / 2 - 5*FONT_SIZE;
+        static const double DEBUG_Y_STEP = -FONT_SIZE*2;
+        std::string title_txt = "";
+        for(auto title_str : titles){
+            title_txt += fmt::format(" {:>6s}",title_str);
+        }
+        GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y),title_txt,COLOR_PURPLE,0,FONT_SIZE);
+        for(int i=0;i<robots_command.command_size();i++){
+            auto&& cmd = robots_command.command(i);
+            std::array<std::string,DATA_SIZE> info_str{
+                fmt::format("{:>7d}",cmd.robot_id()),
+                fmt::format("{:>7.0f}",cmd.velocity_x()),
+                fmt::format("{:>7.0f}",cmd.velocity_y()),
+                fmt::format("{:>7.2f}",cmd.velocity_r()),
+                fmt::format("{:>7.2f}",cmd.dribbler_spin()),
+                fmt::format("{:>7s}",cmd.kick() ? "✓" : "✕"),
+                fmt::format("{:>7.0f}",cmd.power())
+            };
+            std::array<bool,DATA_SIZE> info_check{1, std::abs(cmd.velocity_x())>1, std::abs(cmd.velocity_y())>1, std::abs(cmd.velocity_r())>0.01, cmd.dribbler_spin()>0.1, cmd.kick(), cmd.power()>100};
+            std::string msg,msg_invalid;
+            for(int j=0;j<info_str.size();j++){
+                msg += fmt::format("{:>7s}",info_check[j] ? info_str[j] : "");
+                msg_invalid += fmt::format("{:>7s}",info_check[j] ? "" : "--");
+            }
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * (i+1)),msg,COLOR_PURPLE,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * (i+1)),msg_invalid,COLOR_GRAY,0,FONT_SIZE);
+        }
+    }
     int size = ::robots_command.ByteSizeLong();
     QByteArray data(size, 0);
     ::robots_command.SerializeToArray(data.data(), size);

diff --git a/Core/src/Wireless/RobotSensor.cpp b/Core/src/Wireless/RobotSensor.cpp
@@ -27,9 +27,9 @@ void CRobotSensor::Update(const CVisionModule* pVision){
             robotMsg[i]._mutex.unlock();
         }
         for(int i=0;i<DATA_SIZE;i++){
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * i),fmt::format("{:6s}: ",text[i]),COLOR_GRAY,0,FONT_SIZE);
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs[i],COLOR_GRAY,0,FONT_SIZE);
-            GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs2[i],COLOR_GREEN,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X, DEBUG_Y + DEBUG_Y_STEP * i),fmt::format("{:6s}: ",text[i]),COLOR_GRAY,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs[i],COLOR_GRAY,0,FONT_SIZE);
+            GDebugEngine::Instance()->gui_debug_msg_fix(CGeoPoint(DEBUG_X+DEBUG_TEXT_LENGTH,DEBUG_Y + DEBUG_Y_STEP * i),msgs2[i],COLOR_GREEN,0,FONT_SIZE);
         }
     }
     for(int i=0;i<PARAM::Field::MAX_PLAYER;i++){