From 1ae6a3816fc4856f4970ea5261c1c1c90ec95c64 Mon Sep 17 00:00:00 2001
From: Damien SIX <damien@robotsix.net>
Date: Fri, 28 Jun 2024 15:35:02 +0200
Subject: [PATCH] add angular velocity feedback (#43)

---
 px4_ros2_cpp/CMakeLists.txt                   |  2 +
 .../message_compatibility_check.hpp           |  1 +
 .../px4_ros2/odometry/angular_velocity.hpp    | 40 +++++++++++++++++++
 .../src/odometry/angular_velocity.cpp         | 19 +++++++++
 4 files changed, 62 insertions(+)
 create mode 100644 px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp
 create mode 100644 px4_ros2_cpp/src/odometry/angular_velocity.cpp

diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt
index 45516a8..874799f 100644
--- a/px4_ros2_cpp/CMakeLists.txt
+++ b/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
 )
diff --git a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp b/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp
index 3299bba..191ed61 100644
--- a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp
+++ b/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"}, \
diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp
new file mode 100644
index 0000000..e857755
--- /dev/null
+++ b/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 */
diff --git a/px4_ros2_cpp/src/odometry/angular_velocity.cpp b/px4_ros2_cpp/src/odometry/angular_velocity.cpp
new file mode 100644
index 0000000..28ad218
--- /dev/null
+++ b/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