added get protocol version api

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,12 +32,14 @@ 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
   virtual void SetMotionMode(uint8_t mode){};
 
-    // any specific robot will use a specialized version of the two functions
+  // any specific robot will use a specialized version of the two functions
   virtual void SendMotionCommand(double linear_vel, double angular_vel,
                                  double lateral_velocity,
                                  double steering_angle){

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;