pybind: updated scout robot binding, not fully tested yet

CMakeLists.txt

@@ -11,8 +11,6 @@ endif()
 option(BUILD_TESTING "Build tests" OFF)
 option(STATIC_CHECK "Run static check" OFF)
 option(PYTHON_BINDING "Build python binding" OFF)
-# set(CMAKE_BUILD_TYPE Release)
-# set(CMAKE_BUILD_TYPE Debug)
 
 ## Check if pkg is built with ROS catkin
 if(CATKIN_DEVEL_PREFIX)

MANIFEST.in

@@ -0,0 +1,9 @@
+include CMakeLists.txt
+include LICENSE
+include README.md
+include package.xml
+recursive-include sample *
+recursive-include cmake *
+recursive-include include *
+recursive-include src *
+recursive-include python *

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>ugv_sdk</name>
-  <version>0.1.6</version>
+  <version>0.8.0</version>
   <description>Weston Robot Platform SDK</description>
 
   <author email="[email protected]">Ruixiang Du</author>

python/ugv_sdk/agilex_message.cpp

@@ -4,17 +4,25 @@
  * Created on 5/8/24 7:34 PM
  * Description:
  *
- * Copyright (c) 2024 Ruixiang Du (rdu)
+ * Copyright (c) 2024 Weston Robot Pte. Ltd.
  */
 
 #include <pybind11/pybind11.h>
 
 #include "agilex_message.hpp"
 #include "ugv_sdk/details/interface/agilex_message.h"
