saved work

include/ugv_sdk/details/interface/agilex_message.h

@@ -44,6 +44,18 @@ typedef struct {
   uint8_t motion_mode;
 } MotionModeMessage;
 
+// V1-only messages
+typedef struct {
+  ControlMode control_mode;
+  bool clear_all_error;
+  float linear;
+  float angular;
+} MotionCommandMessageV1;
+
+typedef struct {
+  bool set_neutral;
+} ValueSetCommandMessageV1;
+
 /**************** Feedback messages *****************/
 
 #define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
@@ -64,7 +76,6 @@ typedef struct {
   uint16_t error_code;
 } SystemStateMessage;
 
-// 0x221
 typedef struct {
   float linear_velocity;
   float angular_velocity;  // only valid for differential drivering
@@ -72,7 +83,6 @@ typedef struct {
   float steering_angle;  // only valid for ackermann steering
 } MotionStateMessage;
 
-// 0x231
 typedef LightCommandMessage LightStateMessage;
 
 typedef struct {
@@ -94,12 +104,6 @@ typedef struct {
   int32_t pulse_count;
 } ActuatorHSStateMessage;
 
-typedef struct {
-  uint8_t motion_mode;
-  uint8_t mode_changing;
-} MotionModeFeedbackMessage;
-
-// 0x261 - 0x264
 #define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01)
 #define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02)
 #define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04)
@@ -117,6 +121,18 @@ typedef struct {
   uint8_t driver_state;
 } ActuatorLSStateMessage;
 
+typedef struct {
+  uint8_t motion_mode;
+  uint8_t mode_changing;
+} MotionModeFeedbackMessage;
+
+// V1-only messages
+typedef struct {
+  float current;
+  int16_t rpm;
+  float temperature;
+} ActuatorStateMessageV1;
+
 /***************** Sensor messages ******************/
 
 typedef struct {
@@ -167,7 +183,6 @@ typedef struct {
   float temperature;
 } BmsBasicMessage;
 
-// 0x362
 #define BMS_PROT1_CHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x01)
 #define BMS_PROT1_CHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x02)
 #define BMS_PROT1_DISCHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x10)
@@ -273,7 +288,11 @@ typedef enum {
   AgxMsgControlModeConfig,
   AgxMsgSteerNeutralRequest,
   AgxMsgSteerNeutralResponse,
-  AgxMsgStateResetConfig
+  AgxMsgStateResetConfig,
+  // V1-only messages
+  AgxMsgMotionCommandV1,
+  AgxMsgValueSetCommandV1,
+  AgxMsgActuatorStateV1
 } MsgType;
 
 typedef struct {
@@ -309,6 +328,10 @@ typedef struct {
     SteerNeutralRequestMessage steer_neutral_request_msg;
     SteerNeutralResponseMessage steer_neutral_response_msg;
     StateResetConfigMessage state_reset_config_msg;
+    // V1-only messages
+    MotionCommandMessageV1 v1_motion_command_msg;
+    ValueSetCommandMessageV1 v1_value_set_command_msg;
+    ActuatorStateMessageV1 v1_actuator_stage_msg;
   } body;
 } AgxMessage;
 

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -16,18 +16,16 @@
 
 namespace westonrobot {
 struct ScoutState {
-  // system state
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
 
-  RcStateMessage rc_state;
-
+  // actuator state
+  // - for v2 robots only
   ActuatorHSStateMessage actuator_hs_state[4];
   ActuatorLSStateMessage actuator_ls_state[4];
-
-  // sensor data
-  OdometryMessage odometry;
+  // - for v1 robots only
+  ActuatorStateMessageV1 actuator_state[4];
 };
 
 struct ScoutInterface {

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -61,12 +61,13 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
 
   void ParseCANFrame(can_frame *rx_frame) override {
     AgxMessage status_msg;
-    AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg);
-    std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
-    UpdateScoutState(status_msg, scout_state_);
+    if (AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
+      UpdateScoutState(status_msg, scout_state_);
+    }
   }
 
   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;
@@ -83,10 +84,10 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
         state.light_state = status_msg.body.light_state_msg;
         break;
       }
-      case AgxMsgRcState: {
-        state.rc_state = status_msg.body.rc_state_msg;
-        break;
-      }
+        //   case AgxMsgRcState: {
+        //     state.rc_state = status_msg.body.rc_state_msg;
+        //     break;
+        //   }
       case AgxMsgActuatorHSState: {
         // std::cout << "actuator hs feedback received" << std::endl;
         state
@@ -101,11 +102,11 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
             status_msg.body.actuator_ls_state_msg;
         break;
       }
-      /* sensor feedback */
-      case AgxMsgOdometry: {
-        // std::cout << "Odometer msg feedback received" << std::endl;
-        state.odometry = status_msg.body.odometry_msg;
-      }
+        /* sensor feedback */
+        //   case AgxMsgOdometry: {
+        //     // std::cout << "Odometer msg feedback received" << std::endl;
+        //     state.odometry = status_msg.body.odometry_msg;
+        //   }
       default:
         break;
     }