Vortex utility nodes

vortex_utility_nodes/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(vortex_utility_nodes)
+set(CMAKE_CXX_STANDARD 20)
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_executable(message_publisher_node
+  src/message_publisher.cpp
+)
+
+ament_target_dependencies(message_publisher_node PUBLIC
+  rclcpp
+  nav_msgs
+  Eigen3
+  vortex_utils
+  vortex_msgs
+)
+
+install(TARGETS message_publisher_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()

vortex_utility_nodes/README.md

@@ -0,0 +1,36 @@
+# Vortex Utility Nodes
+
+A collection of lightweight ROS2 utility nodes for testing, debugging, and operating the Vortex AUV.
+These are standalone helper nodes not core pipeline components. Use them to inspect data, convert formats, or bridge gaps during development and field operations.
+
+## Nodes
+
+| Node | Description |
+|------|-------------|
+| `euler_odometry_publisher_node` | Subscribes to `odometry` (resolved from config) and publishes odometry pose in Euler angles (roll, pitch, yaw). |
+
+## Build
+```bash
+colcon build --packages-select vortex_utility_nodes
+source install/setup.bash
+```
+
+## Run
+```bash
+ros2 launch vortex_utility_nodes euler_odometry_publisher_node.launch.py
+```
+
+## Adding New Nodes
+
+1. Create your node source file in `src/`
+2. Add an `add_executable` entry in `CMakeLists.txt`
+3. Add the install target to `lib/${PROJECT_NAME}`
+4. Update this table
+
+## Current Dependencies
+
+- `rclcpp`
+- `nav_msgs`
+- `vortex_msgs`
+- `vortex_utils`
+- `Eigen3`

vortex_utility_nodes/config/message_publisher.yaml

@@ -0,0 +1,8 @@
+message_publisher_node:
+  ros__parameters:
+    input_type: "odometry"
+    topics:
+      odom: "nautilus/odom"
+      waypoint: "fsm/waypoint"
+      reference_filter: "guidance/reference_filter"
+      output: "utils/message_publisher/"

vortex_utility_nodes/launch/message_publisher.launch.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('vortex_utility_nodes'),
+        'config',
+        'message_publisher.yaml'
+    )
+
+    return LaunchDescription([
+        Node(
+            package='vortex_utility_nodes',
+            executable='message_publisher_node',
+            name='message_publisher_node',
+            parameters=[config],
+            output='screen',
+        ),
+    ])

vortex_utility_nodes/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vortex_utility_nodes</name>
+  <version>0.0.0</version>
+  <description>Useful nodes for debugging and testing</description>
+  <maintainer email="cyprian.github@gmail.com"> Cyprian Osinski</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>eigen</depend>
+  <depend>yaml-cpp</depend>
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>vortex_utils</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+

vortex_utility_nodes/src/message_publisher.cpp

@@ -0,0 +1,121 @@
+#include <vortex/utils/math.hpp>
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "vortex_msgs/msg/reference_filter_quat.hpp"
+#include "vortex_msgs/msg/rpy.hpp"
+#include "vortex_msgs/msg/waypoint.hpp"
+
+#include <variant>
+
+class MessagePublisherNode : public rclcpp::Node {
+   public:
+    MessagePublisherNode() : Node("message_publisher_node") {
+        this->declare_parameter<std::string>("input_type", "odometry");
+        this->declare_parameter<std::string>("topics.odom", "nautilus/odom");
+        this->declare_parameter<std::string>("topics.waypoint",
+                                             "nautilus/waypoint");
+        this->declare_parameter<std::string>("topics.reference_filter",
+                                             "nautilus/reference_filter");
+        this->declare_parameter<std::string>("topics.output",
+                                             "utils/odometry/euler");
+
+        auto input_type = this->get_parameter("input_type").as_string();
+        auto output_topic = this->get_parameter("topics.output").as_string();
+
+        rpy_pub_ =
+            this->create_publisher<vortex_msgs::msg::RPY>(output_topic, 2);
+
+        if (input_type == "odometry") {
+            auto topic = this->get_parameter("topics.odom").as_string();
+            sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+                topic, 2,
+                std::bind(&MessagePublisherNode::odom_callback, this,
+                          std::placeholders::_1));
+            RCLCPP_INFO(this->get_logger(),
+                        "Euler publisher [odometry]: %s -> %s", topic.c_str(),
+                        output_topic.c_str());
+
+        } else if (input_type == "waypoint") {
+            auto topic = this->get_parameter("topics.waypoint").as_string();
+            sub_ = this->create_subscription<vortex_msgs::msg::Waypoint>(
+                topic, 2,
+                std::bind(&MessagePublisherNode::waypoint_callback, this,
+                          std::placeholders::_1));
+            RCLCPP_INFO(this->get_logger(),
+                        "Euler publisher [waypoint]: %s -> %s", topic.c_str(),
+                        output_topic.c_str());
+
+        } else if (input_type == "reference_filter") {
+            auto topic =
+                this->get_parameter("topics.reference_filter").as_string();
+            sub_ = this->create_subscription<
+                vortex_msgs::msg::ReferenceFilterQuat>(
+                topic, 2,
+                std::bind(&MessagePublisherNode::ref_filter_callback, this,
+                          std::placeholders::_1));
+            RCLCPP_INFO(this->get_logger(),
+                        "Euler publisher [reference_filter]: %s -> %s",
+                        topic.c_str(), output_topic.c_str());
+
+        } else {
+            RCLCPP_FATAL(this->get_logger(),
+                         "Unknown input_type: '%s'. Must be 'odometry', "
+                         "'waypoint', or 'reference_filter'.",
+                         input_type.c_str());
+            rclcpp::shutdown();
+            return;
+        }
+    }
+
+   private:
+    Eigen::Vector3d convert_quat(double w, double x, double y, double z) {
+        Eigen::Quaterniond eigen_q(w, x, y, z);
+        return vortex::utils::math::quat_to_euler(eigen_q);
+    }
+
+    void publish_rpy(const std_msgs::msg::Header& header,
+                     const Eigen::Vector3d& euler) {
+        vortex_msgs::msg::RPY msg;
+        msg.header = header;
+        msg.roll = euler.x();
+        msg.pitch = euler.y();
+        msg.yaw = euler.z();
+        rpy_pub_->publish(msg);
+    }
+
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+        const auto& q = msg->pose.pose.orientation;
+        auto euler = convert_quat(q.w, q.x, q.y, q.z);
+        publish_rpy(msg->header, euler);
+    }
+
+    void waypoint_callback(const vortex_msgs::msg::Waypoint::SharedPtr msg) {
+        const auto& q = msg->pose.orientation;
+        auto euler = convert_quat(q.w, q.x, q.y, q.z);
+        std_msgs::msg::Header header;
+        header.stamp = this->now();
+        publish_rpy(header, euler);
+    }
+
+    void ref_filter_callback(
+        const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) {
+        auto euler = convert_quat(msg->qw, msg->qx, msg->qy, msg->qz);
+        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>;
+
+    SubVariant sub_;
+    rclcpp::Publisher<vortex_msgs::msg::RPY>::SharedPtr rpy_pub_;
+};
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MessagePublisherNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}