diff --git a/vortex_utility_nodes/CMakeLists.txt b/vortex_utility_nodes/CMakeLists.txt new file mode 100644 index 0000000..6720a17 --- /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(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() 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/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 new file mode 100644 index 0000000..9343657 --- /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 + Opensource or something i dont know + + ament_cmake + + eigen + yaml-cpp + rclcpp + nav_msgs + vortex_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..73f6ce0 --- /dev/null +++ b/vortex_utility_nodes/src/euler_odometry_publisher_node.cpp @@ -0,0 +1,61 @@ +#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; +}