made scout_base to be class template

CMakeLists.txt

@@ -81,16 +81,18 @@ add_library(${PROJECT_NAME}
     ########################
     ## protocol v2 support
     # parser
-    # src/protocol_v2/agilex_base.cpp
     src/protocol_v2/agx_msg_parser.c
     src/protocol_v2/protocol_v2_parser.cpp
     # robots
-    src/protocol_v2/scout_base.cpp
+    # src/protocol_v2/scout_base.cpp
     # src/protocol_v2/tracer_base.cpp
     # src/protocol_v2/ranger_base.cpp
     ########################
     ## legacy protocol v1 support
-    src/protocol_v1/scout_can_parser_v1.c
+    src/protocol_v1/scout/scout_uart_parser.c
+    src/protocol_v1/scout/scout_can_parser.c
+    src/protocol_v1/scout/scout_protocol_v1_parser.cpp
+
 )
 target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads)
 target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES)

demo/scout_demo/scout_v2_demo.cpp

@@ -7,7 +7,7 @@
  * Copyright (c) 2019 Ruixiang Du (rdu)
  */
 
-#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
+#include "ugv_sdk/details/robot_base/scout_base.hpp"
 
 using namespace westonrobot;
 

demo/tracer_demo/tracer_demo.cpp

@@ -23,7 +23,7 @@ int main(int argc, char **argv) {
     return -1;
   }
 
-  TracerBase tracer;
+  TracerBaseV2 tracer;
   tracer.Connect(device_name);
 
   tracer.EnableCommandedMode();

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -119,11 +119,8 @@ class AgilexBase : public RobotInterface {
   }
 
  protected:
-  std::mutex state_mutex_;
-  //   std::mutex motion_cmd_mutex_;
-  MotionCommandMessage current_motion_cmd_;
-
   ParserType parser_;
+  std::mutex state_mutex_;
 
   // communication interface
   bool can_connected_ = false;

include/ugv_sdk/details/robot_base/scout_base.hpp

@@ -0,0 +1,120 @@
+/*
+ * scout_base.hpp
+ *
+ * Created on: Dec 23, 2020 14:39
+ * Description:
+ *
+ * Copyright (c) 2020 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_BASE_HPP
+#define SCOUT_BASE_HPP
+
+#include <string>
+#include <cstdint>
+#include <thread>
+#include <mutex>
+
+#include "ugv_sdk/interface/scout_interface.hpp"
+#include "ugv_sdk/details/robot_base/agilex_base.hpp"
+#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
+
+namespace westonrobot {
+template <typename Parser>
+class ScoutBase : public AgilexBase<Parser>, public ScoutInterface {
+ public:
+  ScoutBase() : AgilexBase<Parser>(){};
+  ~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));
+  }
+
+  void Connect(std::string uart_name, uint32_t baudrate) override {
+    // TODO
+  }
+
+  // robot control
+  void SetMotionCommand(double linear_vel, double angular_vel) override {
+    AgilexBase<Parser>::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);
+  }
+
+  // get robot state
+  ScoutState GetRobotState() override {
+    std::lock_guard<std::mutex> guard(AgilexBase<Parser>::state_mutex_);
+    return scout_state_;
+  }
+
+  void ResetRobotState() override {
+    // TODO
+  }
+
+ private:
+  ScoutState scout_state_;
+
+  void ParseCANFrame(can_frame *rx_frame) override {
+    AgxMessage status_msg;
+    DecodeCanFrame(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) {
+    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 AgxMsgRcState: {
+        state.rc_state = status_msg.body.rc_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;
+      }
+      /* sensor feedback */
+      case AgxMsgOdometry: {
+        // std::cout << "Odometer msg feedback received" << std::endl;
+        state.odometry = status_msg.body.odometry_msg;
+      }
+      default:
+        break;
+    }
+  }
+};
+
+
+using ScoutBaseV2 = ScoutBase<ProtocolV2Parser>;
+}  // namespace westonrobot
+
+#endif /* SCOUT_BASE_HPP */

include/ugv_sdk/details/robot_base/tracer_base.hpp

