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];
-
+  // - for v1 robots only
-  // sensor data
+  ActuatorStateMessageV1 actuator_state[4];
-  OdometryMessage odometry;
 };
 
 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);
+    if (AgilexBase<Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
-    std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
       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: {
+        //   case AgxMsgRcState: {
-        state.rc_state = status_msg.body.rc_state_msg;
+        //     state.rc_state = status_msg.body.rc_state_msg;
-        break;
+        //     break;
-      }
+        //   }
       case AgxMsgActuatorHSState: {
         // std::cout << "actuator hs feedback received" << std::endl;
         state
@@ -102,10 +103,10 @@ class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
         break;
       }
         /* sensor feedback */
-      case AgxMsgOdometry: {
+        //   case AgxMsgOdometry: {
-        // std::cout << "Odometer msg feedback received" << std::endl;
+        //     // std::cout << "Odometer msg feedback received" << std::endl;
-        state.odometry = status_msg.body.odometry_msg;
+        //     state.odometry = status_msg.body.odometry_msg;
-      }
+        //   }
       default:
         break;
     }

src/protocol_v1/agilex_message_v1.h

@@ -1,111 +0,0 @@
-/*
- * agilex_message_v1.h
- *
- * Created on: Jul 09, 2021 20:55
- * Description:
- *  all values are using SI units (e.g. meter/second/radian)
- *
- * Re-implemented as a subset of AgxMessage defined in protocol v2
- *
- * Copyright (c) 2021 Ruixiang Du (rdu)
- */
-
-#ifndef AGILEX_MESSAGE_V1_H
-#define AGILEX_MESSAGE_V1_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <string.h>
-
-#include "ugv_sdk/details/interface/agilex_types.h"
-
-/***************** Control messages *****************/
-
-typedef struct {
-  ControlMode control_mode;
-  bool clear_all_error;
-  float linear;
-  float angular;
-} MotionCommandMessage;
-
-typedef struct {
-  bool enable_cmd_ctrl;
-  LightOperation front_light;
-  LightOperation rear_light;
-} LightCommandMessage;
-
-typedef struct {
-  bool set_neutral;
-} ValueSetCommandMessage;
-
-/**************** Feedback messages *****************/
-
-#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100)
-#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200)
-#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001)
-#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002)
-#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004)
-#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008)
-#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010)
-#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020)
-#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040)
-#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080)
-
-typedef struct {
-  VehicleState vehicle_state;
-  ControlMode control_mode;
-  float battery_voltage;
-  uint16_t error_code;
-} SystemStateMessage;
-
-typedef struct {
-  float linear_velocity;
-  float angular_velocity;
-} MotionStateMessage;
-
-typedef LightCommandMessage LightStateMessage;
-
-typedef struct {
-  float current;
-  int16_t rpm;
-  float temperature;
-} ActuatorStateMessage;
-
-//////////////////////////////////////////////////////
-
-typedef enum {
-  AgxMsgUnkonwn = 0x00,
-  // command
-  AgxMsgMotionCommand,
-  AgxMsgLightCommand,
-  AgxMsgValueSetCommand,
-  // state feedback
-  AgxMsgSystemState,
-  AgxMsgMotionState,
-  AgxMsgLightState,
-  AgxMsgActuatorState,
-} MsgType;
-
-typedef struct {
-  MsgType type;
-  union {
-    // command
-    MotionCommandMessage motion_command_msg;
-    LightCommandMessage light_command_msg;
-    // state feedback
-    SystemStateMessage system_state_msg;
-    MotionStateMessage motion_state_msg;
-    LightStateMessage light_state_msg;
-    ActuatorStateMessage actuator_state_msg;
-  } body;
-} AgxMessageV1;
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* AGILEX_MESSAGE_V1_H */

src/protocol_v1/agilex_msg_parser_v1.c

@@ -13,6 +13,6 @@
 #include "stdio.h"
 #include "string.h"
 
-bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg) {}
+bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {}
-void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame) {}
+void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {}
 uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {}

src/protocol_v1/agilex_msg_parser_v1.h

@@ -28,10 +28,10 @@ struct can_frame {
 };
 #endif
 
-#include "protocol_v1/agilex_message_v1.h"
+#include "ugv_sdk/details/interface/agilex_message.h"
 
-bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessageV1 *msg);
+bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg);
-void EncodeCanFrameV1(const AgxMessageV1 *msg, struct can_frame *tx_frame);
+void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame);
 uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc);
 
 #ifdef __cplusplus