src: fix scout bms feedback interface

westonrobot · Feb 5, 2024 · b0184f2 · b0184f2
1 parent b764db0
commit b0184f2
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 0 deletions.
diff --git a/include/ugv_sdk/details/interface/scout_interface.hpp b/include/ugv_sdk/details/interface/scout_interface.hpp
@@ -53,6 +53,7 @@ struct ScoutInterface {
   // get robot state
   virtual ScoutCoreState GetRobotState() = 0;
   virtual ScoutActuatorState GetActuatorState() = 0;
+  virtual ScoutCommonSensorState GetCommonSensorState() = 0;
 };
 
 struct ScoutOmniInterface {

diff --git a/include/ugv_sdk/details/robot_base/scout_base.hpp b/include/ugv_sdk/details/robot_base/scout_base.hpp
@@ -71,6 +71,18 @@ class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
     }
     return scout_actuator;
   }
+
+  ScoutCommonSensorState GetCommonSensorState() override {
+    auto common_sensor =
+        AgilexBase<ParserType>::GetCommonSensorStateMsgGroup();
+
+    ScoutCommonSensorState scout_bms;
+
+    scout_bms.time_stamp = common_sensor.time_stamp;
+    scout_bms.bms_basic_state = common_sensor.bms_basic_state;
+
+    return scout_bms;
+  }
 };
 
 template <typename ParserType>

diff --git a/include/ugv_sdk/mobile_robot/scout_robot.hpp b/include/ugv_sdk/mobile_robot/scout_robot.hpp
@@ -39,6 +39,7 @@ class ScoutRobot : public RobotCommonInterface, public ScoutInterface {
   // get robot state
   ScoutCoreState GetRobotState() override;
   ScoutActuatorState GetActuatorState() override;
+  ScoutCommonSensorState GetCommonSensorState() override;
 
  protected:
   RobotCommonInterface* robot_;

diff --git a/src/mobile_robot/scout_robot.cpp b/src/mobile_robot/scout_robot.cpp
@@ -72,6 +72,10 @@ ScoutActuatorState ScoutRobot::GetActuatorState() {
   auto scout = dynamic_cast<ScoutInterface*>(robot_);
   return scout->GetActuatorState();
 }
+ScoutCommonSensorState ScoutRobot::GetCommonSensorState() {
+  auto scout = dynamic_cast<ScoutInterface*>(robot_);
+  return scout->GetCommonSensorState();
+}
 
 ///////////////////////////////////////////////////////////////////////////