@@ -19,10 +19,10 @@
 #include "ugv_sdk/protocol_v2/agilex_base.hpp"
 
 namespace westonrobot {
-class TracerBase : public AgilexBase, public TracerInterface {
+class TracerBaseV2 : public AgilexBase, public TracerInterface {
  public:
-  TracerBase() : AgilexBase(){};
-  ~TracerBase() = default;
+  TracerBaseV2() : AgilexBase(){};
+  ~TracerBaseV2() = default;
 
   // set up connection
   void Connect(std::string can_name) override;

include/ugv_sdk/interface/parser_interface.hpp

@@ -36,14 +36,16 @@ struct ParserInterface {
                                     uint8_t dlc) = 0;
 
   // UART support
-  virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg){
+  virtual bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) {
     // throw exception
+    return false;
   };
   virtual void EncodeMessage(const AgxMessage *msg, uint8_t *buf, uint8_t *len){
       // throw exception
   };
-  virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len){
+  virtual uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) {
     // throw exception
+    return 0;
   };
 };
 

include/ugv_sdk/mobile_base/scout_robot.hpp

@@ -24,7 +24,7 @@ class ScoutRobot : public RobotInterface, public ScoutInterface {
   void Connect(std::string can_name) override;
   void Connect(std::string uart_name, uint32_t baudrate) override;
 
-  void EnableCommandedMode();
+  void EnableCommandedMode() override;
 
   void SetMotionCommand(double linear_vel, double angular_vel) override;
   void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,

include/ugv_sdk/protocol_v1/scout_base_v1.hpp

@@ -1,14 +1,14 @@
 /*
- * scout_base.hpp
+ * scout_base_v1.hpp
  *
- * Created on: Dec 23, 2020 14:39
+ * Created on: Jul 08, 2021 22:47
  * Description:
  *
- * Copyright (c) 2020 Ruixiang Du (rdu)
+ * Copyright (c) 2021 Ruixiang Du (rdu)
  */
 
-#ifndef SCOUT_BASE_HPP
-#define SCOUT_BASE_HPP
+#ifndef SCOUT_BASE_V1_HPP
+#define SCOUT_BASE_V1_HPP
 
 #include <string>
 #include <cstdint>
@@ -18,13 +18,14 @@
 #include "ugv_sdk/interface/scout_interface.hpp"
 
 #include "ugv_sdk/mobile_base/agilex_base.hpp"
-#include "ugv_sdk/protocol_v2/protocol_v2_parser.hpp"
+#include "ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp"
 
 namespace westonrobot {
-class ScoutBaseV2 : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
+class ScoutBaseV1 : public AgilexBase<ScoutProtocolV1Parser>,
+                    public ScoutInterface {
  public:
-  ScoutBaseV2() : AgilexBase(){};
-  ~ScoutBaseV2() = default;
+  ScoutBaseV1() : AgilexBase<ScoutProtocolV1Parser>(){};
+  ~ScoutBaseV1() = default;
 
   // set up connection
   void Connect(std::string can_name) override;
@@ -48,4 +49,4 @@ class ScoutBaseV2 : public AgilexBase<ProtocolV2Parser>, public ScoutInterface {
 };
 }  // namespace westonrobot
 
-#endif /* SCOUT_BASE_HPP */
+#endif /* SCOUT_BASE_V1_HPP */

include/ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp

@@ -0,0 +1,32 @@
+/*
+ * scout_protocol_v1_parser.hpp
+ *
+ * Created on: Jul 08, 2021 22:30
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_PROTOCOL_V1_PARSER_HPP
+#define SCOUT_PROTOCOL_V1_PARSER_HPP
+
+#include "ugv_sdk/interface/parser_interface.hpp"
+
+namespace westonrobot {
+class ScoutProtocolV1Parser : public ParserInterface {
+ public:
+  bool DecodeMessage(const struct can_frame *rx_frame,
+                     AgxMessage *msg) override;
+  void EncodeMessage(const AgxMessage *msg,
+                     struct can_frame *tx_frame) override;
+  uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
+
+  // UART support
+  bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override;
+  void EncodeMessage(const AgxMessage *msg, uint8_t *buf,
+                     uint8_t *len) override;
+  uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override;
+};
+}  // namespace westonrobot
+
+#endif /* SCOUT_PROTOCOL_V1_PARSER_HPP */

sample/tracer_demo.cpp

@@ -23,7 +23,7 @@ int main(int argc, char **argv) {
     return -1;
   }
 
-  TracerBase tracer;
+  TracerBaseV2 tracer;
   tracer.Connect(device_name);
 
   tracer.EnableCommandedMode();

src/mobile_base/scout_robot.cpp

@@ -8,7 +8,7 @@
  */
 
 #include "ugv_sdk/mobile_base/scout_robot.hpp"
-#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
+#include "ugv_sdk/details/robot_base/scout_base.hpp"
 
 namespace westonrobot {
 ScoutRobot::ScoutRobot(ProtocolType protocol) {

src/protocol_v1/scout/scout_can_parser.c

@@ -7,7 +7,7 @@
  * Copyright (c) 2019 Ruixiang Du (rdu)
  */
 
-#include "ugv_sdk/protocol_v1/scout_can_parser_v1.h"
+#include "protocol_v1/scout/scout_can_parser.h"
 
 #include "string.h"
 

src/protocol_v1/scout/scout_can_parser.h

@@ -17,7 +17,7 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 
-#include "ugv_sdk/protocol_v1/scout_protocol_v1.h"
+#include "protocol_v1/scout/scout_protocol.h"
 
 #ifdef __linux__
 #include <linux/can.h>

src/protocol_v1/scout/scout_protocol_v1_parser.cpp

@@ -0,0 +1,32 @@
+/*
+ * scout_protocol_v1_parser.cpp
+ *
+ * Created on: Jul 08, 2021 22:43
+ * Description:
+ *
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */
+
+#include "ugv_sdk/protocol_v1/scout_protocol_v1_parser.hpp"
+
+#include "protocol_v1/scout/scout_can_parser.h"
+
+namespace westonrobot {
+bool ScoutProtocolV1Parser::DecodeMessage(const struct can_frame *rx_frame,
+                                          AgxMessage *msg) {}
+
+void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg,
+                                          struct can_frame *tx_frame) {}
+
+uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint16_t id, uint8_t *data,
+                                                 uint8_t dlc) {}
+
+// UART support
+bool ScoutProtocolV1Parser::DecodeMessage(uint8_t *data, uint8_t dlc,
+                                          AgxMessage *msg) {}
+
+void ScoutProtocolV1Parser::EncodeMessage(const AgxMessage *msg, uint8_t *buf,
+                                          uint8_t *len) {}
+
+uint8_t ScoutProtocolV1Parser::CalculateChecksum(uint8_t *buf, uint8_t len) {}
+}  // namespace westonrobot

src/protocol_v1/scout/scout_uart_parser.c

@@ -0,0 +1,629 @@
+/* 
+ * scout_uart_parser.c
+ * 
+ * Created on: Aug 14, 2019 12:02
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#include "protocol_v1/scout/scout_uart_parser.h"
+
+// #define USE_XOR_CHECKSUM
+
+// #define PRINT_CPP_DEBUG_INFO
+// #define PRINT_JLINK_DEBUG_INFO
+
+#ifdef PRINT_CPP_DEBUG_INFO
+#undef PRINT_JLINK_DEBUG_INFO
+#endif
+
+#ifdef PRINT_CPP_DEBUG_INFO
+#define <iostream>
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+#include "segger/jlink_rtt.h"
+#endif
+
+typedef enum
+{
+    WAIT_FOR_SOF1 = 0,
+    WAIT_FOR_SOF2,
+    WAIT_FOR_FRAME_LEN,
+    WAIT_FOR_FRAME_TYPE,
+    WAIT_FOR_FRAME_ID,
+    WAIT_FOR_PAYLOAD,
+    WAIT_FOR_FRAME_COUNT,
+    WAIT_FOR_CHECKSUM
+} ScoutSerialDecodeState;
+
+#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2)
+
+#define FRAME_SOF_LEN ((uint8_t)2)
+#define FRAME_FIXED_FIELD_LEN ((uint8_t)4)
+
+#define FRAME_SOF1 ((uint8_t)0x5a)
+#define FRAME_SOF2 ((uint8_t)0xa5)
+
+#define FRAME_TYPE_CONTROL ((uint8_t)0x55)
+#define FRAME_TYPE_STATUS ((uint8_t)0xaa)
+
+#define FRAME_NONE_ID ((uint8_t)0x00)
+
+// frame buffer
+static struct
+{
+    uint8_t frame_id;
+    uint8_t frame_type;
+    uint8_t frame_len;
+    uint8_t frame_cnt;
+    uint8_t frame_checksum;
+    uint8_t internal_checksum;
+    uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE];
+    size_t payload_data_pos;
+} uart_parsing_data;
+
+// statisctics
+typedef struct
+{
+    uint32_t frame_parsed;
+    uint32_t frame_with_wrong_checksum;
+} UARTParsingStats;
+
+static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123};
+
+// internal functions
+static bool ParseChar(uint8_t c, ScoutMessage *msg);
+static uint8_t CalcBufferedFrameChecksum();
+static bool ConstructStatusMessage(ScoutMessage *msg);
+static bool ConstructControlMessage(ScoutMessage *msg);
+
+static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len);
+static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len);
+
+void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len)
+{
+    // SOF
+    buf[0] = FRAME_SOF1;
+    buf[1] = FRAME_SOF2;
+
+    // frame len, type, ID
+    buf[2] = 0x0a;
+    buf[3] = FRAME_TYPE_STATUS;
+
+    switch (msg->type)
+    {
+    // in the current implementation, both MsgType and can_frame include 8 * uint8_t
+    case ScoutMotionStatusMsg:
+    {
+        buf[4] = UART_FRAME_MOTION_STATUS_ID;
+        buf[5] = msg->body.motion_status_msg.data.status.linear_velocity.high_byte;
+        buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte;
+        buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte;
+        buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte;
+        buf[9] = 0;
+        buf[10] = 0;
+        buf[11] = msg->body.motion_status_msg.data.status.count;
+        break;
+    }
+    case ScoutLightStatusMsg:
+    {
+        buf[4] = UART_FRAME_LIGHT_STATUS_ID;
+        buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable;
+        buf[6] = msg->body.light_status_msg.data.status.front_light_mode;
+        buf[7] = msg->body.light_status_msg.data.status.front_light_custom;
+        buf[8] = msg->body.light_status_msg.data.status.rear_light_mode;
+        buf[9] = msg->body.light_status_msg.data.status.rear_light_custom;
+        buf[10] = 0;
+        buf[11] = msg->body.light_status_msg.data.status.count;
+        break;
+    }
+    case ScoutSystemStatusMsg:
+    {
+        buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
+        buf[5] = msg->body.system_status_msg.data.status.base_state;
+        buf[6] = msg->body.system_status_msg.data.status.control_mode;
+        buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte;
+        buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte;
+        buf[9] = msg->body.system_status_msg.data.status.fault_code.high_byte;
+        buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte;
+        buf[11] = msg->body.system_status_msg.data.status.count;
+        break;
+    }
+    case ScoutMotorDriverStatusMsg:
+    {
+        if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
+            buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
+        else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
+            buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID;
+        else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
+            buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID;
+        else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
+            buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID;
+        buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte;
+        buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte;
+        buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte;
+        buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte;
+        buf[9] = msg->body.motor_driver_status_msg.data.status.temperature;
+        buf[10] = 0;
+        buf[11] = msg->body.motor_driver_status_msg.data.status.count;
+        break;
+    }
+    case ScoutMotionControlMsg:
+    {
+        EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len);
+        break;
+    }
+    case ScoutLightControlMsg:
+    {
+        EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len);
+        break;
+    }
+    default:
+        break;
+    }
+
+    buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
+
+    // length: SOF + Frame + Checksum
+    *len = buf[2] + FRAME_SOF_LEN + 1;
+}
+
+bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg)
+{
+    static ScoutMessage decoded_msg;
+
+    bool result = ParseChar(c, &decoded_msg);
+    if (result)
+        *msg = decoded_msg;
+    return result;
+}
+
+void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len)
+{
+    // SOF
+    buf[0] = FRAME_SOF1;
+    buf[1] = FRAME_SOF2;
+
+    // frame len, type, ID
+    buf[2] = 0x0a;
+    buf[3] = FRAME_TYPE_CONTROL;
+    buf[4] = UART_FRAME_MOTION_CONTROL_ID;
+
+    // frame payload
+    buf[5] = msg->data.cmd.control_mode;
+    buf[6] = msg->data.cmd.fault_clear_flag;
+    buf[7] = msg->data.cmd.linear_velocity_cmd;
+    buf[8] = msg->data.cmd.angular_velocity_cmd;
+    buf[9] = 0x00;
+    buf[10] = 0x00;
+
+    // frame count, checksum
+    buf[11] = msg->data.cmd.count;
+    buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
+
+    // length: SOF + Frame + Checksum
+    *len = buf[2] + FRAME_SOF_LEN + 1;
+}
+
+void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len)
+{
+    // SOF
+    buf[0] = FRAME_SOF1;
+    buf[1] = FRAME_SOF2;
+
+    // frame len, type, ID
+    buf[2] = 0x0a;
+    buf[3] = FRAME_TYPE_CONTROL;
+    buf[4] = UART_FRAME_LIGHT_CONTROL_ID;
+
+    // frame payload
+    buf[5] = msg->data.cmd.light_ctrl_enable;
+    buf[6] = msg->data.cmd.front_light_mode;
+    buf[7] = msg->data.cmd.front_light_custom;
+    buf[8] = msg->data.cmd.rear_light_mode;
+    buf[9] = msg->data.cmd.rear_light_custom;
+    buf[10] = 0x00;
+
+    // frame count, checksum
+    buf[11] = msg->data.cmd.count;
+    buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
+
+    // length: SOF + Frame + Checksum
+    *len = buf[2] + FRAME_SOF_LEN + 1;
+}
+
+bool ParseChar(uint8_t c, ScoutMessage *msg)
+{
+    static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;
+
+    bool new_frame_parsed = false;
+    switch (decode_state)
+    {
+    case WAIT_FOR_SOF1:
+    {
+        if (c == FRAME_SOF1)
+        {
+            uart_parsing_data.frame_id = FRAME_NONE_ID;
+            uart_parsing_data.frame_type = 0;
+            uart_parsing_data.frame_len = 0;
+            uart_parsing_data.frame_cnt = 0;
+            uart_parsing_data.frame_checksum = 0;
+            uart_parsing_data.internal_checksum = 0;
+            uart_parsing_data.payload_data_pos = 0;
+            memset(uart_parsing_data.payload_buffer, 0, PAYLOAD_BUFFER_SIZE);
+
+            decode_state = WAIT_FOR_SOF2;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "found sof1" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "found sof1\n");
+#endif
+        }
+        break;
+    }
+    case WAIT_FOR_SOF2:
+    {
+        if (c == FRAME_SOF2)
+        {
+            decode_state = WAIT_FOR_FRAME_LEN;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "found sof2" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "found sof2\n");
+#endif
+        }
+        else
+        {
+            decode_state = WAIT_FOR_SOF1;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "failed to find sof2" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "failed to find sof2\n");
+#endif
+        }
+        break;
+    }
+    case WAIT_FOR_FRAME_LEN:
+    {
+        uart_parsing_data.frame_len = c;
+        decode_state = WAIT_FOR_FRAME_TYPE;
+#ifdef PRINT_CPP_DEBUG_INFO
+        std::cout << "frame len: " << std::hex << static_cast<int>(frame_len) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+        JLinkRTTPrintf(0, "frame len: %d\n", frame_len);
+#endif
+        break;
+    }
+    case WAIT_FOR_FRAME_TYPE:
+    {
+        switch (c)
+        {
+        case FRAME_TYPE_CONTROL:
+        {
+            uart_parsing_data.frame_type = FRAME_TYPE_CONTROL;
+            decode_state = WAIT_FOR_FRAME_ID;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "control type frame received" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "control type frame received\n");
+#endif
+            break;
+        }
+        case FRAME_TYPE_STATUS:
+        {
+            uart_parsing_data.frame_type = FRAME_TYPE_STATUS;
+            decode_state = WAIT_FOR_FRAME_ID;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "status type frame received" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "status type frame received\n");
+#endif
+            break;
+        }
+        default:
+        {
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cerr << "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n");
+#endif
+            decode_state = WAIT_FOR_SOF1;
+        }
+        }
+        break;
+    }
+    case WAIT_FOR_FRAME_ID:
+    {
+        switch (c)
+        {
+        case UART_FRAME_SYSTEM_STATUS_ID:
+        case UART_FRAME_MOTION_STATUS_ID:
+        case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
+        case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
+        case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
+        case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
+        case UART_FRAME_LIGHT_STATUS_ID:
+        {
+            uart_parsing_data.frame_id = c;
+            decode_state = WAIT_FOR_PAYLOAD;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "frame id: " << std::hex << static_cast<int>(frame_id) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkRTTPrintf(0, "frame id: %d\n", frame_id);
+#endif
+            break;
+        }
+        default:
+        {
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cerr << "ERROR: Unknown frame id" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "ERROR: Unknown frame id\n");
+#endif
+            decode_state = WAIT_FOR_SOF1;
+        }
+        }
+        break;
+    }
+    case WAIT_FOR_PAYLOAD:
+    {
+        uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c;
+#ifdef PRINT_CPP_DEBUG_INFO
+        std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+        JLinkRTTPrintf(0, "1 byte added: %d\n", c);
+#endif
+        if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN))
+            decode_state = WAIT_FOR_FRAME_COUNT;
+        break;
+    }
+    case WAIT_FOR_FRAME_COUNT:
+    {
+        uart_parsing_data.frame_cnt = c;
+        decode_state = WAIT_FOR_CHECKSUM;
+#ifdef PRINT_CPP_DEBUG_INFO
+        std::cout << "frame count: " << std::hex << static_cast<int>(frame_cnt) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+        JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt);
+#endif
+        break;
+    }
+    case WAIT_FOR_CHECKSUM:
+    {
+        uart_parsing_data.frame_checksum = c;
+        uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum();
+        new_frame_parsed = true;
+        decode_state = WAIT_FOR_SOF1;
+#ifdef PRINT_CPP_DEBUG_INFO
+        std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
+        std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+        JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum);
+        JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", internal_checksum);
+#endif
+        break;
+    }
+    default:
+        break;
+    }
+
+    if (new_frame_parsed)
+    {
+        if (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum)
+        {
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "checksum correct" << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "checksum correct\n");
+#endif
+            if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS)
+                ConstructStatusMessage(msg);
+            else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL)
+                ConstructControlMessage(msg);
+            ++uart_parsing_stats.frame_parsed;
+        }
+        else
+        {
+            ++uart_parsing_stats.frame_with_wrong_checksum;
+#ifdef PRINT_CPP_DEBUG_INFO
+            std::cout << "checksum is NOT correct" << std::endl;
+            std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl;
+            std::cout << "payload: ";
+            for (int i = 0; i < payload_data_pos; ++i)
+                std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " ";
+            std::cout << std::endl;
+            std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
+            std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(internal_checksum) << std::dec << std::endl;
+#elif (defined(PRINT_JLINK_DEBUG_INFO))
+            JLinkWriteString(0, "checksum is NOT correct\n");
+#endif
+        }
+    }
+
+    return new_frame_parsed;
+}
+
+bool ConstructControlMessage(ScoutMessage *msg)
+{
+    if (msg == NULL)
+        return false;
+
+    switch (uart_parsing_data.frame_id)
+    {
+    case UART_FRAME_MOTION_CONTROL_ID:
+    {
+        msg->type = ScoutMotionControlMsg;
+        msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0];
+        msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1];
+        msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2];
+        msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3];
+        msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4];
+        msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5];
+        msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
+        msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_LIGHT_CONTROL_ID:
+    {
+        msg->type = ScoutLightControlMsg;
+        msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
+        msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1];
+        msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2];
+        msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3];
+        msg->body.light_control_msg.data.cmd.rear_light_custom = uart_parsing_data.payload_buffer[4];
+        msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5];
+        msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
+        msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    }
+    return true;
+}
+
+bool ConstructStatusMessage(ScoutMessage *msg)
+{
+    if (msg == NULL)
+        return false;
+
+    switch (uart_parsing_data.frame_id)
+    {
+    case UART_FRAME_SYSTEM_STATUS_ID:
+    {
+        msg->type = ScoutSystemStatusMsg;
+        msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0];
+        msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1];
+        msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4];
+        msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5];
+        msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_MOTION_STATUS_ID:
+    {
+        msg->type = ScoutMotionStatusMsg;
+        msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0];
+        msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1];
+        msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.motion_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motion_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
+    {
+        msg->type = ScoutMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
+        msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
+        msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
+        msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
+        msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
+    {
+        msg->type = ScoutMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
+        msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
+        msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
+        msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
+        msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
+    {
+        msg->type = ScoutMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
+        msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
+        msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
+        msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
+        msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
+    {
+        msg->type = ScoutMotorDriverStatusMsg;
+        msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
+        msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
+        msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
+        msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
+        msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
+        msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
+        msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
+        msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    case UART_FRAME_LIGHT_STATUS_ID:
+    {
+        msg->type = ScoutLightStatusMsg;
+        msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
+        msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1];
+        msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2];
+        msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3];
+        msg->body.light_status_msg.data.status.rear_light_custom = uart_parsing_data.payload_buffer[4];
+        msg->body.light_status_msg.data.status.reserved0 = 0x00;
+        msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt;
+        msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
+        break;
+    }
+    }
+    return true;
+}
+
+uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len)
+{
+    uint8_t checksum = 0;
+
+#ifdef USE_XOR_CHECKSUM
+    for (int i = 0; i < len; ++i)
+        checksum ^= buf[i];
+#else
+    for (int i = 0; i < len; ++i)
+        checksum += buf[i];
+#endif
+
+    return checksum;
+}
+
+uint8_t CalcBufferedFrameChecksum()
+{
+    uint8_t checksum = 0x00;
+
+#ifdef USE_XOR_CHECKSUM
+    checksum ^= FRAME_SOF1;
+    checksum ^= FRAME_SOF2;
+    checksum ^= uart_parsing_data.frame_len;
+    checksum ^= uart_parsing_data.frame_type;
+    checksum ^= uart_parsing_data.frame_id;
+    for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
+        checksum ^= uart_parsing_data.payload_buffer[i];
+    checksum ^= uart_parsing_data.frame_cnt;
+#else
+    checksum += FRAME_SOF1;
+    checksum += FRAME_SOF2;
+    checksum += uart_parsing_data.frame_len;
+    checksum += uart_parsing_data.frame_type;
+    checksum += uart_parsing_data.frame_id;
+    for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
+        checksum += uart_parsing_data.payload_buffer[i];
+    checksum += uart_parsing_data.frame_cnt;
+#endif
+
+    return checksum;
+}

src/protocol_v1/scout/scout_uart_parser.h

@@ -0,0 +1,32 @@
+/* 
+ * scout_uart_parser.h
+ * 
+ * Created on: Aug 14, 2019 12:01
+ * Description: 
+ * 
+ * Copyright (c) 2019 Ruixiang Du (rdu)
+ */
+
+#ifndef SCOUT_UART_PARSER_H
+#define SCOUT_UART_PARSER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include "protocol_v1/scout/scout_protocol.h"
+
+bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg);
+void EncodeScoutMsgToUART(const ScoutMessage *msg, uint8_t *buf, uint8_t *len);
+
+uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* SCOUT_UART_PARSER_H */

src/protocol_v2/scout_base.cpp

@@ -1,98 +0,0 @@
-/*
- * scout_base.cpp
- *
- * Created on: Jul 08, 2021 12:07
- * Description:
- *
- * Copyright (c) 2021 Weston Robot Pte. Ltd.
- */
-
-#include "ugv_sdk/protocol_v2/scout_base_v2.hpp"
-
-#include <string>
-#include <cstring>
-#include <iostream>
-#include <algorithm>
-#include <array>
-#include <chrono>
-#include <cstdint>
-#include <ratio>
-#include <thread>
-
-#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
-
-namespace westonrobot {
-void ScoutBaseV2::Connect(std::string dev_name) {
-  AgilexBase::ConnectPort(dev_name, std::bind(&ScoutBaseV2::ParseCANFrame, this,
-                                              std::placeholders::_1));
-}
-
-void ScoutBaseV2::Connect(std::string uart_name, uint32_t baudrate) {
-  // TODO
-}
-
-void ScoutBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
-  AgilexBase::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
-}
-
-void ScoutBaseV2::SetLightCommand(LightMode f_mode, uint8_t f_value,
-                                LightMode r_mode, uint8_t r_value) {
-  AgilexBase::SendLightCommand(f_mode, f_value, r_mode, r_value);
-}
-
-ScoutState ScoutBaseV2::GetRobotState() {
-  std::lock_guard<std::mutex> guard(state_mutex_);
-  return scout_state_;
-}
-
-void ScoutBaseV2::ParseCANFrame(can_frame *rx_frame) {
-  AgxMessage status_msg;
-  DecodeCanFrame(rx_frame, &status_msg);
-  std::lock_guard<std::mutex> guard(state_mutex_);
-  UpdateScoutState(status_msg, scout_state_);
-}
-
-void ScoutBaseV2::UpdateScoutState(const AgxMessage &status_msg,
-                                 ScoutState &state) {
-  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 AgxMsgRcState: {
-      state.rc_state = status_msg.body.rc_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;
-    }
-    /* sensor feedback */
-    case AgxMsgOdometry: {
-      // std::cout << "Odometer msg feedback received" << std::endl;
-      state.odometry = status_msg.body.odometry_msg;
-    }
-    default:
-      break;
-  }
-}
-}  // namespace westonrobot

