Add support to auto transform messages using the odom sensor

Author: evan-palmer

topic_sensors/README.md

@@ -1,14 +1,20 @@
 # Topic Sensors
 
-This package provides a collection of Sensor interfaces that write incoming
+This package provides a collection of sensor interfaces that write incoming
 ROS 2 messages to ros2_control state interfaces.
 
 ## Odometry Sensor
 
 A sensor plugin that reads incoming `nav_msgs/Odometry` messages and writes
-them to state interfaces. No manipulation is done to the data, beyond proxying
-the message values.
+them to state interfaces.
 
 ### Plugin Library
 
 topic_sensors/odom_sensor
+
+### Parameters
+
+- topic: The topic that the sensor should subscribe to
+- transform: Whether or not the sensor should transform the message from the
+ROS [REP-105](https://ros.org/reps/rep-0105.html) convention to the maritime
+convention documented in [REP-156](https://github.com/ros-infrastructure/rep/pull/398).

topic_sensors/src/odom_sensor.cpp

@@ -24,6 +24,7 @@
 #include <ranges>
 
 #include "controller_common/common.hpp"
+#include "message_transforms/transforms.hpp"
 
 namespace topic_sensors
 {
@@ -49,8 +50,17 @@ auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/
   }
   RCLCPP_INFO(logger_, std::format("Subscribing to topic: {}", topic).c_str());  // NOLINT
 
+  const bool transform_message = info_.hardware_parameters.at("transform_message") == "true";
+  if (transform_message) {
+    // NOLINTNEXTLINE
+    RCLCPP_INFO(logger_, "Incoming messages will be transform from the ROS mobile standard to the maritime standard");
+  }
+
   state_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
-    topic, rclcpp::SensorDataQoS(), [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {  // NOLINT
+    topic, rclcpp::SensorDataQoS(), [this, &transform_message](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
+      if (transform_message) {
+        m2m::transforms::transform_message(*msg, "map_ned", "base_link_fsd");
+      }
       state_.writeFromNonRT(*msg);
     });
   return hardware_interface::CallbackReturn::SUCCESS;