updated ranger base

demo/CMakeLists.txt

@@ -1,6 +1,7 @@
 # demo
 add_subdirectory(scout_demo)
 add_subdirectory(tracer_demo)
+add_subdirectory(ranger_demo)
 
 # add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp)
 # target_link_libraries(app_hunter_demo ugv_sdk)

demo/ranger_demo/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_executable(demo_ranger_robot ranger_demo.cpp)
+target_link_libraries(demo_ranger_robot ugv_sdk)

demo/ranger_demo/ranger_demo.cpp

@@ -7,7 +7,7 @@
  * Copyright  : AgileX Robotics
  **/
 
-#include "ugv_sdk/protocol_v2/ranger_base.hpp"
+#include "ugv_sdk/mobile_robot/ranger_robot.hpp"
 
 using namespace westonrobot;
 
@@ -23,7 +23,7 @@ int main(int argc, char *argv[]) {
     return -1;
   }
 
-  RangerBase ranger;
+  RangerRobot ranger;
   ranger.Connect(device_name);
 
   ranger.EnableCommandedMode();
@@ -35,7 +35,7 @@ int main(int argc, char *argv[]) {
       ranger.SetMotionCommand(0.2, 0.0);
     }
 
-    auto state = ranger.GetRangerState();
+    auto state = ranger.GetRobotState();
     std::cout << "-------------------------------" << std::endl;
     std::cout << "count: " << count << std::endl;
     std::cout << "control mode: "

include/ugv_sdk/details/interface/ranger_interface.hpp

@@ -12,45 +12,23 @@
 
 #include <string>
 