src/protocol_v2/tracer_base.cpp

@@ -15,39 +15,39 @@
 #include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
 
 namespace westonrobot {
-void TracerBase::Connect(std::string dev_name) {
-  AgilexBase::ConnectPort(dev_name, std::bind(&TracerBase::ParseCANFrame, this,
+void TracerBaseV2::Connect(std::string dev_name) {
+  AgilexBase::ConnectPort(dev_name, std::bind(&TracerBaseV2::ParseCANFrame, this,
                                               std::placeholders::_1));
 }
 
-void TracerBase::Connect(std::string uart_name, uint32_t baudrate) {
+void TracerBaseV2::Connect(std::string uart_name, uint32_t baudrate) {
   // TODO
 }
 
-void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) {
+void TracerBaseV2::SetMotionCommand(double linear_vel, double angular_vel) {
   AgilexBase::SetMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
 }
 
-void TracerBase::SetLightCommand(const TracerLightCmd &cmd) {
+void TracerBaseV2::SetLightCommand(const TracerLightCmd &cmd) {
   if (cmd.cmd_ctrl_allowed) {
     AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
                                  LightMode::CONST_OFF, 0);
   }
 }
 
-TracerState TracerBase::GetTracerState() {
+TracerState TracerBaseV2::GetTracerState() {
   std::lock_guard<std::mutex> guard(state_mutex_);
   return tracer_state_;
 }
 
-void TracerBase::ParseCANFrame(can_frame *rx_frame) {
+void TracerBaseV2::ParseCANFrame(can_frame *rx_frame) {
   AgxMessage status_msg;
   DecodeCanFrame(rx_frame, &status_msg);
   std::lock_guard<std::mutex> guard(state_mutex_);
   UpdateTracerState(status_msg, tracer_state_);
 }
 
-void TracerBase::UpdateTracerState(const AgxMessage &status_msg,
+void TracerBaseV2::UpdateTracerState(const AgxMessage &status_msg,
                                    TracerState &state) {
   switch (status_msg.type) {
     case AgxMsgSystemState: {