diff --git a/vortex_utility_nodes/CMakeLists.txt b/vortex_utility_nodes/CMakeLists.txt new file mode 100644 index 0000000..bca3ea5 --- /dev/null +++ b/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() diff --git a/vortex_utility_nodes/README.md b/vortex_utility_nodes/README.md new file mode 100644 index 0000000..3700267 --- /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` (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` diff --git a/vortex_utility_nodes/config/message_publisher.yaml b/vortex_utility_nodes/config/message_publisher.yaml new file mode 100644 index 0000000..0459a59 --- /dev/null +++ b/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/" diff --git a/vortex_utility_nodes/launch/message_publisher.launch.py b/vortex_utility_nodes/launch/message_publisher.launch.py new file mode 100644 index 0000000..f6af51f --- /dev/null +++ b/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', + ), + ]) diff --git a/vortex_utility_nodes/package.xml b/vortex_utility_nodes/package.xml new file mode 100644 index 0000000..7da45f1 --- /dev/null +++ b/vortex_utility_nodes/package.xml @@ -0,0 +1,23 @@ + + + + vortex_utility_nodes + 0.0.0 + Useful nodes for debugging and testing + Cyprian Osinski + MIT + + ament_cmake + + eigen + yaml-cpp + rclcpp + nav_msgs + vortex_msgs + vortex_utils + + + ament_cmake + + + 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; +}