From 920d42c2b39dfd3fcaf5218cb6402d25440e3507 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Fri, 27 Mar 2026 20:13:15 +0100 Subject: [PATCH 1/6] 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/6] 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/6] 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/6] 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` From 73370f50c03b7dbf47d9b14cdf867a36ea4194f2 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sat, 28 Mar 2026 16:31:23 +0100 Subject: [PATCH 5/6] made the node more general purpose --- vortex_utility_nodes/CMakeLists.txt | 8 +- .../config/euler_odometry_publisher.yaml | 5 - .../config/message_publisher.yaml | 8 ++ ....launch.py => message_publisher.launch.py} | 6 +- vortex_utility_nodes/package.xml | 2 +- .../src/euler_odometry_publisher_node.cpp | 61 --------- .../src/message_publisher.cpp | 121 ++++++++++++++++++ 7 files changed, 137 insertions(+), 74 deletions(-) delete mode 100644 vortex_utility_nodes/config/euler_odometry_publisher.yaml create mode 100644 vortex_utility_nodes/config/message_publisher.yaml rename vortex_utility_nodes/launch/{euler_odometry_publisher.launch.py => message_publisher.launch.py} (76%) delete mode 100644 vortex_utility_nodes/src/euler_odometry_publisher_node.cpp create mode 100644 vortex_utility_nodes/src/message_publisher.cpp diff --git a/vortex_utility_nodes/CMakeLists.txt b/vortex_utility_nodes/CMakeLists.txt index 6720a17..bca3ea5 100644 --- a/vortex_utility_nodes/CMakeLists.txt +++ b/vortex_utility_nodes/CMakeLists.txt @@ -10,11 +10,11 @@ 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 +add_executable(message_publisher_node + src/message_publisher.cpp ) -ament_target_dependencies(euler_odometry_publisher_node PUBLIC +ament_target_dependencies(message_publisher_node PUBLIC rclcpp nav_msgs Eigen3 @@ -22,7 +22,7 @@ ament_target_dependencies(euler_odometry_publisher_node PUBLIC vortex_msgs ) -install(TARGETS euler_odometry_publisher_node +install(TARGETS message_publisher_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/vortex_utility_nodes/config/euler_odometry_publisher.yaml b/vortex_utility_nodes/config/euler_odometry_publisher.yaml deleted file mode 100644 index 8112350..0000000 --- a/vortex_utility_nodes/config/euler_odometry_publisher.yaml +++ /dev/null @@ -1,5 +0,0 @@ -euler_odometry_publisher_node: - ros__parameters: - topics: - input_odom: "nautilus/odom" - output_euler: "utils/odometry/euler" diff --git a/vortex_utility_nodes/config/message_publisher.yaml b/vortex_utility_nodes/config/message_publisher.yaml new file mode 100644 index 0000000..9af88b9 --- /dev/null +++ b/vortex_utility_nodes/config/message_publisher.yaml @@ -0,0 +1,8 @@ +euler_odometry_publisher_node: + ros__parameters: + input_type: "odometry" + topics: + odom: "nautilus/odom" + waypoint: "fsm/waypoint" + reference_filter: "guidance/reference_filter" + output: "utils/message_publisher/" diff --git a/vortex_utility_nodes/launch/euler_odometry_publisher.launch.py b/vortex_utility_nodes/launch/message_publisher.launch.py similarity index 76% rename from vortex_utility_nodes/launch/euler_odometry_publisher.launch.py rename to vortex_utility_nodes/launch/message_publisher.launch.py index 7b5eb08..f6af51f 100644 --- a/vortex_utility_nodes/launch/euler_odometry_publisher.launch.py +++ b/vortex_utility_nodes/launch/message_publisher.launch.py @@ -7,14 +7,14 @@ def generate_launch_description(): config = os.path.join( get_package_share_directory('vortex_utility_nodes'), 'config', - 'euler_odometry_publisher.yaml' + 'message_publisher.yaml' ) return LaunchDescription([ Node( package='vortex_utility_nodes', - executable='euler_odometry_publisher_node', - name='euler_odometry_publisher_node', + executable='message_publisher_node', + name='message_publisher_node', parameters=[config], output='screen', ), diff --git a/vortex_utility_nodes/package.xml b/vortex_utility_nodes/package.xml index 9343657..7da45f1 100644 --- a/vortex_utility_nodes/package.xml +++ b/vortex_utility_nodes/package.xml @@ -5,7 +5,7 @@ 0.0.0 Useful nodes for debugging and testing Cyprian Osinski - Opensource or something i dont know + MIT ament_cmake diff --git a/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp deleted file mode 100644 index 73f6ce0..0000000 --- a/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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( - input_topic, 2, - std::bind(&EulerOdometryPublisherNode::odom_callback, this, - std::placeholders::_1)); - - euler_odom_pub_ = - this->create_publisher( - 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::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; -} diff --git a/vortex_utility_nodes/src/message_publisher.cpp b/vortex_utility_nodes/src/message_publisher.cpp new file mode 100644 index 0000000..9f8a371 --- /dev/null +++ b/vortex_utility_nodes/src/message_publisher.cpp @@ -0,0 +1,121 @@ +#include +#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 + +class MessagePublisherNode : public rclcpp::Node { + public: + MessagePublisherNode() : Node("message_publisher_node") { + this->declare_parameter("input_type", "odometry"); + this->declare_parameter("topics.odom", "nautilus/odom"); + this->declare_parameter("topics.waypoint", + "nautilus/waypoint"); + this->declare_parameter("topics.reference_filter", + "nautilus/reference_filter"); + this->declare_parameter("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(output_topic, 2); + + if (input_type == "odometry") { + auto topic = this->get_parameter("topics.odom").as_string(); + sub_ = this->create_subscription( + 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( + 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::SharedPtr, + rclcpp::Subscription::SharedPtr, + rclcpp::Subscription::SharedPtr>; + + SubVariant sub_; + rclcpp::Publisher::SharedPtr rpy_pub_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From f1190d8d7725ec5532655929b8972bf9005946d0 Mon Sep 17 00:00:00 2001 From: Cyprian Osinski Date: Sat, 28 Mar 2026 17:18:42 +0100 Subject: [PATCH 6/6] Cleaned up leftover old node name after refactor --- vortex_utility_nodes/config/message_publisher.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vortex_utility_nodes/config/message_publisher.yaml b/vortex_utility_nodes/config/message_publisher.yaml index 9af88b9..0459a59 100644 --- a/vortex_utility_nodes/config/message_publisher.yaml +++ b/vortex_utility_nodes/config/message_publisher.yaml @@ -1,4 +1,4 @@ -euler_odometry_publisher_node: +message_publisher_node: ros__parameters: input_type: "odometry" topics: