Merge pull request #12 from RuikerLiang/master

Author: nice-mee

interfaces/operation_interface/msg/RobotState.msg

@@ -1,19 +1,10 @@
 uint8 robot_id
 uint8 robot_level
-uint16 remain_hp
-uint16 max_hp
-uint16 shooter_id1_17mm_cooling_rate
-uint16 shooter_id1_17mm_cooling_limit
-uint16 shooter_id1_17mm_speed_limit
-
-uint16 shooter_id2_17mm_cooling_rate
-uint16 shooter_id2_17mm_cooling_limit
-uint16 shooter_id2_17mm_speed_limit
-uint16 shooter_id1_42mm_cooling_rate
-uint16 shooter_id1_42mm_cooling_limit
-uint16 shooter_id1_42mm_speed_limit
-
+uint16 current_hp
+uint16 maximum_hp
+uint16 shooter_barrel_cooling_value
+uint16 shooter_barrel_heat_limit
 uint16 chassis_power_limit
-bool mains_power_gimbal_output
-bool mains_power_chassis_output
-bool mains_power_shooter_output
+bool power_management_gimbal_output
+bool power_management_chassis_output
+bool power_management_shooter_output

perception/referee_serial/include/referee_serial/robot_state.hpp

@@ -27,25 +27,16 @@ class RobotState
 
         struct [[gnu::packed]] Data
         {
-            uint8_t robot_id; 
-            uint8_t robot_level; 
-            uint16_t remain_HP; 
-            uint16_t max_HP; 
-            uint16_t shooter_id1_17mm_cooling_rate; 
-            uint16_t shooter_id1_17mm_cooling_limit; 
-            uint16_t shooter_id1_17mm_speed_limit; 
- 
-            uint16_t shooter_id2_17mm_cooling_rate; 
-            uint16_t shooter_id2_17mm_cooling_limit; 
-            uint16_t shooter_id2_17mm_speed_limit; 
-            uint16_t shooter_id1_42mm_cooling_rate; 
-            uint16_t shooter_id1_42mm_cooling_limit; 
-            uint16_t shooter_id1_42mm_speed_limit; 
-                        
-            uint16_t chassis_power_limit; 
-            uint8_t mains_power_gimbal_output : 1; 
-            uint8_t mains_power_chassis_output : 1; 
-            uint8_t mains_power_shooter_output : 1;
+            uint8_t robot_id;
+            uint8_t robot_level;
+            uint16_t current_HP;
+            uint16_t maximum_HP;
+            uint16_t shooter_barrel_cooling_value;
+            uint16_t shooter_barrel_heat_limit;
+            uint16_t chassis_power_limit;
+            uint8_t power_management_gimbal_output : 1;
+            uint8_t power_management_chassis_output : 1;
+            uint8_t power_management_shooter_output : 1;
         };
 
         Header header;

perception/referee_serial/src/referee_serial.cpp

@@ -10,6 +10,7 @@
 #include <rclcpp/utilities.hpp>
 #include <string>
 #include <vector>
+#include <queue>
 #include "referee_serial/crc.h"
 
 #define DEBUG false
@@ -74,14 +75,19 @@ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr RefereeSerial::get_node_ba
 
 void RefereeSerial::receive()
 {
-    std::vector<uint8_t> prefix(7); // header + cmd_id
+    std::vector<uint8_t> prefix; // header + cmd_id
+    std::vector<uint8_t> receive_bit(1);
     RCLCPP_INFO(node_->get_logger(), "Receiving serial frames");
 
     while (rclcpp::ok())
     {
         try {
-            serial_driver_->port()->receive(prefix);
-
+            serial_driver_->port()->receive(receive_bit);
+            prefix.push_back(receive_bit[0]);
+            if (prefix.size() < 7)
+                continue;
+            while (prefix.size() > 7)
+                prefix.erase(prefix.begin());
             if (KeyMouse::is_wanted_pre(prefix)) // key mouse
             {
                 handle_frame<operation_interface::msg::KeyMouse, KeyMouse>(

perception/referee_serial/src/robot_state.cpp

@@ -2,46 +2,39 @@
 #include <bitset>
 #include <cstdint>
 #include <vector>
+#include <iostream>
 
 bool RobotState::is_wanted_pre(const std::vector<uint8_t> &prefix)
 {
-    if (prefix[0] != 0xA5) return false;
+    if (prefix[0] != 0xA5)
+        return false;
 
     uint16_t length = static_cast<uint16_t>(prefix[1]) | (static_cast<uint16_t>(prefix[2]) << 8);
     uint16_t cmd_id = static_cast<uint16_t>(prefix[5]) | (static_cast<uint16_t>(prefix[6]) << 8);
 
-    if (cmd_id != 0x0201 || length != 27) return false;
+    if (cmd_id != 0x0201 || length != 13)
+        return false;
     return true;
 }
 
 RobotState::RobotState(const std::vector<uint8_t> &frame)
 {
     // copy the uint8_t vector to the struct
-    std::copy(frame.begin(), frame.end(), reinterpret_cast<uint8_t*>(&interpreted));
+    std::copy(frame.begin(), frame.end(), reinterpret_cast<uint8_t *>(&interpreted));
 }
 
 operation_interface::msg::RobotState RobotState::msg()
 {
     operation_interface::msg::RobotState msg;
     msg.robot_id = interpreted.data.robot_id;
     msg.robot_level = interpreted.data.robot_level;
-    msg.remain_hp = interpreted.data.remain_HP;
-    msg.max_hp = interpreted.data.max_HP;
-    msg.shooter_id1_17mm_cooling_rate = interpreted.data.shooter_id1_17mm_cooling_rate;
-    msg.shooter_id1_17mm_cooling_limit = interpreted.data.shooter_id1_17mm_cooling_limit;
-    msg.shooter_id1_17mm_speed_limit = interpreted.data.shooter_id1_17mm_speed_limit;
-  
-    msg.shooter_id2_17mm_cooling_rate = interpreted.data.shooter_id2_17mm_cooling_rate;
-    msg.shooter_id2_17mm_cooling_limit = interpreted.data.shooter_id2_17mm_cooling_limit;
-    msg.shooter_id2_17mm_speed_limit = interpreted.data.shooter_id2_17mm_speed_limit;
-  
-    msg.shooter_id1_42mm_cooling_rate = interpreted.data.shooter_id1_42mm_cooling_rate;
-    msg.shooter_id1_42mm_cooling_limit = interpreted.data.shooter_id1_42mm_cooling_limit;
-    msg.shooter_id1_42mm_speed_limit = interpreted.data.shooter_id1_42mm_speed_limit;
-
+    msg.current_hp = interpreted.data.current_HP;
+    msg.maximum_hp = interpreted.data.maximum_HP;
+    msg.shooter_barrel_cooling_value = interpreted.data.shooter_barrel_cooling_value;
+    msg.shooter_barrel_heat_limit = interpreted.data.shooter_barrel_heat_limit;
     msg.chassis_power_limit = interpreted.data.chassis_power_limit;
-    msg.mains_power_gimbal_output = interpreted.data.mains_power_gimbal_output;
-    msg.mains_power_chassis_output = interpreted.data.mains_power_chassis_output;
-    msg.mains_power_shooter_output = interpreted.data.mains_power_shooter_output;
+    msg.power_management_gimbal_output = interpreted.data.power_management_gimbal_output;
+    msg.power_management_chassis_output = interpreted.data.power_management_chassis_output;
+    msg.power_management_shooter_output = interpreted.data.power_management_shooter_output;
     return msg;
 }