From 920d42c2b39dfd3fcaf5218cb6402d25440e3507 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 20:13:15 +0100 Subject: [PATCH 1/4] added vortex_utility_nodes package, that should hold nodes useful for debugging or for missions --- vortex_utility_nodes/CMakeLists.txt | 30 ++++++++++++ vortex_utility_nodes/package.xml | 22 +++++++++ .../src/euler_odometry_publisher_node.cpp | 49 +++++++++++++++++++ 3 files changed, 101 insertions(+) create mode 100644 vortex_utility_nodes/CMakeLists.txt create mode 100644 vortex_utility_nodes/package.xml create mode 100644 vortex_utility_nodes/src/euler_odometry_publisher_node.cpp diff --git a/vortex_utility_nodes/CMakeLists.txt b/vortex_utility_nodes/CMakeLists.txt new file mode 100644 index 0000000..06d5183 --- /dev/null +++ b/vortex_utility_nodes/CMakeLists.txt @@ -0,0 +1,30 @@ +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_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 +) + +install(TARGETS euler_odometry_publisher_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +ament_package() diff --git a/vortex_utility_nodes/package.xml b/vortex_utility_nodes/package.xml new file mode 100644 index 0000000..002d463 --- /dev/null +++ b/vortex_utility_nodes/package.xml @@ -0,0 +1,22 @@ + + + + vortex_utility_nodes + 0.0.0 + Useful nodes for debugging and testing + Cyprian Osinski + Opensource or something i dont know + + ament_cmake + + eigen + yaml-cpp + rclcpp + nav_msgs + vortex_utils + + + ament_cmake + + + diff --git a/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp new file mode 100644 index 0000000..aec15b0 --- /dev/null +++ b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp @@ -0,0 +1,49 @@ +#include +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" + +class EulerOdometryPublisherNode : public rclcpp::Node { + public: + EulerOdometryPublisherNode() : Node("euler_odometry_publisher_node") { + odom_sub_ = this->create_subscription( + "nautilus/odometry", 2, + std::bind(&EulerOdometryPublisherNode::odom_callback, this, + std::placeholders::_1)); + + euler_odom_pub_ = this->create_publisher( + "utils/odometry/euler", 2); + + RCLCPP_INFO(this->get_logger(), + "Euler odometry publisher node started."); + } + + private: + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + auto euler_msg = *msg; + + const auto& q = msg->pose.pose.orientation; + + // Convert geometry_msgs quaternion to Eigen quaternion + Eigen::Quaterniond eigen_q(q.w, q.x, q.y, q.z); + auto euler = vortex::utils::math::quat_to_euler(eigen_q); + + // TODO: add custom message for naming + euler_msg.pose.pose.orientation.x = euler.x(); + euler_msg.pose.pose.orientation.y = euler.y(); + euler_msg.pose.pose.orientation.z = euler.z(); + euler_msg.pose.pose.orientation.w = 0.0; + + euler_odom_pub_->publish(euler_msg); + } + + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr euler_odom_pub_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 93d474bab5379ab4e65a8b4f1484a63a1d99e359 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 20:15:30 +0100 Subject: [PATCH 2/4] added readme for the euler_odom_publisher_node --- vortex_utility_nodes/README.md | 36 ++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 vortex_utility_nodes/README.md diff --git a/vortex_utility_nodes/README.md b/vortex_utility_nodes/README.md new file mode 100644 index 0000000..6f0eb15 --- /dev/null +++ b/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` (`nav_msgs/Odometry`) and republishes on `odometry/euler` with orientation converted from quaternions to Euler angles (roll, pitch, yaw). | + +## Build +```bash +colcon build --packages-select vortex_utility_nodes +source install/setup.bash +``` + +## Run +```bash +ros2 run vortex_utility_nodes euler_odometry_publisher_node +``` + +## 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_utils` +- `Eigen3` From 6368873ebcb8ed13f1467c4a218d776dc6802cd4 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 21:00:02 +0100 Subject: [PATCH 3/4] fleshed out the implementation with launch file, config file and using a suitable message for publishing --- vortex_utility_nodes/CMakeLists.txt | 12 ++++++ .../config/euler_odometry_publisher.yaml | 5 +++ .../launch/euler_odometry_publisher.launch.py | 21 ++++++++++ vortex_utility_nodes/package.xml | 1 + .../src/euler_odometry_publisher_node.cpp | 38 ++++++++++++------- 5 files changed, 64 insertions(+), 13 deletions(-) create mode 100644 vortex_utility_nodes/config/euler_odometry_publisher.yaml create mode 100644 vortex_utility_nodes/launch/euler_odometry_publisher.launch.py diff --git a/vortex_utility_nodes/CMakeLists.txt b/vortex_utility_nodes/CMakeLists.txt index 06d5183..6720a17 100644 --- a/vortex_utility_nodes/CMakeLists.txt +++ b/vortex_utility_nodes/CMakeLists.txt @@ -6,6 +6,7 @@ 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) @@ -18,12 +19,23 @@ ament_target_dependencies(euler_odometry_publisher_node PUBLIC 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}) diff --git a/vortex_utility_nodes/config/euler_odometry_publisher.yaml b/vortex_utility_nodes/config/euler_odometry_publisher.yaml new file mode 100644 index 0000000..8112350 --- /dev/null +++ b/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" diff --git a/vortex_utility_nodes/launch/euler_odometry_publisher.launch.py b/vortex_utility_nodes/launch/euler_odometry_publisher.launch.py new file mode 100644 index 0000000..7b5eb08 --- /dev/null +++ b/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', + ), + ]) diff --git a/vortex_utility_nodes/package.xml b/vortex_utility_nodes/package.xml index 002d463..9343657 100644 --- a/vortex_utility_nodes/package.xml +++ b/vortex_utility_nodes/package.xml @@ -13,6 +13,7 @@ yaml-cpp rclcpp nav_msgs + vortex_msgs vortex_utils diff --git a/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp index aec15b0..73f6ce0 100644 --- a/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp +++ b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp @@ -1,43 +1,55 @@ #include #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("topics.input_odom", + "nautilus/odom"); + this->declare_parameter("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( - "nautilus/odometry", 2, + input_topic, 2, std::bind(&EulerOdometryPublisherNode::odom_callback, this, std::placeholders::_1)); - euler_odom_pub_ = this->create_publisher( - "utils/odometry/euler", 2); + euler_odom_pub_ = + this->create_publisher( + output_topic, 2); RCLCPP_INFO(this->get_logger(), - "Euler odometry publisher node started."); + "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) { - auto euler_msg = *msg; + vortex_msgs::msg::PoseEulerStamped euler_msg; const auto& q = msg->pose.pose.orientation; - - // Convert geometry_msgs quaternion to Eigen quaternion Eigen::Quaterniond eigen_q(q.w, q.x, q.y, q.z); auto euler = vortex::utils::math::quat_to_euler(eigen_q); - // TODO: add custom message for naming - euler_msg.pose.pose.orientation.x = euler.x(); - euler_msg.pose.pose.orientation.y = euler.y(); - euler_msg.pose.pose.orientation.z = euler.z(); - euler_msg.pose.pose.orientation.w = 0.0; + 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::SharedPtr odom_sub_; - rclcpp::Publisher::SharedPtr euler_odom_pub_; + rclcpp::Publisher::SharedPtr + euler_odom_pub_; }; int main(int argc, char** argv) { From 0fb63b7ae7294e90610b2b388a385c496389b389 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 21:18:44 +0100 Subject: [PATCH 4/4] updated readme to so it has correct launch command --- vortex_utility_nodes/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/vortex_utility_nodes/README.md b/vortex_utility_nodes/README.md index 6f0eb15..3700267 100644 --- a/vortex_utility_nodes/README.md +++ b/vortex_utility_nodes/README.md @@ -1,14 +1,13 @@ # 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. +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` (`nav_msgs/Odometry`) and republishes on `odometry/euler` with orientation converted from quaternions to Euler angles (roll, pitch, yaw). | +| `euler_odometry_publisher_node` | Subscribes to `odometry` (resolved from config) and publishes odometry pose in Euler angles (roll, pitch, yaw). | ## Build ```bash @@ -18,7 +17,7 @@ source install/setup.bash ## Run ```bash -ros2 run vortex_utility_nodes euler_odometry_publisher_node +ros2 launch vortex_utility_nodes euler_odometry_publisher_node.launch.py ``` ## Adding New Nodes @@ -32,5 +31,6 @@ ros2 run vortex_utility_nodes euler_odometry_publisher_node - `rclcpp` - `nav_msgs` +- `vortex_msgs` - `vortex_utils` - `Eigen3`