src: cleanup sensor state msgs

westonrobot · Jan 10, 2024 · 8c97c60 · 8c97c60
1 parent d88be70
commit 8c97c60
Show file tree

Hide file tree

Showing 11 changed files with 69 additions and 29 deletions.
diff --git a/include/ugv_sdk/details/interface/hunter_interface.hpp b/include/ugv_sdk/details/interface/hunter_interface.hpp
@@ -34,12 +34,13 @@ struct HunterActuatorState {
   ActuatorStateMessageV1 actuator_state[3];
 };
 
-struct HunterCommonSensorState {
-  AgxMsgTimeStamp time_stamp;
+struct HunterSensorState {
+  SdkTimePoint time_stamp;
 
   BmsBasicMessage bms_basic_state;
   BmsExtendedMessage bms_extend_state;
 };
+
 struct HunterInterface {
   virtual ~HunterInterface() = default;
 
@@ -51,7 +52,7 @@ struct HunterInterface {
   // get robot state
   virtual HunterCoreState GetRobotState() = 0;
   virtual HunterActuatorState GetActuatorState() = 0;
-  virtual HunterCommonSensorState GetCommonSensorState() = 0;
+  virtual HunterSensorState GetSensorState() = 0;
 };
 }  // namespace westonrobot
 

diff --git a/include/ugv_sdk/details/interface/ranger_interface.hpp b/include/ugv_sdk/details/interface/ranger_interface.hpp
@@ -27,7 +27,6 @@ struct RangerCoreState {
 
   RcStateMessage rc_state;
   OdometryMessage odometry;
-  BmsBasicMessage bms_basic_state;
 };
 
 struct RangerActuatorState {
@@ -40,6 +39,12 @@ struct RangerActuatorState {
   ActuatorLSStateMessage actuator_ls_state[8];
 };
 
+struct RangerSensorState {
+  SdkTimePoint time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+};
+
 /////////////////////////////////////////////////////////////////////////
 
 struct RangerInterface {
@@ -64,6 +69,7 @@ struct RangerInterface {
   // get robot state
   virtual RangerCoreState GetRobotState() = 0;
   virtual RangerActuatorState GetActuatorState() = 0;
+  virtual RangerSensorState GetSensorState() = 0;
 };
 }  // namespace westonrobot
 

diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp
@@ -23,7 +23,6 @@ struct ScoutCoreState {
   MotionStateMessage motion_state;
   LightStateMessage light_state;
   RcStateMessage rc_state;
-  BmsBasicMessage bms_basic_state;
 };
 
 struct ScoutActuatorState {
@@ -37,6 +36,12 @@ struct ScoutActuatorState {
   ActuatorStateMessageV1 actuator_state[4];
 };
 
+struct ScoutSensorState {
+  SdkTimePoint time_stamp;
+
+  BmsBasicMessage bms_basic_state;
+};
+
 struct ScoutInterface {
   virtual ~ScoutInterface() = default;
 

diff --git a/include/ugv_sdk/details/robot_base/agilex_base.hpp b/include/ugv_sdk/details/robot_base/agilex_base.hpp
@@ -31,8 +31,6 @@ struct CoreStateMsgGroup {
   LightStateMessage light_state;
   MotionModeStateMessage motion_mode_state;
   RcStateMessage rc_state;
-
-  BmsBasicMessage bms_basic_state;
 };
 
 struct ActuatorStateMsgGroup {
@@ -48,6 +46,7 @@ struct ActuatorStateMsgGroup {
 
 struct SensorStateMsgGroup {
   SdkTimePoint time_stamp;
+  BmsBasicMessage bms_basic_state;
   BmsExtendedMessage bms_extend_state;
 };
 
@@ -319,12 +318,12 @@ class AgilexBase : public RobotCommonInterface {
         core_state_msgs_.rc_state = status_msg.body.rc_state_msg;
         break;
       }
-      case AgxMsgBmsBasic: {
-        //      std::cout << "system status feedback received" << std::endl;
-        core_state_msgs_.time_stamp = SdkClock::now();
-        core_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg;
-        break;
-      }
+      // case AgxMsgBmsBasic: {
+      //   //      std::cout << "system status feedback received" << std::endl;
+      //   core_state_msgs_.time_stamp = SdkClock::now();
+      //   core_state_msgs_.bms_basic_state = status_msg.body.bms_basic_msg;
+      //   break;
+      // }
       default:
         break;
     }
@@ -389,7 +388,7 @@ class AgilexBase : public RobotCommonInterface {
     switch (status_msg.type) {
       case AgxMsgBmsBasic: {
         //      std::cout << "system status feedback received" << std::endl;
-        common_sensor_state_msgs_.time_stamp = AgxMsgRefClock::now();
+        common_sensor_state_msgs_.time_stamp = SdkClock::now();
         common_sensor_state_msgs_.bms_basic_state =
             status_msg.body.bms_basic_msg;
         break;

diff --git a/include/ugv_sdk/details/robot_base/hunter_base.hpp b/include/ugv_sdk/details/robot_base/hunter_base.hpp
@@ -61,11 +61,11 @@ class HunterBase : public AgilexBase<ParserType>, public HunterInterface {
     return hunter_actuator;
   }
 
-  HunterCommonSensorState GetCommonSensorState() override {
+  HunterSensorState GetSensorState() override {
     auto common_sensor =
-        AgilexBase<ParserType>::GetCommonSensorStateMsgGroup();
+        AgilexBase<ParserType>::GetSensorStateMsgGroup();
 
-    HunterCommonSensorState hunter_bms;
+    HunterSensorState hunter_bms;
 
     hunter_bms.time_stamp = common_sensor.time_stamp;
 

diff --git a/include/ugv_sdk/details/robot_base/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp
@@ -63,17 +63,17 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     ranger_state.rc_state = state.rc_state;
     ranger_state.motion_mode_state = state.motion_mode_state;
 
-    ranger_state.bms_basic_state.current = state.bms_basic_state.current;
+    // ranger_state.bms_basic_state.current = state.bms_basic_state.current;
     // Note: BMS CAN message definition is not consistent across AgileX robots.
     // Robots with steering mechanism should additionally divide the voltage by
     // 10.
-    ranger_state.bms_basic_state.voltage = state.bms_basic_state.voltage * 0.1f;
-    ranger_state.bms_basic_state.battery_soc =
-        state.bms_basic_state.battery_soc;
-    ranger_state.bms_basic_state.battery_soh =
-        state.bms_basic_state.battery_soh;
-    ranger_state.bms_basic_state.temperature =
-        state.bms_basic_state.temperature;
+    // ranger_state.bms_basic_state.voltage = state.bms_basic_state.voltage * 0.1f;
+    // ranger_state.bms_basic_state.battery_soc =
+    //     state.bms_basic_state.battery_soc;
+    // ranger_state.bms_basic_state.battery_soh =
+    //     state.bms_basic_state.battery_soh;
+    // ranger_state.bms_basic_state.temperature =
+    //     state.bms_basic_state.temperature;
 
     return ranger_state;
   }
@@ -99,6 +99,30 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     }
     return ranger_actuator;
   }
+
+  RangerSensorState GetSensorState() override {
+    auto common_sensor =
+        AgilexBase<ProtocolV2Parser>::GetSensorStateMsgGroup();
+
+    RangerSensorState ranger_bms;
+
+    ranger_bms.time_stamp = common_sensor.time_stamp;
+
+    ranger_bms.bms_basic_state.current = common_sensor.bms_basic_state.current;
+    // Note: BMS CAN message definition is not consistent across AgileX robots.
+    // Robots with steering mechanism should additionally divide the voltage by
+    // 10.
+    ranger_bms.bms_basic_state.voltage =
+        common_sensor.bms_basic_state.voltage * 0.1f;
+    ranger_bms.bms_basic_state.battery_soc =
+        common_sensor.bms_basic_state.battery_soc;
+    ranger_bms.bms_basic_state.battery_soh =
+        common_sensor.bms_basic_state.battery_soh;
+    ranger_bms.bms_basic_state.temperature =
+        common_sensor.bms_basic_state.temperature;
+
+    return ranger_bms;
+  }
 };
 
 // Note: Ranger Mini V1 uses a modified AgileX V2 protocol

diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp
@@ -56,7 +56,7 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     scout_state.motion_state = state.motion_state;
     scout_state.light_state = state.light_state;
     scout_state.rc_state = state.rc_state;
-    scout_state.bms_basic_state = state.bms_basic_state;
+    // scout_state.bms_basic_state = state.bms_basic_state;
     return scout_state;
   }
 

diff --git a/include/ugv_sdk/mobile_robot/hunter_robot.hpp b/include/ugv_sdk/mobile_robot/hunter_robot.hpp
@@ -39,7 +39,7 @@ class HunterRobot : public RobotCommonInterface, public HunterInterface {
   // get robot state
   HunterCoreState GetRobotState() override;
   HunterActuatorState GetActuatorState() override;
-  HunterCommonSensorState GetCommonSensorState() override;
+  HunterSensorState GetSensorState() override;
 
  private:
   RobotCommonInterface* robot_;

diff --git a/include/ugv_sdk/mobile_robot/ranger_robot.hpp b/include/ugv_sdk/mobile_robot/ranger_robot.hpp
@@ -44,6 +44,7 @@ class RangerRobot : public RobotCommonInterface, public RangerInterface {
   // get robot state
   RangerCoreState GetRobotState() override;
   RangerActuatorState GetActuatorState() override;
+  RangerSensorState GetSensorState() override;
 
  private:
   RobotCommonInterface* robot_;

diff --git a/src/mobile_robot/hunter_robot.cpp b/src/mobile_robot/hunter_robot.cpp
@@ -63,8 +63,8 @@ HunterActuatorState HunterRobot::GetActuatorState() {
   auto hunter = dynamic_cast<HunterInterface*>(robot_);
   return hunter->GetActuatorState();
 }
-HunterCommonSensorState HunterRobot::GetCommonSensorState() {
+HunterSensorState HunterRobot::GetSensorState() {
   auto hunter = dynamic_cast<HunterInterface*>(robot_);
-  return hunter->GetCommonSensorState();
+  return hunter->GetSensorState();
 }
 }  // namespace westonrobot
diff --git a/src/mobile_robot/ranger_robot.cpp b/src/mobile_robot/ranger_robot.cpp
@@ -67,4 +67,8 @@ RangerActuatorState RangerRobot::GetActuatorState() {
   auto ranger = dynamic_cast<RangerInterface*>(robot_);
   return ranger->GetActuatorState();
 }
+RangerSensorState RangerRobot::GetSensorState() {
+  auto ranger = dynamic_cast<RangerInterface*>(robot_);
+  return ranger->GetSensorState();
+}
 }  // namespace westonrobot