moved state update function to agilex base class

include/ugv_sdk/details/interface/agilex_message.h

@@ -127,7 +127,7 @@ typedef struct {
 typedef struct {
   uint8_t motion_mode;
   uint8_t mode_changing;
-} MotionModeFeedbackMessage;
+} MotionModeStateMessage;
 
 // V1-only messages
 typedef struct {
@@ -273,10 +273,11 @@ typedef enum {
   AgxMsgSystemState,
   AgxMsgMotionState,
   AgxMsgLightState,
+  AgxMsgMotionModeState,
   AgxMsgRcState,
+  // actuator feedback
   AgxMsgActuatorHSState,
   AgxMsgActuatorLSState,
-  AgxMsgMotionModeState,
   // sensor
   AgxMsgOdometry,
   AgxMsgImuAccel,
@@ -309,14 +310,15 @@ typedef struct {
     LightCommandMessage light_command_msg;
     BrakingCommandMessage braking_command_msg;
     MotionModeCommandMessage motion_mode_msg;
-    // state feedback
+    // core state feedback
     SystemStateMessage system_state_msg;
     MotionStateMessage motion_state_msg;
     LightStateMessage light_state_msg;
+    MotionModeStateMessage motion_mode_state_msg;
     RcStateMessage rc_state_msg;
+    // actuator feedback
     ActuatorHSStateMessage actuator_hs_state_msg;
     ActuatorLSStateMessage actuator_ls_state_msg;
-    MotionModeFeedbackMessage motion_mode_feedback_msg;
     // sensor
     OdometryMessage odometry_msg;
     ImuAccelMessage imu_accel_msg;

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -25,7 +25,7 @@ struct RangerState {
 
   ActuatorHSStateMessage actuator_hs_state[8];
   ActuatorLSStateMessage actuator_ls_state[8];
-  MotionModeFeedbackMessage current_motion_mode;
+  MotionModeStateMessage current_motion_mode;
 
   // sensor data
   OdometryMessage odometry;

include/ugv_sdk/details/interface/robot_interface.hpp

@@ -15,7 +15,25 @@
 #include "ugv_sdk/details/interface/agilex_message.h"
 #include "ugv_sdk/details/interface/parser_interface.hpp"
 
+#define AGX_MAX_ACTUATOR_NUM 8
+
 namespace westonrobot {
+struct CoreStateMsgGroup {
+  SystemStateMessage system_state;
+  MotionStateMessage motion_state;
+  LightStateMessage light_state;
+  MotionModeStateMessage motion_mode_state;
+  RcStateMessage rc_state;
+};
+
+struct ActuatorStateMsgGroup {
+  ActuatorHSStateMessage actuator_hs_state[AGX_MAX_ACTUATOR_NUM];  // v2 only
+  ActuatorLSStateMessage actuator_ls_state[AGX_MAX_ACTUATOR_NUM];  // v2 only
+  ActuatorStateMessageV1 actuator_state[AGX_MAX_ACTUATOR_NUM];     // v1 only
+};
+
+struct CommonSensorStateMsgGroup {};
+
 class RobotInterface {
  public:
   ~RobotInterface() = default;
@@ -38,6 +56,8 @@ class RobotInterface {
   /****** functions not available/valid to all robots ******/
   // functions to be implemented by class AgilexBase
   virtual void SetMotionMode(uint8_t mode){};
+  virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup(){};
+  virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup(){};
 
   // any specific robot will use a specialized version of the two functions
   virtual void SendMotionCommand(double linear_vel, double angular_vel,

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -15,11 +15,14 @@
 #include "ugv_sdk/details/interface/agilex_message.h"
 
 namespace westonrobot {
-struct ScoutState {
+struct ScoutCoreState {
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
+  RcStateMessage rc_state;
+};
 
+struct ScoutActuatorState {
   // actuator state
   // - for v2 robots only
   ActuatorHSStateMessage actuator_hs_state[4];
@@ -36,10 +39,8 @@ struct ScoutInterface {
                                LightMode r_mode, uint8_t r_value) = 0;
 
   // get robot state
-  virtual ScoutState GetRobotState() = 0;
-  virtual RcStateMessage GetRcState(){
-      // TODO
-  };
+  virtual ScoutCoreState GetRobotState() = 0;
+  virtual ScoutActuatorState GetActuatorState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -37,6 +37,11 @@ class AgilexBase : public RobotInterface {
   AgilexBase(const AgilexBase &hunter) = delete;
   AgilexBase &operator=(const AgilexBase &hunter) = delete;
 
+  void Connect(std::string can_name) override {
+    ConnectPort(can_name, std::bind(&AgilexBase<ParserType>::ParseCANFrame,
+                                    this, std::placeholders::_1));
+  }
+
   // switch to commanded mode
   void EnableCommandedMode() {
     // construct message
@@ -72,7 +77,7 @@ class AgilexBase : public RobotInterface {
 
       std::cout << "sending motion cmd: " << linear_vel << "," << angular_vel
                 << std::endl;
-                
+
       // send to can bus
       can_frame frame;
       if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
@@ -123,13 +128,38 @@ class AgilexBase : public RobotInterface {
     }
   }
 
+  void ResetRobotState() override {}
+
   ProtocolVersion GetProtocolVersion() override {
     return parser_.GetProtocolVersion();
   }
 
+  CoreStateMsgGroup GetRobotCoreStateMsgGroup() override {
+    std::lock_guard<std::mutex> guard(core_state_mtx_);
+    return core_state_msgs_;
+  }
+
+  ActuatorStateMsgGroup GetActuatorStateMsgGroup() override {
+    std::lock_guard<std::mutex> guard(actuator_state_mtx_);
+    return actuator_state_msgs_;
+  }
+
  protected:
   ParserType parser_;
-  std::mutex state_mutex_;
+
+  // divide feedback messages into smaller groups to avoid the
+  // state mutex being locked for too often such that accessing
+  // the data become difficult
+
+  /* feedback group 1: core state */
+  std::mutex core_state_mtx_;
+  CoreStateMsgGroup core_state_msgs_;
+
+  /* feedback group 2: actuator state */
+  std::mutex actuator_state_mtx_;
+  ActuatorStateMsgGroup actuator_state_msgs_;
+
+  /* feedback group 3: common sensor */
 
   // communication interface
   bool can_connected_ = false;
@@ -148,7 +178,66 @@ class AgilexBase : public RobotInterface {
     if (can_connected_) can_->StopService();
   }
 
-  virtual void ParseCANFrame(can_frame *rx_frame) = 0;
+  virtual void ParseCANFrame(can_frame *rx_frame) {
+    AgxMessage status_msg;
+    if (parser_.DecodeMessage(rx_frame, &status_msg)) {
+      UpdateRobotCoreState(status_msg);
+      UpdateActuatorState(status_msg);
+    }
+  }
+
+  void UpdateRobotCoreState(const AgxMessage &status_msg) {
+    std::lock_guard<std::mutex> guard(core_state_mtx_);
+    switch (status_msg.type) {
+      case AgxMsgSystemState: {
+        //   std::cout << "system status feedback received" << std::endl;
+        core_state_msgs_.system_state = status_msg.body.system_state_msg;
+        break;
+      }
+      case AgxMsgMotionState: {
+        // std::cout << "motion control feedback received" << std::endl;
+        core_state_msgs_.motion_state = status_msg.body.motion_state_msg;
+        break;
+      }
+      case AgxMsgLightState: {
+        // std::cout << "light control feedback received" << std::endl;
+        core_state_msgs_.light_state = status_msg.body.light_state_msg;
+        break;
+      }
+      default:
+        break;
+    }
+  }
+
+  void UpdateActuatorState(const AgxMessage &status_msg) {
+    std::lock_guard<std::mutex> guard(actuator_state_mtx_);
+    switch (status_msg.type) {
+      case AgxMsgActuatorHSState: {
+        // std::cout << "actuator hs feedback received" << std::endl;
+        actuator_state_msgs_
+            .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
+            status_msg.body.actuator_hs_state_msg;
+        break;
+      }
+      case AgxMsgActuatorLSState: {
+        // std::cout << "actuator ls feedback received" << std::endl;
+
+        actuator_state_msgs_
+            .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
+            status_msg.body.actuator_ls_state_msg;
+        break;
+      }
+      case AgxMsgActuatorStateV1: {
+        // std::cout << "actuator v1 feedback received" << std::endl;
+        actuator_state_msgs_
+            .actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
+            status_msg.body.v1_actuator_state_msg;
+        break;
+      }
+      default:
+        break;
+    }
+  }
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/ranger_base.cpp

@@ -96,7 +96,7 @@ void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
       break;
     }
     case AgxMsgMotionModeState: {
-      state.current_motion_mode = status_msg.body.motion_mode_feedback_msg;
+      state.current_motion_mode = status_msg.body.motion_mode_state_msg;
       break;
     }
     /* sensor feedback */

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -19,17 +19,15 @@
 #include "ugv_sdk/details/robot_base/agilex_base.hpp"
 
 namespace westonrobot {
-template <typename Parser>
-class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
+template <typename ParserType>
+class ScoutBase : public AgilexBase<ParserType>, public ScoutInterface {
  public:
-  ScoutBase() : AgilexBase<Parser>(){};
+  ScoutBase() : AgilexBase<ParserType>(){};
   ~ScoutBase() = default;
 
   // set up connection
   void Connect(std::string can_name) override {
-    AgilexBase<Parser>::ConnectPort(
-        can_name, std::bind(&ScoutBase<Parser>::ParseCANFrame, this,
-                            std::placeholders::_1));
+    AgilexBase<ParserType>::Connect(can_name);
   }
 
   void Connect(std::string uart_name, uint32_t baudrate) override {
@@ -38,92 +36,37 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
 
   // robot control
   void SetMotionCommand(double linear_vel, double angular_vel) override {
-    AgilexBase<Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+    AgilexBase<ParserType>::SendMotionCommand(linear_vel, angular_vel, 0.0,
+                                              0.0);
   }
 
   void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
                        uint8_t r_value) override {
-    AgilexBase<Parser>::SendLightCommand(f_mode, f_value, r_mode, r_value);
+    AgilexBase<ParserType>::SendLightCommand(f_mode, f_value, r_mode, r_value);
   }
 
   // get robot state
-  ScoutState GetRobotState() override {
-    std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
-    return scout_state_;
+  ScoutCoreState GetRobotState() override {
+    auto state = AgilexBase<ParserType>::GetRobotCoreStateMsgGroup();
+
+    ScoutCoreState scout_state;
+    scout_state.system_state = state.system_state;
+    scout_state.motion_state = state.motion_state;
+    scout_state.light_state = state.light_state;
+    scout_state.rc_state = state.rc_state;
+    return scout_state;
   }
 
-  RcStateMessage GetRcState() override {}
+  ScoutActuatorState GetActuatorState() override {
+    auto actuator = AgilexBase<ParserType>::GetActuatorStateMsgGroup();
 
-  void ResetRobotState() override {
-    std::lock_guard<std::mutex> guard(rc_state_mutex_);
-  }
-
- private:
-  ScoutState scout_state_;
-  RcStateMessage rc_state_;
-  std::mutex rc_state_mutex_;
-
-  void ParseCANFrame(can_frame *rx_frame) override {
-    AgxMessage status_msg;
-    if (AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
-      if (status_msg.type == AgxMsgRcState)
-        UpdateRcState(status_msg);
-      else
-        UpdateScoutState(status_msg, scout_state_);
-    }
-  }
-
-  void UpdateRcState(const AgxMessage &status_msg) {
-    std::lock_guard<std::mutex> guard(rc_state_mutex_);
-    rc_state_ = status_msg.body.rc_state_msg;
-  }
-
-  void UpdateScoutState(const AgxMessage &status_msg, ScoutState &state) {
-    std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
-    switch (status_msg.type) {
-      case AgxMsgSystemState: {
-        //   std::cout << "system status feedback received" << std::endl;
-        state.system_state = status_msg.body.system_state_msg;
-        break;
-      }
-      case AgxMsgMotionState: {
-        // std::cout << "motion control feedback received" << std::endl;
-        state.motion_state = status_msg.body.motion_state_msg;
-        break;
-      }
-      case AgxMsgLightState: {
-        // std::cout << "light control feedback received" << std::endl;
-        state.light_state = status_msg.body.light_state_msg;
-        break;
-      }
-      case AgxMsgActuatorHSState: {
-        // std::cout << "actuator hs feedback received" << std::endl;
-        state
-            .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
-            status_msg.body.actuator_hs_state_msg;
-        break;
-      }
-      case AgxMsgActuatorLSState: {
-        // std::cout << "actuator ls feedback received" << std::endl;
-        state
-            .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
-            status_msg.body.actuator_ls_state_msg;
-        break;
-      }
-      case AgxMsgActuatorStateV1: {
-        // std::cout << "actuator v1 feedback received" << std::endl;
-        state.actuator_state[status_msg.body.v1_actuator_state_msg.motor_id] =
-            status_msg.body.v1_actuator_state_msg;
-        break;
-      }
-        /* sensor feedback */
-        //   case AgxMsgOdometry: {
-        //     // std::cout << "Odometer msg feedback received" << std::endl;
-        //     state.odometry = status_msg.body.odometry_msg;
-        //   }
-      default:
-        break;
+    ScoutActuatorState scout_actuator;
+    for (int i = 0; i < 4; ++i) {
+      scout_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
+      scout_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
+      scout_actuator.actuator_state[i] = actuator.actuator_state[i];
     }
+    return scout_actuator;
   }
 };
 }  // namespace westonrobot

include/ugv_sdk/details/robot_base/tracer_base.hpp

@@ -21,7 +21,8 @@
 #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
 
 namespace westonrobot {
-class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface {
+class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>,
+                     public TracerInterface {
  public:
   TracerBaseV2() : AgilexBase<ProtocolV2Parser>(){};
   ~TracerBaseV2() = default;
@@ -29,8 +30,8 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface
   // set up connection
   void Connect(std::string can_name) override {
     AgilexBase<ProtocolV2Parser>::ConnectPort(
-        can_name, std::bind(&TracerBaseV2::ParseCANFrame, this,
-                            std::placeholders::_1));
+        can_name,
+        std::bind(&TracerBaseV2::ParseCANFrame, this, std::placeholders::_1));
   }
   void Connect(std::string uart_name, uint32_t baudrate) override {
     // TODO
@@ -38,78 +39,24 @@ class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface
 
   // robot control
   void SetMotionCommand(double linear_vel, double angular_vel) override {
-    AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+    AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, angular_vel,
+                                                    0.0, 0.0);
   }
 
   void SetLightCommand(LightMode f_mode, uint8_t f_value) override {
-    AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, CONST_OFF, 0);
+    AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, CONST_OFF,
+                                                   0);
   }
 
   // get robot state
   TracerState GetRobotState() override {
-    std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);
-    return tracer_state_;
+    // std::lock_guard<std::mutex>
+    // guard(AgilexBase<ProtocolV2Parser>::state_mutex_); return tracer_state_;
   }
 
   void ResetRobotState() override {
     // TODO
   }
-  
- private:
-  TracerState tracer_state_;
-
-  void ParseCANFrame(can_frame *rx_frame) override {
-    AgxMessage status_msg;
-    if (AgilexBase<ProtocolV2Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
-      UpdateTracerState(status_msg, tracer_state_);
-    }
-  }
-  
-  void UpdateTracerState(const AgxMessage &status_msg, TracerState &state) {
-    std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);  
-    switch (status_msg.type) {
-      case AgxMsgSystemState: {
-        //   std::cout << "system status feedback received" << std::endl;
-        state.system_state = status_msg.body.system_state_msg;
-        break;
-      }
-      case AgxMsgMotionState: {
-        // std::cout << "motion control feedback received" << std::endl;
-        state.motion_state = status_msg.body.motion_state_msg;
-        break;
-      }
-      case AgxMsgLightState: {
-        // std::cout << "light control feedback received" << std::endl;
-        state.light_state = status_msg.body.light_state_msg;
-        break;
-      }
-      case AgxMsgActuatorHSState: {
-        // std::cout << "actuator hs feedback received" << std::endl;
-        state
-            .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] =
-            status_msg.body.actuator_hs_state_msg;
-        break;
-      }
-      case AgxMsgActuatorLSState: {
-        // std::cout << "actuator ls feedback received" << std::endl;
-        state
-            .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] =
-            status_msg.body.actuator_ls_state_msg;
-        break;
-      }
-        //   case AgxMsgRcState: {
-        //     state.rc_state = status_msg.body.rc_state_msg;
-        //     break;
-        //   }
-        /* sensor feedback */
-        //   case AgxMsgOdometry: {
-        //     // std::cout << "Odometer msg feedback received" << std::endl;
-        //     state.odometry = status_msg.body.odometry_msg;
-        //   }
-      default:
-        break;
-    }
-  }
 };
 }  // namespace westonrobot
 

include/ugv_sdk/mobile_robot/scout_robot.hpp

@@ -37,7 +37,8 @@ class ScoutRobot : public RobotInterface, public ScoutInterface {
   ProtocolVersion GetProtocolVersion() override;
 
   // get robot state
-  ScoutState GetRobotState() override;
+  ScoutCoreState GetRobotState() override;
+  ScoutActuatorState GetActuatorState() override;
 
  private:
   RobotInterface* robot_;