odometry: add OdometryAttitude class

px4_ros2_cpp/CMakeLists.txt

@@ -41,6 +41,7 @@ set(HEADER_FILES
         include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp
         include/px4_ros2/navigation/experimental/local_position_measurement_interface.hpp
         include/px4_ros2/navigation/experimental/navigation_interface_base.hpp
+        include/px4_ros2/odometry/attitude.hpp
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
         include/px4_ros2/utils/frame_conversion.hpp
@@ -66,6 +67,7 @@ add_library(px4_ros2_cpp
         src/control/setpoint_types/experimental/trajectory.cpp
         src/navigation/experimental/global_position_measurement_interface.cpp
         src/navigation/experimental/local_position_measurement_interface.cpp
+        src/odometry/attitude.cpp
         src/odometry/global_position.cpp
         src/odometry/local_position.cpp
         src/utils/geodesic.cpp

px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ * Copyright (c) 2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <Eigen/Eigen>
+#include <px4_msgs/msg/vehicle_attitude.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/odometry/subscription.hpp>
+#include <px4_ros2/utils/geometry.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup odometry
+ *  @{
+ */
+
+/**
+ * Provides access to the vehicle's attitude estimate
+ */
+class OdometryAttitude : public Subscription<px4_msgs::msg::VehicleAttitude>
+{
+public:
+  explicit OdometryAttitude(Context & context);
+
+  /**
+   * @brief Get the vehicle's attitude.
+   *
+   * @return the attitude quaternion
+   */
+  Eigen::Quaternionf attitude() const
+  {
+    const px4_msgs::msg::VehicleAttitude & att = last();
+    return Eigen::Quaternionf{att.q[0], att.q[1], att.q[2], att.q[3]};
+  }
+
+  /**
+   * @brief Get the vehicle's roll in extrinsic RPY order.
+   *
+   * @return the attitude roll in radians within [-pi, pi]
+   */
+  float roll() const
+  {
+    const px4_msgs::msg::VehicleAttitude & att = last();
+    const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]};
+    return quaternionToRoll(q);
+  }
+
+  /**
+   * @brief Get the vehicle's pitch in extrinsic RPY order.
+   *
+   * @return the attitude pitch in radians within [-pi, pi]
+   */
+  float pitch() const
+  {
+    const px4_msgs::msg::VehicleAttitude & att = last();
+    const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]};
+    return quaternionToPitch(q);
+  }
+
+  /**
+   * @brief Get the vehicle's yaw in extrinsic RPY order.
+   *
+   * @return the attitude yaw in radians within [-pi, pi]
+   */
+  float yaw() const
+  {
+    const px4_msgs::msg::VehicleAttitude & att = last();
+    const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]};
+    return quaternionToYaw(q);
+  }
+};
+
+/** @}*/
+} /* namespace px4_ros2 */

px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp

@@ -60,6 +60,17 @@ class OdometryLocalPosition : public Subscription<px4_msgs::msg::VehicleLocalPos
     const px4_msgs::msg::VehicleLocalPosition & pos = last();
     return {pos.ax, pos.ay, pos.az};
   }
+
+  /**
+   * @brief Get the vehicle's heading relative to NED earth-fixed frame.
+   *
+   * @return The vehicle's yaw in radians within [-pi, pi] using extrinsic RPY order
+  */
+  float heading() const
+  {
+    const px4_msgs::msg::VehicleLocalPosition & pos = last();
+    return pos.heading;
+  }
 };
 
 /** @}*/

px4_ros2_cpp/src/odometry/attitude.cpp

@@ -0,0 +1,19 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#include <px4_ros2/odometry/attitude.hpp>
+
+namespace px4_ros2
+{
+
+OdometryAttitude::OdometryAttitude(Context & context)
+: Subscription<px4_msgs::msg::VehicleAttitude>(context, "/fmu/out/vehicle_attitude")
+{
+  RequirementFlags requirements{};
+  requirements.attitude = true;
+  context.setRequirement(requirements);
+}
+
+} // namespace px4_ros2