+#include "ugv_sdk/details/parser_base.hpp"
 
 namespace py = pybind11;
 namespace westonrobot {
     // clang-format off
+    void BindProtocolVersion(pybind11::module &m) {
+        py::enum_<ProtocolVersion>(m, "ProtocolVersion")
+                .value("AGX_V1", ProtocolVersion::AGX_V1)
+                .value("AGX_V2", ProtocolVersion::AGX_V2)
+                .export_values();
+    }
+
     void BindSystemStateMessage(py::module &m) {
         py::enum_<AgxVehicleState>(m, "AgxVehicleState")
                 .value("VEHICLE_STATE_NORMAL", AgxVehicleState::VEHICLE_STATE_NORMAL)
@@ -85,5 +93,44 @@ namespace westonrobot {
                 .def_readwrite("stick_left_h", &RcStateMessage::stick_left_h)
                 .def_readwrite("var_a", &RcStateMessage::var_a);
     }
+
+    void BindActuatorHSStateMessage(pybind11::module &m) {
+        py::class_<ActuatorHSStateMessage>(m, "ActuatorHSStateMessage")
+                .def(py::init<>())
+                .def_readwrite("motor_id", &ActuatorHSStateMessage::motor_id)
+                .def_readwrite("rpm", &ActuatorHSStateMessage::rpm)
+                .def_readwrite("current", &ActuatorHSStateMessage::current)
+                .def_readwrite("pulse_count", &ActuatorHSStateMessage::pulse_count);
+    }
+
+    void BindActuatorLSStateMessage(pybind11::module &m) {
+        py::class_<ActuatorLSStateMessage>(m, "ActuatorLSStateMessage")
+                .def(py::init<>())
+                .def_readwrite("motor_id", &ActuatorLSStateMessage::motor_id)
+                .def_readwrite("driver_voltage", &ActuatorLSStateMessage::driver_voltage)
+                .def_readwrite("driver_temp", &ActuatorLSStateMessage::driver_temp)
+                .def_readwrite("motor_temp", &ActuatorLSStateMessage::motor_temp)
+                .def_readwrite("driver_state", &ActuatorLSStateMessage::driver_state);
+    }
+
+    void BindActuatorStateMessageV1(pybind11::module &m) {
+        py::class_<ActuatorStateMessageV1>(m, "ActuatorStateMessageV1")
+                .def(py::init<>())
+                .def_readwrite("motor_id", &ActuatorStateMessageV1::motor_id)
+                .def_readwrite("current", &ActuatorStateMessageV1::current)
+                .def_readwrite("rpm", &ActuatorStateMessageV1::rpm)
+                .def_readwrite("driver_temp", &ActuatorStateMessageV1::driver_temp)
+                .def_readwrite("motor_temp", &ActuatorStateMessageV1::motor_temp);
+    }
+
+    void BindBmsBasicMessage(pybind11::module &m) {
+        py::class_<BmsBasicMessage>(m, "BmsBasicMessage")
+                .def(py::init<>())
+                .def_readwrite("battery_soc", &BmsBasicMessage::battery_soc)
+                .def_readwrite("battery_soh", &BmsBasicMessage::battery_soh)
+                .def_readwrite("voltage", &BmsBasicMessage::voltage)
+                .def_readwrite("current", &BmsBasicMessage::current)
+                .def_readwrite("temperature", &BmsBasicMessage::temperature);
+    }
     // clang-format on
 }

python/ugv_sdk/agilex_message.hpp

@@ -4,7 +4,7 @@
  * Created on 5/8/24 7:36 PM
  * Description:
  *
- * Copyright (c) 2024 Ruixiang Du (rdu)
+ * Copyright (c) 2024 Weston Robot Pte. Ltd.
  */
 
 #ifndef UGV_SDK_AGILEX_MESSAGE_HPP
@@ -13,14 +13,16 @@
 #include <pybind11/pybind11.h>
 
 namespace westonrobot {
+    void BindProtocolVersion(pybind11::module &m);
     void BindSystemStateMessage(pybind11::module &m);
     void BindMotionStateMessage(pybind11::module &m);
     void BindLightStateMessage(pybind11::module &m);
     void BindRcStateMessage(pybind11::module &m);
     void BindActuatorHSStateMessage(pybind11::module &m);
     void BindActuatorLSStateMessage(pybind11::module &m);
-    void BindMotionModeStateMessage(pybind11::module &m);
     void BindActuatorStateMessageV1(pybind11::module &m);
+    void BindMotionModeStateMessage(pybind11::module &m);
+    void BindBmsBasicMessage(pybind11::module &m);
 }
 
 #endif //UGV_SDK_AGILEX_MESSAGE_HPP

python/ugv_sdk/scout_robot.cpp

@@ -4,7 +4,7 @@
  * Created on 5/8/24 2:57 PM
  * Description:
  *
- * Copyright (c) 2024 Ruixiang Du (rdu)
+ * Copyright (c) 2024 Weston Robot Pte. Ltd.
  */
 
 #include <pybind11/pybind11.h>
@@ -20,53 +20,131 @@ using namespace westonrobot;
 PYBIND11_MODULE(scout_robot, m) {
     m.doc() = "Python bindings for handling a Scout robot";
 
+    BindProtocolVersion(m);
     BindSystemStateMessage(m);
     BindMotionStateMessage(m);
     BindLightStateMessage(m);
     BindRcStateMessage(m);
-
-    py::enum_<ProtocolVersion>(m, "ProtocolVersion")
-        .value("AGX_V1", ProtocolVersion::AGX_V1)
-        .value("AGX_V2", ProtocolVersion::AGX_V2)
-        .export_values();
+    BindActuatorHSStateMessage(m);
+    BindActuatorLSStateMessage(m);
+    BindBmsBasicMessage(m);
 
     py::class_<ScoutCoreState>(m, "ScoutCoreState")
         .def(py::init<>())
         .def_property("time_stamp",
         [](const ScoutCoreState &s) {
-                        // Convert time_point to duration since epoch in milliseconds
-                        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
-                                s.time_stamp.time_since_epoch());
-                        return duration.count();
-                        },
-                        [](ScoutCoreState &s, long long ms) {
-                        // Convert milliseconds back to time_point
-                        s.time_stamp = std::chrono::steady_clock::time_point(
-                                std::chrono::milliseconds(ms));
-                        })
+            // Convert time_point to duration since epoch in milliseconds
+            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+                    s.time_stamp.time_since_epoch());
+            return duration.count();
+            },
+            [](ScoutCoreState &s, long long ms) {
+            // Convert milliseconds back to time_point
+            s.time_stamp = std::chrono::steady_clock::time_point(
+                    std::chrono::milliseconds(ms));
+            })
         .def_readwrite("system_state", &ScoutCoreState::system_state)
         .def_readwrite("motion_state", &ScoutCoreState::motion_state)
         .def_readwrite("light_state", &ScoutCoreState::light_state)
         .def_readwrite("rc_state", &ScoutCoreState::rc_state);
 
