From 0bcf68bcb635199adcd134e8932932054e863c0d Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sun, 26 Mar 2023 22:12:38 -0700 Subject: [PATCH] frame_transforms.h: add function to directly convert between ROS and px4 (#192) frame conventions. Fix warning in src/examples/advertisers/debug_vect_advertiser.cpp Signed-off-by: Beniamino Pozzan --- include/px4_ros_com/frame_transforms.h | 28 +++++++++++++++++++ .../advertisers/debug_vect_advertiser.cpp | 2 +- 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/include/px4_ros_com/frame_transforms.h b/include/px4_ros_com/frame_transforms.h index c9c433db..14ad7bee 100644 --- a/include/px4_ros_com/frame_transforms.h +++ b/include/px4_ros_com/frame_transforms.h @@ -304,6 +304,34 @@ template inline T baselink_to_aircraft_orientation(const T &in) return transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT); } +/** + * @brief Transform from orientation represented in PX4 format to ROS format + * PX4 format is aircraft to NED + * ROS format is baselink to ENU + * + * Two steps conversion: + * 1. aircraft to NED is converted to aircraft to ENU (NED_to_ENU conversion) + * 2. aircraft to ENU is converted to baselink to ENU (baselink_to_aircraft conversion) + */ +template inline T px4_to_ros_orientation(const T &in) +{ + return baselink_to_aircraft_orientation(ned_to_enu_orientation(in)); +} + +/** + * @brief Transform from orientation represented in ROS format to PX4 format + * PX4 format is aircraft to NED + * ROS format is baselink to ENU + * + * Two steps conversion: + * 1. baselink to ENU is converted to baselink to NED (ENU_to_NED conversion) + * 2. baselink to NED is converted to aircraft to NED (aircraft_to_baselink conversion) + */ +template inline T ros_to_px4_orientation(const T &in) +{ + return aircraft_to_baselink_orientation(enu_to_ned_orientation(in)); +} + /** * @brief Transform data expressed in NED to ENU local frame. */ diff --git a/src/examples/advertisers/debug_vect_advertiser.cpp b/src/examples/advertisers/debug_vect_advertiser.cpp index f1bb86f2..dc8f3afb 100644 --- a/src/examples/advertisers/debug_vect_advertiser.cpp +++ b/src/examples/advertisers/debug_vect_advertiser.cpp @@ -59,7 +59,7 @@ class DebugVectAdvertiser : public rclcpp::Node debug_vect.x = 1.0; debug_vect.y = 2.0; debug_vect.z = 3.0; - RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %llu x: %f y: %f z: %f \033[0m", + RCLCPP_INFO(this->get_logger(), "\033[97m Publishing debug_vect: time: %lu x: %f y: %f z: %f \033[0m", debug_vect.timestamp, debug_vect.x, debug_vect.y, debug_vect.z); this->publisher_->publish(debug_vect);