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(euler_odometry_publisher_node
+  src/euler_odometry_publisher_node.cpp
+)
+
+ament_target_dependencies(euler_odometry_publisher_node PUBLIC
+  rclcpp
+  nav_msgs
+  Eigen3
+  vortex_utils
+  vortex_msgs
+)
+
+install(TARGETS euler_odometry_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/euler_odometry_publisher.yaml

@@ -0,0 +1,5 @@
+euler_odometry_publisher_node:
+  ros__parameters:
+    topics:
+      input_odom: "nautilus/odom"
+      output_euler: "utils/odometry/euler"

vortex_utility_nodes/launch/euler_odometry_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',
+        'euler_odometry_publisher.yaml'
+    )
+
+    return LaunchDescription([
+        Node(
+            package='vortex_utility_nodes',
+            executable='euler_odometry_publisher_node',
+            name='euler_odometry_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>Opensource or something i dont know</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/euler_odometry_publisher_node.cpp

@@ -0,0 +1,61 @@
+#include <vortex/utils/math.hpp>
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "vortex_msgs/msg/pose_euler_stamped.hpp"
+
+class EulerOdometryPublisherNode : public rclcpp::Node {
+   public:
+    EulerOdometryPublisherNode() : Node("euler_odometry_publisher_node") {
+        this->declare_parameter<std::string>("topics.input_odom",
+                                             "nautilus/odom");
+        this->declare_parameter<std::string>("topics.output_euler",
+                                             "utils/odometry/euler");
+
+        auto input_topic = this->get_parameter("topics.input_odom").as_string();
+        auto output_topic =
+            this->get_parameter("topics.output_euler").as_string();
+
+        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+            input_topic, 2,
+            std::bind(&EulerOdometryPublisherNode::odom_callback, this,
+                      std::placeholders::_1));
+
+        euler_odom_pub_ =
+            this->create_publisher<vortex_msgs::msg::PoseEulerStamped>(
+                output_topic, 2);
+
+        RCLCPP_INFO(this->get_logger(),
+                    "Euler odometry publisher started: %s -> %s",
+                    input_topic.c_str(), output_topic.c_str());
+    }
+
+   private:
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+        vortex_msgs::msg::PoseEulerStamped euler_msg;
+
+        const auto& q = msg->pose.pose.orientation;
+        Eigen::Quaterniond eigen_q(q.w, q.x, q.y, q.z);
+        auto euler = vortex::utils::math::quat_to_euler(eigen_q);
+
+        euler_msg.x = msg->pose.pose.position.x;
+        euler_msg.y = msg->pose.pose.position.y;
+        euler_msg.z = msg->pose.pose.position.z;
+        euler_msg.roll = euler.x();
+        euler_msg.pitch = euler.y();
+        euler_msg.yaw = euler.z();
+
+        euler_odom_pub_->publish(euler_msg);
+    }
+
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+    rclcpp::Publisher<vortex_msgs::msg::PoseEulerStamped>::SharedPtr
+        euler_odom_pub_;
+};
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<EulerOdometryPublisherNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}