added get protocol version api

demo/scout_demo/scout_robot_demo.cpp

@@ -34,9 +34,11 @@ int main(int argc, char **argv) {
 
   std::unique_ptr<ScoutRobot> scout;
   if (protocol_version == "v1") {
-    scout = std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolType::AGX_V1));
+    scout =
+        std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V1));
   } else if (protocol_version == "v2") {
-    scout = std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolType::AGX_V2));
+    scout =
+        std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V2));
   } else {
     std::cout << "Error: invalid protocol version string" << std::endl;
     return -1;
@@ -46,7 +48,13 @@ int main(int argc, char **argv) {
     std::cout << "Failed to create robot object" << std::endl;
 
   scout->Connect(device_name);
+
+  if (scout->GetProtocolVersion() == ProtocolVersion::AGX_V2) {
     scout->EnableCommandedMode();
+    std::cout << "Protocol version 2" << std::endl;
+  } else {
+    std::cout << "Protocol version 1" << std::endl;
+  }
 
   // light control
   std::cout << "Light: const off" << std::endl;
@@ -67,10 +75,10 @@ int main(int argc, char **argv) {
   int count = 0;
   while (true) {
     // motion control
-    if (count < 100) {
-      std::cout << "Motor: 0.2, 0" << std::endl;
-      scout->SetMotionCommand(0.2, 0.0);
-    }
+    // if (count < 100) {
+    //   std::cout << "Motor: 0.2, 0" << std::endl;
+    //   scout->SetMotionCommand(0.2, 0.0);
+    // }
 
     // get robot state
     auto state = scout->GetRobotState();
@@ -80,12 +88,23 @@ int main(int argc, char **argv) {
               << static_cast<int>(state.system_state.control_mode)
               << " , vehicle state: "
               << static_cast<int>(state.system_state.vehicle_state)
-              << std::endl;
-    std::cout << "battery voltage: " << state.system_state.battery_voltage
+              << " , error code: " << std::hex << state.system_state.error_code
+              << ", battery voltage: " << state.system_state.battery_voltage
               << std::endl;
     std::cout << "velocity (linear, angular): "
               << state.motion_state.linear_velocity << ", "
               << state.motion_state.angular_velocity << std::endl;
+
+    if (scout->GetProtocolVersion() == ProtocolVersion::AGX_V1) {
+      for (int i = 0; i < 4; ++i) {
+        printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
+               state.actuator_state[i].motor_id,
+               state.actuator_state[i].current, state.actuator_state[i].rpm,
+               state.actuator_state[i].driver_temp,
+               state.actuator_state[i].motor_temp);
+      }
+    } else {
+    }
     std::cout << "-------------------------------" << std::endl;
 
     usleep(20000);

include/ugv_sdk/details/interface/parser_interface.hpp

@@ -26,11 +26,17 @@ struct can_frame {
 
 #include "ugv_sdk/details/interface/agilex_message.h"
 
-struct ParserInterface {
+enum class ProtocolVersion { AGX_V1, AGX_V2 };
+
+template <ProtocolVersion VersionNumber = ProtocolVersion::AGX_V2>
+class ParserInterface {
+ public:
+  virtual ~ParserInterface() = default;
+
   // CAN support
   virtual bool DecodeMessage(const struct can_frame *rx_frame,
                              AgxMessage *msg) = 0;
-  virtual void EncodeMessage(const AgxMessage *msg,
+  virtual bool EncodeMessage(const AgxMessage *msg,
                              struct can_frame *tx_frame) = 0;
   virtual uint8_t CalculateChecksum(uint16_t id, uint8_t *data,
                                     uint8_t dlc) = 0;
@@ -47,6 +53,8 @@ struct ParserInterface {
     // throw exception
     return 0;
   };
+
+  ProtocolVersion GetProtocolVersion() { return VersionNumber; }
 };
 
 #endif /* PASER_INTERFACE_HPP */

include/ugv_sdk/details/interface/robot_interface.hpp

@@ -13,10 +13,9 @@
 #include <string>
 
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/parser_interface.hpp"
 
 namespace westonrobot {
-enum class ProtocolType { AGX_V1, AGX_V2 };
-
 class RobotInterface {
  public:
   ~RobotInterface() = default;
@@ -33,6 +32,8 @@ class RobotInterface {
   };
   virtual void ResetRobotState() = 0;
 
+  virtual ProtocolVersion GetProtocolVersion() = 0;
+
  protected:
   /****** functions not available/valid to all robots ******/
   // functions to be implemented by class AgilexBase

include/ugv_sdk/details/interface/scout_interface.hpp

@@ -29,7 +29,7 @@ struct ScoutState {
 };
 
 struct ScoutInterface {
-  ~ScoutInterface() = default;
+  virtual ~ScoutInterface() = default;
 
   virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
   virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,

include/ugv_sdk/details/protocol_v1/agilex_msg_parser_v1.h

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

include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp

@@ -17,14 +17,14 @@
 
 namespace westonrobot {
 template <typename RobotLimitsType>
-class ProtocolV1Parser : public ParserInterface {
+class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
  public:
   bool DecodeMessage(const struct can_frame *rx_frame,
                      AgxMessage *msg) override {
     return DecodeCanFrameV1(rx_frame, msg);
   }
 
-  void EncodeMessage(const AgxMessage *msg,
+  bool EncodeMessage(const AgxMessage *msg,
                      struct can_frame *tx_frame) override {
     AgxMessage msg_v1 = *msg;
     if (msg->type == AgxMsgMotionCommandV1) {
@@ -44,7 +44,7 @@ class ProtocolV1Parser : public ParserInterface {
       msg_v1.body.v1_motion_command_msg.angular =
           angular / RobotLimitsType::max_angular * 100.0;
     }
-    EncodeCanFrameV1(&msg_v1, tx_frame);
+    return EncodeCanFrameV1(&msg_v1, tx_frame);
   }
 
   uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override {

include/ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp

@@ -13,11 +13,11 @@
 #include "ugv_sdk/details/interface/parser_interface.hpp"
 
 namespace westonrobot {
-class ProtocolV2Parser : public ParserInterface {
+class ProtocolV2Parser : public ParserInterface<ProtocolVersion::AGX_V2> {
  public:
   bool DecodeMessage(const struct can_frame *rx_frame,
                      AgxMessage *msg) override;
-  void EncodeMessage(const AgxMessage *msg,
+  bool EncodeMessage(const AgxMessage *msg,
                      struct can_frame *tx_frame) override;
   uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) override;
 };

include/ugv_sdk/details/robot_base/agilex_base.hpp

@@ -46,8 +46,7 @@ class AgilexBase : public RobotInterface {
 
     // encode msg to can frame and send to bus
     can_frame frame;
-    parser_.EncodeMessage(&msg, &frame);
-    can_->SendFrame(frame);
+    if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
   }
 
   // must be called at a frequency >= 50Hz
@@ -64,8 +63,7 @@ class AgilexBase : public RobotInterface {
 
       // send to can bus
       can_frame frame;
-      parser_.EncodeMessage(&msg, &frame);
-      can_->SendFrame(frame);
+      if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
     }
   }
 
@@ -83,8 +81,7 @@ class AgilexBase : public RobotInterface {
 
       // send to can bus
       can_frame frame;
-      parser_.EncodeMessage(&msg, &frame);
-      can_->SendFrame(frame);
+      if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
     }
   }
 
@@ -97,8 +94,7 @@ class AgilexBase : public RobotInterface {
 
       // send to can bus
       can_frame frame;
-      parser_.EncodeMessage(&msg, &frame);
-      can_->SendFrame(frame);
+      if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
     }
   }
 
@@ -111,11 +107,14 @@ class AgilexBase : public RobotInterface {
 
       // send to can bus
       can_frame frame;
-      parser_.EncodeMessage(&msg, &frame);
-      can_->SendFrame(frame);
+      if (parser_.EncodeMessage(&msg, &frame)) can_->SendFrame(frame);
     }
   }
 
+  ProtocolVersion GetProtocolVersion() override {
+    return parser_.GetProtocolVersion();
+  }
+
  protected:
   ParserType parser_;
   std::mutex state_mutex_;

include/ugv_sdk/mobile_robot/scout_robot.hpp

@@ -18,7 +18,7 @@
 namespace westonrobot {
 class ScoutRobot : public RobotInterface, public ScoutInterface {
  public:
-  ScoutRobot(ProtocolType protocol = ProtocolType::AGX_V2);
+  ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2);
   ~ScoutRobot();
 
   void Connect(std::string can_name) override;
@@ -33,6 +33,8 @@ class ScoutRobot : public RobotInterface, public ScoutInterface {
 
   void ResetRobotState() override;
 
+  ProtocolVersion GetProtocolVersion() override;
+
   // get robot state
   ScoutState GetRobotState() override;
 

src/mobile_robot/scout_robot.cpp

@@ -11,10 +11,10 @@
 #include "ugv_sdk/details/robot_base/scout_base.hpp"
 
 namespace westonrobot {
-ScoutRobot::ScoutRobot(ProtocolType protocol) {
-  if (protocol == ProtocolType::AGX_V1) {
+ScoutRobot::ScoutRobot(ProtocolVersion protocol) {
+  if (protocol == ProtocolVersion::AGX_V1) {
     robot_ = new ScoutBaseV1();
-  } else if (protocol == ProtocolType::AGX_V2) {
+  } else if (protocol == ProtocolVersion::AGX_V2) {
     robot_ = new ScoutBaseV2();
   }
 }
@@ -33,6 +33,10 @@ void ScoutRobot::Connect(std::string uart_name, uint32_t baudrate) {
 
 void ScoutRobot::ResetRobotState() { robot_->ResetRobotState(); }
 
+ProtocolVersion ScoutRobot::GetProtocolVersion() {
+  return robot_->GetProtocolVersion();
+}
+
 void ScoutRobot::SetMotionCommand(double linear_vel, double angular_vel) {
   auto scout = dynamic_cast<ScoutInterface*>(robot_);
   scout->SetMotionCommand(linear_vel, angular_vel);

src/protocol_v1/agilex_msg_parser_v1.c

@@ -18,6 +18,7 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
   // if checksum not correct
   if (!CalcCanFrameChecksumV1(rx_frame->can_id, (uint8_t *)rx_frame->data,
                               rx_frame->can_dlc)) {
+    printf("wrong checksum for id: %x-------------->\n", rx_frame->can_id);
     return false;
   }
 
@@ -36,13 +37,16 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
     case CAN_MSG_MOTION_STATE_ID: {
       msg->type = AgxMsgMotionState;
       msg->body.motion_state_msg.linear_velocity =
-          ((((uint16_t)rx_frame->data[0]) << 8) | (uint16_t)rx_frame->data[1]) /
+          ((int16_t)((((uint16_t)rx_frame->data[0]) << 8) |
+                     (uint16_t)rx_frame->data[1])) /
           1000.0f;
       msg->body.motion_state_msg.angular_velocity =
-          ((((uint16_t)rx_frame->data[2]) << 8) | (uint16_t)rx_frame->data[3]) /
+          ((int16_t)((((uint16_t)rx_frame->data[2]) << 8) |
+                     (uint16_t)rx_frame->data[3])) /
           1000.0f;
       msg->body.motion_state_msg.lateral_velocity =
-          ((((uint16_t)rx_frame->data[4]) << 8) | (uint16_t)rx_frame->data[5]) /
+          ((int16_t)((((uint16_t)rx_frame->data[4]) << 8) |
+                     (uint16_t)rx_frame->data[5])) /
           1000.0f;
       break;
     }
@@ -89,7 +93,8 @@ bool DecodeCanFrameV1(const struct can_frame *rx_frame, AgxMessage *msg) {
   return true;
 }
 
-void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
+bool EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
+  bool ret = true;
   switch (msg->type) {
     case AgxMsgMotionCommandV1: {
       static uint8_t count = 0;
@@ -155,9 +160,12 @@ void EncodeCanFrameV1(const AgxMessage *msg, struct can_frame *tx_frame) {
           tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
       break;
     }
-    default:
+    default: {
+      ret = false;
       break;
     }
+  }
+  return ret;
 }
 
 uint8_t CalcCanFrameChecksumV1(uint16_t id, uint8_t *data, uint8_t dlc) {

src/protocol_v2/agilex_msg_parser_v2.c

@@ -50,8 +50,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->body.light_command_msg.front_light.custom_value =
           frame->front_custom;
       msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
-      msg->body.light_command_msg.rear_light.custom_value =
-          frame->rear_custom;
+      msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom;
       break;
     }
     case CAN_MSG_BRAKING_COMMAND_ID: {
@@ -104,8 +103,7 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->body.light_command_msg.front_light.custom_value =
           frame->front_custom;
       msg->body.light_command_msg.rear_light.mode = frame->rear_mode;
-      msg->body.light_command_msg.rear_light.custom_value =
-          frame->rear_custom;
+      msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom;
       break;
     }
     case CAN_MSG_RC_STATE_ID: {
@@ -192,10 +190,9 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
       msg->body.actuator_ls_state_msg.driver_state = frame->driver_state;
       break;
     }
-    case CAN_MSG_CURRENT_CTRL_MODE:
-    {
-      msg->type=AgxMsgMotionModeState;
-      MotionModeStateFrame *frame = (MotionModeStateFrame*)(rx_frame->data);
+    case CAN_MSG_CURRENT_CTRL_MODE: {
+      msg->type = AgxMsgMotionModeState;
+      MotionModeStateFrame *frame = (MotionModeStateFrame *)(rx_frame->data);
 
       msg->body.motion_mode_feedback_msg.motion_mode = frame->motion_mode;
       msg->body.motion_mode_feedback_msg.mode_changing = frame->mode_changing;
@@ -309,7 +306,8 @@ bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg) {
   return true;
 }
 
-void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
+bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
+  bool ret = true;
   switch (msg->type) {
     /***************** command frame *****************/
     case AgxMsgMotionCommand: {
@@ -346,8 +344,7 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
         frame.front_custom =
             msg->body.light_command_msg.front_light.custom_value;
         frame.rear_mode = msg->body.light_command_msg.rear_light.mode;
-        frame.rear_custom =
-            msg->body.light_command_msg.rear_light.custom_value;
+        frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value;
       } else {
         frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL;
         frame.front_mode = 0;
@@ -590,9 +587,12 @@ void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame) {
       memcpy(tx_frame->data, (uint8_t *)(&frame), tx_frame->can_dlc);
       break;
     }
-    default:
+    default: {
+      ret = false;
       break;
     }
+  }
+  return ret;
 }
 
 uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc) {

src/protocol_v2/agilex_msg_parser_v2.h

@@ -31,7 +31,7 @@ struct can_frame {
 #include "ugv_sdk/details/interface/agilex_message.h"
 
 bool DecodeCanFrameV2(const struct can_frame *rx_frame, AgxMessage *msg);
-void EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame);
+bool EncodeCanFrameV2(const AgxMessage *msg, struct can_frame *tx_frame);
 uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc);
 
 #ifdef __cplusplus

src/protocol_v2/protocol_v2_parser.cpp

@@ -16,9 +16,9 @@ bool ProtocolV2Parser::DecodeMessage(const struct can_frame *rx_frame,
   return DecodeCanFrameV2(rx_frame, msg);
 }
 
-void ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
+bool ProtocolV2Parser::EncodeMessage(const AgxMessage *msg,
                                      struct can_frame *tx_frame) {
-  EncodeCanFrameV2(msg, tx_frame);
+  return EncodeCanFrameV2(msg, tx_frame);
 }
 
 uint8_t ProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data,