examples: get attitude using odometry interface

Auterion · Feb 26, 2024 · 7be3c6f · 7be3c6f
1 parent cce0e88
commit 7be3c6f
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 42 deletions.
diff --git a/examples/cpp/modes/goto/include/mode.hpp b/examples/cpp/modes/goto/include/mode.hpp
@@ -8,7 +8,6 @@
 #include <px4_ros2/control/setpoint_types/goto.hpp>
 #include <px4_ros2/odometry/local_position.hpp>
 #include <px4_ros2/utils/geometry.hpp>
-#include <px4_msgs/msg/vehicle_attitude.hpp>
 
 #include <rclcpp/rclcpp.hpp>
 #include <Eigen/Core>
@@ -28,13 +27,6 @@ class FlightModeTest : public px4_ros2::ModeBase
     _goto_setpoint = std::make_shared<px4_ros2::GotoSetpointType>(*this);
 
     _vehicle_local_position = std::make_shared<px4_ros2::OdometryLocalPosition>(*this);
-
-    _vehicle_attitude_sub = node.create_subscription<px4_msgs::msg::VehicleAttitude>(
-      "/fmu/out/vehicle_attitude", rclcpp::QoS(1).best_effort(),
-      [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) {
-        updateVehicleHeading(msg);
-      }
-    );
   }
 
   void onActivate() override
@@ -103,7 +95,7 @@ class FlightModeTest : public px4_ros2::ModeBase
             px4_ros2::degToRad(40.f * speed_scale + (1.f - speed_scale) * 20.f);
 
           if (!_start_heading_set) {
-            _spinning_heading_rad = _vehicle_heading_rad;
+            _spinning_heading_rad = _vehicle_local_position->heading();
             _start_heading_set = true;
           }
 
@@ -158,9 +150,6 @@ class FlightModeTest : public px4_ros2::ModeBase
   Eigen::Vector3f _start_position_m;
   bool _start_position_set{false};
 
-  // [-pi, pi] current vehicle heading from VehicleAttitude subscription
-  float _vehicle_heading_rad{0.f};
-
   // [-pi, pi] current heading setpoint during spinning phase
   float _spinning_heading_rad{0.f};
 
@@ -169,14 +158,6 @@ class FlightModeTest : public px4_ros2::ModeBase
 
   std::shared_ptr<px4_ros2::GotoSetpointType> _goto_setpoint;
   std::shared_ptr<px4_ros2::OdometryLocalPosition> _vehicle_local_position;
-  rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr _vehicle_attitude_sub;
-
-  void updateVehicleHeading(const px4_msgs::msg::VehicleAttitude::UniquePtr & msg)
-  {
-    const Eigen::Quaternionf q = Eigen::Quaternionf(msg->q[0], msg->q[1], msg->q[2], msg->q[3]);
-    const Eigen::Vector3f rpy = px4_ros2::quaternionToEulerRpy(q);
-    _vehicle_heading_rad = rpy(2);
-  }
 
   bool positionReached(const Eigen::Vector3f & target_position_m) const
   {
@@ -191,7 +172,8 @@ class FlightModeTest : public px4_ros2::ModeBase
   bool headingReached(float target_heading_rad) const
   {
     static constexpr float kHeadingErrorThreshold = 7.0_deg;
-    const float heading_error_wrapped = px4_ros2::wrapPi(target_heading_rad - _vehicle_heading_rad);
+    const float heading_error_wrapped = px4_ros2::wrapPi(
+      target_heading_rad - _vehicle_local_position->heading());
     return fabsf(heading_error_wrapped) < kHeadingErrorThreshold;
   }
 };

diff --git a/examples/cpp/modes/goto_global/include/mode.hpp b/examples/cpp/modes/goto_global/include/mode.hpp
@@ -6,10 +6,10 @@
 
 #include <px4_ros2/components/mode.hpp>
 #include <px4_ros2/control/setpoint_types/goto.hpp>
+#include <px4_ros2/odometry/attitude.hpp>
 #include <px4_ros2/odometry/global_position.hpp>
 #include <px4_ros2/utils/geometry.hpp>
 #include <px4_ros2/utils/geodesic.hpp>
-#include <px4_msgs/msg/vehicle_attitude.hpp>
 
 #include <rclcpp/rclcpp.hpp>
 #include <Eigen/Core>
@@ -29,13 +29,7 @@ class FlightModeTest : public px4_ros2::ModeBase
     _goto_setpoint = std::make_shared<px4_ros2::GotoGlobalSetpointType>(*this);
 
     _vehicle_global_position = std::make_shared<px4_ros2::OdometryGlobalPosition>(*this);
-
-    _vehicle_attitude_sub = node.create_subscription<px4_msgs::msg::VehicleAttitude>(
-      "/fmu/out/vehicle_attitude", rclcpp::QoS(1).best_effort(),
-      [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) {
-        updateVehicleHeading(msg);
-      }
-    );
+    _vehicle_attitude = std::make_shared<px4_ros2::OdometryAttitude>(*this);
   }
 
   void onActivate() override
@@ -106,7 +100,7 @@ class FlightModeTest : public px4_ros2::ModeBase
             px4_ros2::degToRad(40.f * speed_scale + (1.f - speed_scale) * 20.f);
 
           if (!_start_heading_set) {
-            _spinning_heading_rad = _vehicle_heading_rad;
+            _spinning_heading_rad = _vehicle_attitude->yaw();
             _start_heading_set = true;
           }
 
@@ -161,9 +155,6 @@ class FlightModeTest : public px4_ros2::ModeBase
   Eigen::Vector3d _start_position_m;
   bool _start_position_set{false};
 
-  // [-pi, pi] current vehicle heading from VehicleAttitude subscription
-  float _vehicle_heading_rad{0.f};
-
   // [-pi, pi] current heading setpoint during spinning phase
   float _spinning_heading_rad{0.f};
 
@@ -172,14 +163,7 @@ class FlightModeTest : public px4_ros2::ModeBase
 
   std::shared_ptr<px4_ros2::GotoGlobalSetpointType> _goto_setpoint;
   std::shared_ptr<px4_ros2::OdometryGlobalPosition> _vehicle_global_position;
-  rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr _vehicle_attitude_sub;
-
-  void updateVehicleHeading(const px4_msgs::msg::VehicleAttitude::UniquePtr & msg)
-  {
-    const Eigen::Quaternionf q = Eigen::Quaternionf(msg->q[0], msg->q[1], msg->q[2], msg->q[3]);
-    const Eigen::Vector3f rpy = px4_ros2::quaternionToEulerRpy(q);
-    _vehicle_heading_rad = rpy(2);
-  }
+  std::shared_ptr<px4_ros2::OdometryAttitude> _vehicle_attitude;
 
   bool positionReached(const Eigen::Vector3d & target_position_m) const
   {
@@ -192,7 +176,8 @@ class FlightModeTest : public px4_ros2::ModeBase
   bool headingReached(float target_heading_rad) const
   {
     static constexpr float kHeadingErrorThreshold = 7.0_deg;
-    const float heading_error_wrapped = px4_ros2::wrapPi(target_heading_rad - _vehicle_heading_rad);
+    const float heading_error_wrapped = px4_ros2::wrapPi(
+      target_heading_rad - _vehicle_attitude->yaw());
     return fabsf(heading_error_wrapped) < kHeadingErrorThreshold;
   }
 };