Use mp-units for type-safe physical units and bug-free conversions.

.github/workflows/ros2.yml

@@ -30,6 +30,36 @@ jobs:
         ros_distro: [ humble ]
 
     steps:   
+    - name: Install gsl-lite
+      run: |
+        git clone https://github.com/gsl-lite/gsl-lite && cd gsl-lite
+        mkdir build && cd build
+        cmake .. && make -j8
+        sudo make install
+
+    - name: Install Catch2
+      run: |
+        git clone https://github.com/catchorg/Catch2 && cd Catch2
+        mkdir build && cd build
+        cmake .. && make -j8
+        sudo make install
+
+    - name: Install fmt
+      run: |
+        git clone https://github.com/fmtlib/fmt && cd fmt
+        mkdir build && cd build
+        cmake -DFMT_TEST=OFF ..
+        make -j8
+        sudo make install
+
+    - name: Install mp-units
+      run: |
+        git clone https://github.com/mpusz/mp-units && cd mp-units
+        mkdir build && cd build
+        cmake -DMP_UNITS_AS_SYSTEM_HEADERS=ON -DMP_UNITS_BUILD_LA=OFF ..
+        make -j8
+        sudo make install
+
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(ros2_heartbeat REQUIRED)
+find_package(mp-units REQUIRED)
 #######################################################################################
 include_directories(
   include
@@ -25,8 +26,8 @@ add_executable(${L3XZ_TELEOP_TARGET}
   src/main.cpp
 )
 #######################################################################################
-target_compile_features(${L3XZ_TELEOP_TARGET} PRIVATE cxx_std_17)
-ament_target_dependencies(${L3XZ_TELEOP_TARGET} rclcpp std_msgs sensor_msgs geometry_msgs ros2_heartbeat)
+target_compile_features(${L3XZ_TELEOP_TARGET} PRIVATE cxx_std_20)
+ament_target_dependencies(${L3XZ_TELEOP_TARGET} rclcpp std_msgs sensor_msgs geometry_msgs ros2_heartbeat mp-units)
 #######################################################################################
 install(TARGETS
   ${L3XZ_TELEOP_TARGET}

README.md

@@ -11,6 +11,37 @@ Teleoperation for L3X-Z via PS3 joystick and ROS topics.
 </p>
 
 #### How-to-build
+* Install `gsl-lite`
+```bash
+git clone https://github.com/gsl-lite/gsl-lite && cd gsl-lite
+mkdir build && cd build
+cmake .. && make -j8
+sudo make install
+```
+* Install `Catch2`
+```bash
+git clone https://github.com/catchorg/Catch2 && cd Catch2
+mkdir build && cd build
+cmake .. && make -j8
+sudo make install
+```
+* Install `fmt`
+```bash
+git clone https://github.com/fmtlib/fmt && cd fmt
+mkdir build && cd build
+cmake -DFMT_TEST=OFF ..
+make -j8
+sudo make install
+```
+* Install `mp-units`
+```bash
+git clone https://github.com/mpusz/mp-units && cd mp-units
+mkdir build && cd build
+cmake -DMP_UNITS_AS_SYSTEM_HEADERS=ON -DMP_UNITS_BUILD_LA=OFF ..
+make -j8
+sudo make install
+```
+* Build with `colcon`
 ```bash
 cd $COLCON_WS/src
 git clone https://github.com/107-systems/l3xz_teleop

src/Node.cpp

@@ -14,10 +14,18 @@
 #include <numeric>
 #include <sstream>
 
+#include <mp-units/systems/si/si.h>
+#include <mp-units/systems/angular/angular.h>
+
 /**************************************************************************************
  * NAMESPACE
  **************************************************************************************/
 
+using namespace mp_units;
+using mp_units::angular::unit_symbols::deg;
+using mp_units::angular::unit_symbols::rad;
+using mp_units::si::unit_symbols::s;
+
 namespace l3xz
 {
 
@@ -120,10 +128,15 @@ void Node::onJoyMsg(sensor_msgs::msg::Joy::SharedPtr const joy_msg)
   _robot_msg.linear.x  = (-1.0f) * joy_msg->axes[1]; /* LEFT_STICK_VERTICAL   */
   _robot_msg.angular.z =           joy_msg->axes[0]; /* LEFT_STICK_HORIZONTAL */
 
-  float const pan_angular_velocity_dps  =           joy_msg->axes[3] * get_parameter("pan_max_dps").as_double();  /* RIGHT_STICK_HORIZONTAL */
-  float const tilt_angular_velocity_dps = (-1.0f) * joy_msg->axes[4] * get_parameter("tilt_max_dps").as_double(); /* RIGHT_STICK_VERTICAL   */
-  _head_msg.angular.z = pan_angular_velocity_dps  * M_PI / 180.0f;
-  _head_msg.angular.y = tilt_angular_velocity_dps * M_PI / 180.0f;
+  /* RIGHT_STICK_HORIZONTAL */
+  auto const pan_angular_velocity =
+    (joy_msg->axes[3] * get_parameter("pan_max_dps").as_double()  * deg / s);
+  /* RIGHT_STICK_VERTICAL   */
+  auto const tilt_angular_velocity =
+    (-1. * joy_msg->axes[4] * get_parameter("tilt_max_dps").as_double() * deg / s);
+
+  _head_msg.angular.z = pan_angular_velocity.numerical_value_in(rad/s);
+  _head_msg.angular.y = tilt_angular_velocity.numerical_value_in(rad/s);
 
   _req_up_msg.data = joy_msg->buttons[0] != 0;