update tracer_interface and tracer_base

include/ugv_sdk/details/interface/tracer_interface.hpp

@@ -12,7 +12,7 @@
 
 #include <string>
 
-#include "ugv_sdk/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/agilex_message.h"
 
 namespace westonrobot {
 struct TracerState {
@@ -21,6 +21,7 @@ struct TracerState {
   MotionStateMessage motion_state;
   LightStateMessage light_state;
 
+  // Is this still needed?
   RcStateMessage rc_state;
 
   ActuatorHSStateMessage actuator_hs_state[2];
@@ -30,36 +31,14 @@ struct TracerState {
   OdometryMessage odometry;
 };
 
-struct TracerMotionCmd {
-  double linear_velocity;
-  double angular_velocity;
-};
-
-struct TracerLightCmd {
-  TracerLightCmd() = default;
-  TracerLightCmd(LightMode f_mode, uint8_t f_value)
-      : enable_cmd_ctrl(true),
-        front_mode(f_mode),
-        front_custom_value(f_value) {}
-
-  bool enable_cmd_ctrl = false;
-  LightMode front_mode;
-  uint8_t front_custom_value;
-};
-
-/////////////////////////////////////////////////////////////////////////
-
 struct TracerInterface {
-  // set up connection
+  virtual ~TracerInterface() = default;
-  virtual void Connect(std::string can_name) = 0;
-  virtual void Connect(std::string uart_name, uint32_t baudrate) = 0;
 
-  // robot control
   virtual void SetMotionCommand(double linear_vel, double angular_vel) = 0;
-  virtual void SetLightCommand(const TracerLightCmd &cmd) = 0;
+  virtual void SetLightCommand(LightMode f_mode, uint8_t f_value) = 0;
 
   // get robot state
-  virtual TracerState GetTracerState() = 0;
+  virtual TracerState GetRobotState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/robot_base/tracer_base.hpp

@@ -15,31 +15,101 @@
 #include <thread>
 #include <mutex>
 
-#include "ugv_sdk/interface/tracer_interface.hpp"
+#include "ugv_sdk/details/interface/tracer_interface.hpp"
-#include "ugv_sdk/protocol_v2/agilex_base.hpp"
+#include "ugv_sdk/details/robot_base/agilex_base.hpp"
+
+#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
 
 namespace westonrobot {
-class TracerBaseV2 : public AgilexBase, public TracerInterface {
+class TracerBaseV2 : public AgilexBase<ProtocolV2Parser>, public TracerInterface {
  public:
-  TracerBaseV2() : AgilexBase(){};
+  TracerBaseV2() : AgilexBase<ProtocolV2Parser>(){};
   ~TracerBaseV2() = default;
 
   // set up connection
-  void Connect(std::string can_name) override;
+  void Connect(std::string can_name) override {
-  void Connect(std::string uart_name, uint32_t baudrate) override;
+    AgilexBase<ProtocolV2Parser>::ConnectPort(
+        can_name, std::bind(&TracerBaseV2::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);
+  void SetMotionCommand(double linear_vel, double angular_vel) override {
-  void SetLightCommand(const TracerLightCmd &cmd);
+    AgilexBase<ProtocolV2Parser>::SendMotionCommand(linear_vel, angular_vel, 0.0, 0.0);
+  }
+
+  void SetLightCommand(LightMode f_mode, uint8_t f_value) override {
+    AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, CONST_OFF, 0);
+  }
 
   // get robot state
-  TracerState GetTracerState();
+  TracerState GetRobotState() override {
+    std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);
+    return tracer_state_;
+  }
+
+  void ResetRobotState() override {
+    // TODO
+  }
   
  private:
   TracerState tracer_state_;
 
-  void ParseCANFrame(can_frame *rx_frame) override;
+  void ParseCANFrame(can_frame *rx_frame) override {
-  void UpdateTracerState(const AgxMessage &status_msg, TracerState &state);
+    AgxMessage status_msg;
+    if (AgilexBase<ProtocolV2Parser>::parser_.DecodeMessage(rx_frame, &status_msg)) {
+      UpdateTracerState(status_msg, tracer_state_);
+    }
+  }
+  
+  void UpdateTracerState(const AgxMessage &status_msg, TracerState &state) {
+    std::lock_guard<std::mutex> guard(AgilexBase<ProtocolV2Parser>::state_mutex_);  
+    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 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;
+      }
+        //   case AgxMsgRcState: {
+        //     state.rc_state = status_msg.body.rc_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