+    py::class_<ScoutActuatorState>(m, "ScoutActuatorState")
+        .def(py::init<>())
+        .def_property("time_stamp",
+        [](const ScoutActuatorState &s) {
+                // Convert time_point to duration since epoch in milliseconds
+                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+                        s.time_stamp.time_since_epoch());
+                return duration.count();
+            },
+            [](ScoutActuatorState &s, long long ms) {
+                // Convert milliseconds back to time_point
+                s.time_stamp = std::chrono::steady_clock::time_point(
+                        std::chrono::milliseconds(ms));
+            })
+        .def_property("actuator_hs_state",
+            [](const ScoutActuatorState &s) {
+                // Convert C-style array to Python list
+                return py::list(py::make_iterator(std::begin(s.actuator_hs_state), std::end(s.actuator_hs_state)));
+            },
+            [](ScoutActuatorState &s, py::list l) {
+                // Convert Python list back to C-style array
+                if (l.size() != 4) throw std::runtime_error("List size must be 4");
+                    for (size_t i = 0; i < 4; ++i) {
+                        s.actuator_hs_state[i] = l[i].cast<ActuatorHSStateMessage>();
+                    }
+            })
+        .def_property("actuator_ls_state",
+            [](const ScoutActuatorState &s) {
+                // Convert C-style array to Python list
+                return py::list(py::make_iterator(std::begin(s.actuator_ls_state), std::end(s.actuator_ls_state)));
+            },
+            [](ScoutActuatorState &s, py::list l) {
+                // Convert Python list back to C-style array
+                if (l.size() != 4) throw std::runtime_error("List size must be 4");
+                for (size_t i = 0; i < 4; ++i) {
+                s.actuator_ls_state[i] = l[i].cast<ActuatorLSStateMessage>();
+            }
+        })
+        .def_property("actuator_state",
+            [](const ScoutActuatorState &s) {
+                // Convert C-style array to Python list
+                return py::list(py::make_iterator(std::begin(s.actuator_state), std::end(s.actuator_state)));
+            },
+            [](ScoutActuatorState &s, py::list l) {
+                // Convert Python list back to C-style array
+                if (l.size() != 4) throw std::runtime_error("List size must be 4");
+                for (size_t i = 0; i < 4; ++i) {
+                s.actuator_state[i] = l[i].cast<ActuatorStateMessageV1>();
+            }
+        });
+
+    py::class_<ScoutCommonSensorState>(m, "ScoutCommonSensorState")
+        .def(py::init<>())
+        .def_property("time_stamp",
+        [](const ScoutCommonSensorState &s) {
+                // Convert time_point to duration since epoch in milliseconds
+                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+                        s.time_stamp.time_since_epoch());
+                return duration.count();
+            },
+            [](ScoutCommonSensorState &s, long long ms) {
+                // Convert milliseconds back to time_point
+                s.time_stamp = std::chrono::steady_clock::time_point(
+                        std::chrono::milliseconds(ms));
+            })
+        .def_readwrite("bms_basic_state", &ScoutCommonSensorState::bms_basic_state);
+
     // ScoutRobot class
     py::class_<ScoutRobot>(m, "ScoutRobot")
         .def(py::init<ProtocolVersion, bool>(),
             py::arg("protocol") = ProtocolVersion::AGX_V2,
             py::arg("is_mini_model") = false,
             "Constructor for ScoutRobot with optional protocol and model type")
