update message publisher with imu

Author: jorgenfj

vortex_utility_nodes/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(nav_msgs REQUIRED)
 find_package(vortex_msgs REQUIRED)
 find_package(vortex_utils REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 add_executable(message_publisher_node
   src/message_publisher.cpp
@@ -22,6 +23,8 @@ ament_target_dependencies(message_publisher_node PUBLIC
   Eigen3
   vortex_utils
   vortex_msgs
+  sensor_msgs
+
 )
 
 install(TARGETS message_publisher_node

vortex_utility_nodes/config/message_publisher.yaml

@@ -1,11 +1,12 @@
 /**:
   ros__parameters:
-    input_type: "reference_filter"
+    input_type: "odometry"
     output_namespace: ""
     topics:
       odometry: "/nautilus/odom"
       waypoint: "/nautilus/waypoint"
       reference_filter: "/nautilus/guidance/dp_quat"
       pose_stamped: "/nautilus/pose_stamped"
       pose_with_covariance_stamped: "/nautilus/pose"
-      output: "utils/message_publisher"
+      imu: "mru/imu"
+      output: "utils/message_publisher/odom_tf_rpy"

vortex_utility_nodes/launch/message_publisher.launch.py

@@ -14,13 +14,6 @@ def generate_launch_description():
         'message_publisher.yaml',
     )
 
-    input_type_arg = DeclareLaunchArgument(
-        'input_type',
-        default_value='odometry',
-        description='Input message type: odometry, waypoint, reference_filter, '
-        'pose_stamped, or pose_with_covariance_stamped',
-    )
-
     namespace_arg = DeclareLaunchArgument(
         'namespace',
         default_value='',
@@ -29,17 +22,13 @@ def generate_launch_description():
 
     return LaunchDescription(
         [
-            input_type_arg,
             namespace_arg,
             Node(
                 package='vortex_utility_nodes',
                 executable='message_publisher_node',
                 name='message_publisher_node',
                 namespace=LaunchConfiguration('namespace'),
-                parameters=[
-                    config,
-                    {'input_type': LaunchConfiguration('input_type')},
-                ],
+                parameters=[config],
                 output='screen',
             ),
         ]

vortex_utility_nodes/package.xml

@@ -16,6 +16,7 @@
   <depend>nav_msgs</depend>
   <depend>vortex_msgs</depend>
   <depend>vortex_utils</depend>
+  <depend>sensor_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

vortex_utility_nodes/src/message_publisher.cpp

@@ -1,3 +1,4 @@
+#include <sensor_msgs/msg/imu.hpp>
 #include <vortex/utils/math.hpp>
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
@@ -19,6 +20,7 @@ class MessagePublisherNode : public rclcpp::Node {
         this->declare_parameter<std::string>("topics.pose_stamped");
         this->declare_parameter<std::string>(
             "topics.pose_with_covariance_stamped");
+        this->declare_parameter<std::string>("topics.imu");
         this->declare_parameter<std::string>("topics.output");
 
         auto input_type = this->get_parameter("input_type").as_string();
@@ -86,6 +88,14 @@ class MessagePublisherNode : public rclcpp::Node {
                 this->get_logger(),
                 "Euler publisher [pose_with_covariance_stamped]: %s -> %s",
                 topic.c_str(), output_topic.c_str());
+        } else if (input_type == "imu") {
+            auto topic = this->get_parameter("topics.imu").as_string();
+            sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+                topic, qos_profile,
+                std::bind(&MessagePublisherNode::imu_callback, this,
+                          std::placeholders::_1));
+            RCLCPP_INFO(this->get_logger(), "Euler publisher [imu]: %s -> %s",
+                        topic.c_str(), output_topic.c_str());
 
         } else {
             RCLCPP_FATAL(
@@ -149,13 +159,20 @@ class MessagePublisherNode : public rclcpp::Node {
         publish_rpy(msg->header, euler);
     }
 
+    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+        const auto& q = msg->orientation;
+        auto euler = convert_quat(q.w, q.x, q.y, q.z);
+        publish_rpy(msg->header, euler);
+    }
+
     using SubVariant = std::variant<
         rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr,
         rclcpp::Subscription<vortex_msgs::msg::Waypoint>::SharedPtr,
         rclcpp::Subscription<vortex_msgs::msg::ReferenceFilterQuat>::SharedPtr,
         rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr,
         rclcpp::Subscription<
-            geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr>;
+            geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr,
+        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr>;
 
     SubVariant sub_;
     rclcpp::Publisher<vortex_msgs::msg::RPY>::SharedPtr rpy_pub_;