From 3ca62e8895a442f751a43fb1c1491d03a1c38533 Mon Sep 17 00:00:00 2001 From: karthee-weston Date: Wed, 9 Oct 2024 14:41:23 +0800 Subject: [PATCH] ranger_base: added support for ranger mini v3 and updated constructor of RangerRobot class to support all ranger variants --- .../details/robot_base/ranger_base.hpp | 38 +++++++++++++++---- include/ugv_sdk/mobile_robot/ranger_robot.hpp | 9 ++++- python/ugv_sdk_py/src/ranger_robot.cpp | 12 ++++-- sample/ranger_demo/ranger_robot_demo.cpp | 18 +++++++-- src/mobile_robot/ranger_robot.cpp | 10 +++-- 5 files changed, 68 insertions(+), 19 deletions(-) diff --git a/include/ugv_sdk/details/robot_base/ranger_base.hpp b/include/ugv_sdk/details/robot_base/ranger_base.hpp index 0d2a16d8..c23e8d13 100644 --- a/include/ugv_sdk/details/robot_base/ranger_base.hpp +++ b/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, - public RangerInterface { +class RangerBase : public AgilexBase, public RangerInterface { public: - RangerBaseV2() : AgilexBase(){}; - virtual ~RangerBaseV2() = default; + RangerBase() : AgilexBase() {}; + virtual ~RangerBase() = default; // set up connection bool Connect(std::string can_name) override { @@ -88,6 +87,30 @@ class RangerBaseV2 : public AgilexBase, return ranger_actuator; } + RangerCommonSensorState GetCommonSensorState() override { + auto common_sensor = + AgilexBase::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::GetCommonSensorStateMsgGroup(); @@ -98,8 +121,7 @@ class RangerBaseV2 : public AgilexBase, 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, // 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 diff --git a/include/ugv_sdk/mobile_robot/ranger_robot.hpp b/include/ugv_sdk/mobile_robot/ranger_robot.hpp index 2f0000cf..81441afc 100644 --- a/include/ugv_sdk/mobile_robot/ranger_robot.hpp +++ b/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; diff --git a/python/ugv_sdk_py/src/ranger_robot.cpp b/python/ugv_sdk_py/src/ranger_robot.cpp index 56b1959d..eddedeb4 100644 --- a/python/ugv_sdk_py/src/ranger_robot.cpp +++ b/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_(m_ranger_robot, "RangerRobot") - .def(py::init(), - py::arg("is_mini_v1") = false, + .def(py::init(), + 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_(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 \ No newline at end of file diff --git a/sample/ranger_demo/ranger_robot_demo.cpp b/sample/ranger_demo/ranger_robot_demo.cpp index 55facea6..1e7f7b9b 100644 --- a/sample/ranger_demo/ranger_robot_demo.cpp +++ b/sample/ranger_demo/ranger_robot_demo.cpp @@ -6,6 +6,7 @@ * @Mail : wangzheqie@qq.com * Copyright : AgileX Robotics **/ +#include #include @@ -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 " << std::endl - << "Example 1: ./app_ranger_demo can0 mini_v1" << std::endl; + << "Example 1: ./app_ranger_demo can0 " + << std::endl; return -1; } // RangerMiniV1Robot ranger; - auto ranger = std::make_shared(is_mini_v1); + auto ranger = std::make_shared(variant); ranger->Connect(device_name); ranger->EnableCommandedMode(); diff --git a/src/mobile_robot/ranger_robot.cpp b/src/mobile_robot/ranger_robot.cpp index 8e585e7d..3fb9da84 100644 --- a/src/mobile_robot/ranger_robot.cpp +++ b/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(); } }