diff --git a/vortex_utility_nodes/README.md b/vortex_utility_nodes/README.md index 3700267..c5c8252 100644 --- a/vortex_utility_nodes/README.md +++ b/vortex_utility_nodes/README.md @@ -7,7 +7,7 @@ These are standalone helper nodes not core pipeline components. Use them to insp | Node | Description | |------|-------------| -| `euler_odometry_publisher_node` | Subscribes to `odometry` (resolved from config) and publishes odometry pose in Euler angles (roll, pitch, yaw). | +| `message_publisher` | Subscribes to `odometry` (resolved from config) and publishes odometry pose in Euler angles (x, y, z, roll, pitch, yaw). | ## Build ```bash @@ -17,7 +17,7 @@ source install/setup.bash ## Run ```bash -ros2 launch vortex_utility_nodes euler_odometry_publisher_node.launch.py +ros2 launch vortex_utility_nodes message_publisher.launch.py ``` ## Adding New Nodes diff --git a/vortex_utility_nodes/src/message_publisher.cpp b/vortex_utility_nodes/src/message_publisher.cpp index 23b7f95..fb30613 100644 --- a/vortex_utility_nodes/src/message_publisher.cpp +++ b/vortex_utility_nodes/src/message_publisher.cpp @@ -169,7 +169,7 @@ class MessagePublisherNode : public rclcpp::Node { void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { const auto& q = msg->orientation; auto euler = convert_quat(q.w, q.x, q.y, q.z); - publish_rpy(msg->header, euler); + publish_pose(msg->header, 0.0, 0.0, 0.0, euler); } using SubVariant = std::variant<