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 <beniamino.pozzan@phd.unipd.it>

include/px4_ros_com/frame_transforms.h

@@ -304,6 +304,34 @@ template <class T> 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 <class T> 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 <class T> 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.
  */

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);