add angular velocity feedback (#43)

px4_ros2_cpp/CMakeLists.txt

@@ -45,6 +45,7 @@ set(HEADER_FILES
         include/px4_ros2/odometry/attitude.hpp
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
+        include/px4_ros2/odometry/angular_velocity.hpp
         include/px4_ros2/utils/frame_conversion.hpp
         include/px4_ros2/utils/geodesic.hpp
         include/px4_ros2/utils/geometry.hpp
@@ -71,6 +72,7 @@ add_library(px4_ros2_cpp
         src/odometry/attitude.cpp
         src/odometry/global_position.cpp
         src/odometry/local_position.cpp
+        src/odometry/angular_velocity.cpp
         src/utils/geodesic.cpp
         src/utils/map_projection_impl.cpp
 )

px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp

@@ -31,6 +31,7 @@ using namespace std::chrono_literals; // NOLINT
   {"fmu/out/mode_completed"}, \
   {"fmu/out/register_ext_component_reply"}, \
   {"fmu/out/vehicle_attitude"}, \
+  {"fmu/out/vehicle_angular_velocity"}, \
   {"fmu/out/vehicle_command_ack"}, \
   {"fmu/out/vehicle_global_position"}, \
   {"fmu/out/vehicle_local_position"}, \

px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ * Copyright (c) 2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <Eigen/Eigen>
+#include <px4_msgs/msg/vehicle_angular_velocity.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/odometry/subscription.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup odometry
+ *  @{
+ */
+
+/**
+ * @brief Provides access to the vehicle's angular velocity estimate
+ */
+class OdometryAngularVelocity : public Subscription<px4_msgs::msg::VehicleAngularVelocity>
+{
+public:
+  explicit OdometryAngularVelocity(Context & context);
+
+  /**
+   * @brief Get the vehicle's angular velocity in FRD frame.
+   *
+   * @return the angular velocity
+   */
+  Eigen::Vector3f angularVelocityFrd() const
+  {
+    const px4_msgs::msg::VehicleAngularVelocity & av = last();
+    return Eigen::Vector3f{av.xyz[0], av.xyz[1], av.xyz[2]};
+  }
+};
+
+/** @}*/
+} /* namespace px4_ros2 */

px4_ros2_cpp/src/odometry/angular_velocity.cpp

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