-#include "ugv_sdk/interface/agilex_message.h"
+#include "ugv_sdk/details/interface/agilex_message.h"
 
 namespace westonrobot {
-struct RangerState {
+struct RangerCoreState {
   // system state
   SystemStateMessage system_state;
   MotionStateMessage motion_state;
   LightStateMessage light_state;
-
-  RcStateMessage rc_state;
-
-  ActuatorHSStateMessage actuator_hs_state[8];
-  ActuatorLSStateMessage actuator_ls_state[8];
   MotionModeStateMessage current_motion_mode;
 
-  // sensor data
+  RcStateMessage rc_state;
   OdometryMessage odometry;
 };
 
-struct RangerMotionCmd {
-  double linear_velocity;
-  double angular_velocity;
-};
-
-struct RangerLightCmd {
-  RangerLightCmd() = default;
-  RangerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode,
-                 uint8_t r_value)
-      : enable_cmd_ctrl(true),
-        front_mode(f_mode),
-        front_custom_value(f_value),
-        rear_mode(r_mode),
-        rear_custom_value(r_value) {}
-
-  bool enable_cmd_ctrl = false;
-  LightMode front_mode;
-  uint8_t front_custom_value;
-  LightMode rear_mode;
-  uint8_t rear_custom_value;
+struct RangerActuatorState {
+  ActuatorHSStateMessage actuator_hs_state[8];
+  ActuatorLSStateMessage actuator_ls_state[8];
 };
 
 /////////////////////////////////////////////////////////////////////////
@@ -59,13 +37,15 @@ struct RangerInterface {
   virtual void Connect(std::string dev_name) = 0;
 
   // robot control
+  virtual void SetMotionMode(uint8_t mode) = 0;
   virtual void SetMotionCommand(double linear_vel, double steer_angle,
                                 double lateral_vel, double angular_vel) = 0;
-  virtual void SetLightCommand(const RangerLightCmd &cmd) = 0;
-  virtual void SetMotionMode(uint8_t mode) = 0;
+  virtual void SetLightCommand(LightMode f_mode, uint8_t f_value,
+                               LightMode r_mode, uint8_t r_value) = 0;
 
   // get robot state
-  virtual RangerState GetRangerState() = 0;
+  virtual RangerCoreState GetRobotState() = 0;
+  virtual RangerActuatorState GetActuatorState() = 0;
 };
 }  // namespace westonrobot
 

include/ugv_sdk/details/interface/robot_interface.hpp

@@ -56,8 +56,12 @@ class RobotInterface {
   /****** functions not available/valid to all robots ******/
   // functions to be implemented by class AgilexBase
   virtual void SetMotionMode(uint8_t mode){};
-  virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup(){};
-  virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup(){};
+  virtual CoreStateMsgGroup GetRobotCoreStateMsgGroup() {
+    return CoreStateMsgGroup{};
+  };
+  virtual ActuatorStateMsgGroup GetActuatorStateMsgGroup() {
+    return ActuatorStateMsgGroup{};
+  };
 
   // any specific robot will use a specialized version of the two functions
   virtual void SendMotionCommand(double linear_vel, double angular_vel,

include/ugv_sdk/details/protocol_v1/protocol_v1_parser.hpp

@@ -52,12 +52,14 @@ class ProtocolV1Parser : public ParserInterface<ProtocolVersion::AGX_V1> {
   }
 
   // UART support
-  bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {}
+  bool DecodeMessage(uint8_t *data, uint8_t dlc, AgxMessage *msg) override {
+    return false;
+  }
 
   void EncodeMessage(const AgxMessage *msg, uint8_t *buf,
                      uint8_t *len) override {}
 
-  uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override {}
+  uint8_t CalculateChecksum(uint8_t *buf, uint8_t len) override { return 0; }
 };
 
 using ScoutProtocolV1Parser = ProtocolV1Parser<ScoutV2Limits>;

include/ugv_sdk/details/robot_base/ranger_base.cpp

@@ -1,111 +0,0 @@
-/**
- * @Kit       : Qt-Creator: Desktop
- * @Author    : Wang Zhe
- * @Date      : 2021-04-19  19:36:24
- * @FileName  : ranger_base.cpp
- * @Mail      : wangzheqie@qq.com
- * Copyright  : AgileX Robotics
- **/
-
-#include "ugv_sdk/protocol_v2/ranger_base.hpp"
-
-#include <algorithm>
-#include <array>
-#include <chrono>
-#include <cstdint>
-#include <cstring>
-#include <iostream>
-#include <ratio>
-#include <string>
-#include <thread>
-
-#include "stopwatch.hpp"
-
-#include "ugv_sdk/protocol_v2/agilex_msg_parser.h"
-
-namespace westonrobot {
-void RangerBase::Connect(std::string dev_name) {
-  AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame, this,
-                                          std::placeholders::_1));
-}
-
-void RangerBase::SetMotionCommand(double linear_vel, double steer_angle,
-                                  double lateral_vel, double angular_vel) {
-  AgilexBase::SetMotionCommand(linear_vel, angular_vel, lateral_vel,
-                               steer_angle / 10.0);
-}
-
-void RangerBase::SetLightCommand(const RangerLightCmd &cmd) {
-  if (cmd.enable_cmd_ctrl) {
-    AgilexBase::SendLightCommand(cmd.front_mode, cmd.front_custom_value,
-                                 LightMode::CONST_OFF, 0);
-  }
-}
-
-void RangerBase::SetMotionMode(uint8_t mode) {
-  AgilexBase::SetMotionMode(mode);
-}
-
-RangerState RangerBase::GetRangerState() {
-  std::lock_guard<std::mutex> guard(state_mutex_);
-  return ranger_state_;
-}
-
-void RangerBase::ParseCANFrame(can_frame *rx_frame) {
-  AgxMessage status_msg;
-  DecodeCanFrameV2(rx_frame, &status_msg);
-  std::lock_guard<std::mutex> guard(state_mutex_);
-  UpdateRangerState(status_msg, ranger_state_);
-}
-
-void RangerBase::UpdateRangerState(const AgxMessage &status_msg,
-                                   RangerState &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;
-      state.motion_state.steering_angle *= 10;
-      // std::cout << "steering angle: " << state.motion_state.steering_angle <<
-      // std::endl;
-      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;
-    }
-    case AgxMsgMotionModeState: {
-      state.current_motion_mode = status_msg.body.motion_mode_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

include/ugv_sdk/details/robot_base/ranger_base.hpp

@@ -15,32 +15,65 @@
 #include <string>
 #include <thread>
 
-#include "ugv_sdk/interface/ranger_interface.hpp"
-#include "ugv_sdk/protocol_v2/agilex_base.hpp"
+#include "ugv_sdk/details/interface/ranger_interface.hpp"
+#include "ugv_sdk/details/robot_base/agilex_base.hpp"
+
+#include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
 
 namespace westonrobot {
-class RangerBase : public AgilexBase, public RangerInterface {
+class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
  public:
-  RangerBase() : AgilexBase(){};
+  RangerBase() : AgilexBase<ProtocolV2Parser>(){};
   ~RangerBase() = default;
 
   // set up connection
-  void Connect(std::string dev_name) override;
+  void Connect(std::string dev_name) override {
+    AgilexBase::ConnectPort(dev_name, std::bind(&RangerBase::ParseCANFrame,
+                                                this, std::placeholders::_1));
+  }
 
   // robot control
+  void SetMotionCommand() {}
+
   void SetMotionCommand(double linear_vel, double steer_angle,
-                        double lateral_vel = 0.0, double angular_vel = 0.0);
-  void SetLightCommand(const RangerLightCmd &cmd);
-  void SetMotionMode(uint8_t mode);
+                        double lateral_vel = 0.0,
+                        double angular_vel = 0.0) override {
+    AgilexBase<ProtocolV2Parser>::SendMotionCommand(
+        linear_vel, angular_vel, lateral_vel, steer_angle / 10.0);
+  }
+
+  void SetLightCommand(LightMode f_mode, uint8_t f_value, LightMode r_mode,
+                       uint8_t r_value) override {
+    AgilexBase<ProtocolV2Parser>::SendLightCommand(f_mode, f_value, r_mode,
+                                                   r_value);
+  }
+
+  void SetMotionMode(uint8_t mode) { AgilexBase::SetMotionMode(mode); }
 
   // get robot state
-  RangerState GetRangerState();
+  RangerCoreState GetRobotState() override {
+    auto state = AgilexBase<ProtocolV2Parser>::GetRobotCoreStateMsgGroup();
 
- private:
-  RangerState ranger_state_;
+    RangerCoreState ranger_state;
+    ranger_state.system_state = state.system_state;
+    ranger_state.motion_state = state.motion_state;
+    ranger_state.light_state = state.light_state;
+    ranger_state.rc_state = state.rc_state;
+    return ranger_state;
+  }
 
-  void ParseCANFrame(can_frame *rx_frame) override;
-  void UpdateRangerState(const AgxMessage &status_msg, RangerState &state);
+  RangerActuatorState GetActuatorState() override {
+    auto actuator = AgilexBase<ProtocolV2Parser>::GetActuatorStateMsgGroup();
+
+    RangerActuatorState ranger_actuator;
+    for (int i = 0; i < 8; ++i) {
+      ranger_actuator.actuator_hs_state[i] = actuator.actuator_hs_state[i];
+      ranger_actuator.actuator_ls_state[i] = actuator.actuator_ls_state[i];
+    }
+    return ranger_actuator;
+  }
 };
+
+using RangerBaseV2 = RangerBase;
 }  // namespace westonrobot
 #endif  // RANGER_BASE_HPP

include/ugv_sdk/mobile_robot/ranger_robot.hpp

@@ -0,0 +1,19 @@
+/* 
+ * ranger_robot.hpp
+ * 
+ * Created on: Jul 14, 2021 21:45
+ * Description: 
+ * 
+ * Copyright (c) 2021 Ruixiang Du (rdu)
+ */ 
+
+#ifndef RANGER_ROBOT_HPP
+#define RANGER_ROBOT_HPP
+
+#include "ugv_sdk/details/robot_base/ranger_base.hpp"
+
+namespace westonrobot {
+using RangerRobot = RangerBaseV2;
+}  // namespace westonrobot
+
+#endif /* RANGER_ROBOT_HPP */