Merge pull request #60 from westonrobot/feature-ranger_mini_v3_support

Feature ranger mini v3 support

include/ugv_sdk/details/robot_base/ranger_base.hpp

@@ -21,11 +21,10 @@
 #include "ugv_sdk/details/protocol_v2/protocol_v2_parser.hpp"
 
 namespace westonrobot {
-class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
-                     public RangerInterface {
+class RangerBase : public AgilexBase<ProtocolV2Parser>, public RangerInterface {
  public:
-  RangerBaseV2() : AgilexBase<ProtocolV2Parser>(){};
-  virtual ~RangerBaseV2() = default;
+  RangerBase() : AgilexBase<ProtocolV2Parser>() {};
+  virtual ~RangerBase() = default;
 
   // set up connection
   bool Connect(std::string can_name) override {
@@ -88,6 +87,30 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
     return ranger_actuator;
   }
 
+  RangerCommonSensorState GetCommonSensorState() override {
+    auto common_sensor =
+        AgilexBase<ProtocolV2Parser>::GetCommonSensorStateMsgGroup();
+
+    RangerCommonSensorState ranger_bms;
+
+    ranger_bms.time_stamp = common_sensor.time_stamp;
+
+    ranger_bms.bms_basic_state.current = common_sensor.bms_basic_state.current;
+    ranger_bms.bms_basic_state.voltage =
+        common_sensor.bms_basic_state.voltage;
+    ranger_bms.bms_basic_state.battery_soc =
+        common_sensor.bms_basic_state.battery_soc;
+    ranger_bms.bms_basic_state.battery_soh =
+        common_sensor.bms_basic_state.battery_soh;
+    ranger_bms.bms_basic_state.temperature =
+        common_sensor.bms_basic_state.temperature;
+
+    return ranger_bms;
+  }
+};
+
+using RangerMiniV3Base = RangerBase;
+class RangerMiniV2Base : public RangerBase {
   RangerCommonSensorState GetCommonSensorState() override {
     auto common_sensor =
         AgilexBase<ProtocolV2Parser>::GetCommonSensorStateMsgGroup();
@@ -98,8 +121,7 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
 
     ranger_bms.bms_basic_state.current = common_sensor.bms_basic_state.current;
     // Note: BMS CAN message definition is not consistent across AgileX robots.
-    // Robots with steering mechanism should additionally divide the voltage by
-    // 10.
+    // RM2 BMS voltage data follows unit: 0.01V
     ranger_bms.bms_basic_state.voltage =
         common_sensor.bms_basic_state.voltage * 0.1f;
     ranger_bms.bms_basic_state.battery_soc =
@@ -116,9 +138,9 @@ class RangerBaseV2 : public AgilexBase<ProtocolV2Parser>,
 // Note: Ranger Mini V1 uses a modified AgileX V2 protocol
 // Here we provide a work-around fix as no new firmware will be provided from
 // AgileX to properly fix the issue.
-class RangerMiniV1Base : public RangerBaseV2 {
+class RangerMiniV1Base : public RangerBase {
  public:
-  RangerMiniV1Base() : RangerBaseV2(){};
+  RangerMiniV1Base() : RangerBase() {};
   ~RangerMiniV1Base() = default;
 
   // robot control

include/ugv_sdk/mobile_robot/ranger_robot.hpp

@@ -17,7 +17,14 @@
 namespace westonrobot {
 class RangerRobot : public RobotCommonInterface, public RangerInterface {
  public:
-  RangerRobot(bool is_mini_v1);
+  enum class Variant {
+    kRangerMiniV1 = 0,
+    kRangerMiniV2,
+    kRangerMiniV3,
+    kRanger,
+  };
+
+  RangerRobot(Variant variant);
   ~RangerRobot();
 
   bool Connect(std::string can_name) override;

python/ugv_sdk_py/src/ranger_robot.cpp

@@ -13,7 +13,6 @@
 namespace py = pybind11;
 
 namespace westonrobot {
-
 // clang-format off
 void BindRangerRobot(pybind11::module &m) {
     py::module_ m_ranger_robot = m.def_submodule("ranger_robot");
@@ -99,8 +98,8 @@ void BindRangerRobot(pybind11::module &m) {
 
     // RangerRobot class
     py::class_<RangerRobot>(m_ranger_robot, "RangerRobot")
-        .def(py::init<bool>(),
-            py::arg("is_mini_v1") = false,
+        .def(py::init<RangerRobot::Variant>(),
+            py::arg("variant"),
             "Constructor for RangerRobot with model version")
         .def("connect", &RangerRobot::Connect,
             py::arg("can_name"),
@@ -138,6 +137,13 @@ void BindRangerRobot(pybind11::module &m) {
             "Get the actuator state")
         .def("get_common_sensor_state", &RangerRobot::GetCommonSensorState,
             "Get the common sensor state");
+
+    py::enum_<RangerRobot::Variant>(m_ranger_robot, "Variant")
+    .value("kRangerMiniV1", RangerRobot::Variant::kRangerMiniV1)
+    .value("kRangerMiniV2", RangerRobot::Variant::kRangerMiniV2)
+    .value("kRangerMiniV3", RangerRobot::Variant::kRangerMiniV3)
+    .value("kRanger", RangerRobot::Variant::kRanger)
+    .export_values();
 }
 // clang-format on
 }  // namespace westonrobot

sample/ranger_demo/ranger_robot_demo.cpp

@@ -6,6 +6,7 @@
  * @Mail      : wangzheqie@qq.com
  * Copyright  : AgileX Robotics
  **/
+#include <string>
 
 #include <iomanip>
 
@@ -15,7 +16,7 @@ using namespace westonrobot;
 
 int main(int argc, char *argv[]) {
   std::string device_name;
-  bool is_mini_v1 = false;
+  RangerRobot::Variant variant = RangerRobot::Variant::kRanger;
 
   if (argc == 2) {
     device_name = {argv[1]};
@@ -24,19 +25,28 @@ int main(int argc, char *argv[]) {
     device_name = {argv[1]};
     std::string check = argv[2];
     if (check == "mini_v1") {
-      is_mini_v1 = true;
+      variant = RangerRobot::Variant::kRangerMiniV1;
       std::cout << "Specified mini v1" << std::endl;
+    } else if (check == "mini_v2") {
+      variant = RangerRobot::Variant::kRangerMiniV2;
+      std::cout << "Specified mini v2" << std::endl;
+    } else if (check == "mini_v3") {
+      variant = RangerRobot::Variant::kRangerMiniV3;
+      std::cout << "Specified mini v3" << std::endl;
+    } else {
+      std::cout << "Default to ranger base" << std::endl;
     }
     std::cout << "Specified CAN: " << device_name << std::endl;
   } else {
     std::cout << "Usage: app_ranger_demo <interface> <ranger_model>"
               << std::endl
-              << "Example 1: ./app_ranger_demo can0 mini_v1" << std::endl;
+              << "Example 1: ./app_ranger_demo can0 <mini_v1, mini_v2, mini_v3>"
+              << std::endl;
     return -1;
   }
 
   // RangerMiniV1Robot ranger;
-  auto ranger = std::make_shared<RangerRobot>(is_mini_v1);
+  auto ranger = std::make_shared<RangerRobot>(variant);
   ranger->Connect(device_name);
   ranger->EnableCommandedMode();
 

src/mobile_robot/ranger_robot.cpp

@@ -10,11 +10,15 @@
 #include "ugv_sdk/details/robot_base/ranger_base.hpp"
 
 namespace westonrobot {
-RangerRobot::RangerRobot(bool is_mini_v1) {
-  if (is_mini_v1) {
+RangerRobot::RangerRobot(Variant variant) {
+  if (variant == Variant::kRangerMiniV1) {
     robot_ = new RangerMiniV1Base();
+  } else if (variant == Variant::kRangerMiniV2) {
+    robot_ = new RangerMiniV2Base();
+  } else if (variant == Variant::kRangerMiniV3) {
+    robot_ = new RangerMiniV3Base();
   } else {
-    robot_ = new RangerBaseV2();
+    robot_ = new RangerBase();
   }
 }