-        .def("Connect", &ScoutRobot::Connect,
+        .def("connect", &ScoutRobot::Connect,
             py::arg("can_name"),
             "Connects the robot to the specified CAN interface.")
-        .def("EnableCommandedMode", &ScoutRobot::EnableCommandedMode,
+        .def("enable_commanded_mode", &ScoutRobot::EnableCommandedMode,
              "To enable robot control")
-        .def("RequestVersion", &ScoutRobot::RequestVersion,
+        .def("request_version", &ScoutRobot::RequestVersion,
             py::arg("timeout_sec") = 3,
             "Request the robot version")
-        .def("SetMotionCommand", &ScoutRobot::SetMotionCommand,
+        .def("set_motion_command", &ScoutRobot::SetMotionCommand,
             py::arg("linear_vel"), py::arg("angular_vel"),
             "Set the motion command for the robot")
+        .def("set_light_command", &ScoutRobot::SetLightCommand,
+            py::arg("f_mode"), py::arg("f_value"),
+            py::arg("r_mode"), py::arg("r_value"),
+            "Set the light command for the robot")
+        .def("disable_light_control", &ScoutRobot::DisableLightControl,
+            "Disable light control")
+        .def("reset_robot_state", &ScoutRobot::ResetRobotState,
+            "Reset the robot state")
+        .def("get_parser_protocol_version", &ScoutRobot::GetParserProtocolVersion,
+            "Get the parser protocol version")
         // get robot state
-        .def("GetRobotState", &ScoutRobot::GetRobotState, "Get the robot state");
+        .def("get_robot_state", &ScoutRobot::GetRobotState, "Get the robot state")
+        .def("get_actuator_state", &ScoutRobot::GetActuatorState, "Get the actuator state")
+        .def("get_common_sensor_state", &ScoutRobot::GetCommonSensorState, "Get the common sensor state");
 }
 // clang-format on

setup.py

@@ -30,15 +30,9 @@ def build_extension(self, ext):
         cfg = 'Debug' if self.debug else 'Release'
         build_args = ['--config', cfg]
 
-        if platform.system() == "Windows":
-            cmake_args += ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)]
-            if sys.maxsize > 2**32:
-                cmake_args += ['-A', 'x64']
-            build_args += ['--', '/m']
-        else:
-            cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg]
-            cmake_args += ['-DPYTHON_BINDING=ON']
-            build_args += ['--', '-j2']
+        cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg]
+        cmake_args += ['-DPYTHON_BINDING=ON']
+        build_args += ['--', '-j2']
 
         env = os.environ.copy()
         env['CXXFLAGS'] = '{} -DVERSION_INFO=\\"{}\\"'.format(env.get('CXXFLAGS', ''),
@@ -50,7 +44,7 @@ def build_extension(self, ext):
 
 setup(
     name='ugv_sdk_py',
-    version='0.8.0',
+    version='0.1.1',
     author='Ruixiang Du',
     author_email='[email protected]',
     description='Python bindings for the ugv_sdk library using pybind11',

test/python/test_scout_robot.py

@@ -4,23 +4,26 @@
 
 def test_scout_robot():
     # Create an instance of ScoutRobot
-    robot = scout_robot.ScoutRobot(scout_robot.ProtocolVersion.AGX_V1, True)
+    robot = scout_robot.ScoutRobot(scout_robot.ProtocolVersion.AGX_V2, True)
 
     # Connect to the robot
     try:
-        robot.Connect("can0")
+        robot.connect("can0")
         print("Connection to the robot established.")
     except Exception as e:
         print(f"Failed to connect to the robot: {e}")
         return
 
     # Enable commanded mode
-    robot.EnableCommandedMode()
+    robot.enable_commanded_mode()
 
     # Set motion command
     while True:
-        robot.SetMotionCommand(1.0, 0.0)
-        state = robot.GetRobotState()
+        # robot control
+        robot.set_motion_command(1.0, 0.0)
+
+        # robot state monitoring
+        state = robot.get_robot_state()
         print("time: ", state.time_stamp)
         print("battery voltage: ", state.system_state.battery_voltage)
         print(
@@ -42,6 +45,7 @@ def test_scout_robot():
             state.rc_state.stick_left_h,
             state.rc_state.var_a
         ))
+
         time.sleep(20 / 1000)