system_state: expand vehicle telemtry interfaces

examples/cpp/modes/rtl_replacement/include/mode.hpp

@@ -8,6 +8,7 @@
 #include <px4_ros2/components/health_and_arming_checks.hpp>
 #include <px4_ros2/components/mode_executor.hpp>
 #include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
+#include <px4_ros2/vehicle_state/land_detected.hpp>
 #include <px4_msgs/msg/vehicle_land_detected.hpp>
 
 #include <rclcpp/rclcpp.hpp>
@@ -24,11 +25,7 @@ class FlightModeTest : public px4_ros2::ModeBase
   explicit FlightModeTest(rclcpp::Node & node)
   : ModeBase(node, Settings{kName, true, ModeBase::kModeIDRtl})
   {
-    _vehicle_land_detected_sub = node.create_subscription<px4_msgs::msg::VehicleLandDetected>(
-      "fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(),
-      [this](px4_msgs::msg::VehicleLandDetected::UniquePtr msg) {
-        _landed = msg->landed;
-      });
+    _land_detected = std::make_shared<px4_ros2::LandDetected>(*this);
     _trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
 
     modeRequirements().home_position = true;
@@ -43,7 +40,7 @@ class FlightModeTest : public px4_ros2::ModeBase
 
   void updateSetpoint(float dt_s) override
   {
-    if (_landed) {
+    if (_land_detected->landed()) {
       completed(px4_ros2::Result::Success);
       return;
     }
@@ -54,7 +51,6 @@ class FlightModeTest : public px4_ros2::ModeBase
 
 private:
   rclcpp::Time _activation_time{};
-  bool _landed{true};
-  rclcpp::Subscription<px4_msgs::msg::VehicleLandDetected>::SharedPtr _vehicle_land_detected_sub;
+  std::shared_ptr<px4_ros2::LandDetected> _land_detected;
   std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
 };

px4_ros2_cpp/CMakeLists.txt

@@ -49,6 +49,11 @@ set(HEADER_FILES
         include/px4_ros2/utils/frame_conversion.hpp
         include/px4_ros2/utils/geodesic.hpp
         include/px4_ros2/utils/geometry.hpp
+        include/px4_ros2/vehicle_state/battery.hpp
+        include/px4_ros2/vehicle_state/home_position.hpp
+        include/px4_ros2/vehicle_state/land_detected.hpp
+        include/px4_ros2/vehicle_state/vehicle_status.hpp
+        include/px4_ros2/vehicle_state/vtol_status.hpp
 )
 
 add_library(px4_ros2_cpp

px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp

@@ -27,15 +27,19 @@ using namespace std::chrono_literals; // NOLINT
   {"fmu/in/vehicle_rates_setpoint"}, \
   {"fmu/in/vehicle_visual_odometry", "VehicleOdometry"}, \
   {"fmu/out/arming_check_request"}, \
+  {"fmu/out/battery_status"}, \
+  {"fmu/out/home_position"}, \
   {"fmu/out/manual_control_setpoint"}, \
   {"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_land_detected"}, \
   {"fmu/out/vehicle_local_position"}, \
-  {"fmu/out/vehicle_status"}
+  {"fmu/out/vehicle_status"}, \
+  {"fmu/out/vtol_vehicle_status"}
 
 
 namespace px4_ros2

px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp

@@ -8,7 +8,7 @@
 #include <Eigen/Eigen>
 #include <px4_msgs/msg/vehicle_angular_velocity.hpp>
 #include <px4_ros2/common/context.hpp>
-#include <px4_ros2/odometry/subscription.hpp>
+#include <px4_ros2/utils/subscription.hpp>
 
 namespace px4_ros2
 {

px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp

@@ -8,8 +8,8 @@
 #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>
+#include <px4_ros2/utils/subscription.hpp>
 
 namespace px4_ros2
 {

px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp

@@ -8,7 +8,7 @@
 #include <Eigen/Eigen>
 #include <px4_msgs/msg/vehicle_global_position.hpp>
 #include <px4_ros2/common/context.hpp>
-#include <px4_ros2/odometry/subscription.hpp>
+#include <px4_ros2/utils/subscription.hpp>
 
 namespace px4_ros2
 {

px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp

@@ -8,7 +8,7 @@
 #include <Eigen/Eigen>
 #include <px4_msgs/msg/vehicle_local_position.hpp>
 #include <px4_ros2/common/context.hpp>
-#include <px4_ros2/odometry/subscription.hpp>
+#include <px4_ros2/utils/subscription.hpp>
 
 namespace px4_ros2
 {

px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp → px4_ros2_cpp/include/px4_ros2/utils/subscription.hpp

@@ -86,9 +86,10 @@ class Subscription
     return hasReceivedMessages() && _node.get_clock()->now() - _last_message_time < max_delay;
   }
 
-private:
+protected:
   rclcpp::Node & _node;
 
+private:
   typename rclcpp::Subscription<RosMessageType>::SharedPtr _subscription{nullptr};
 
   RosMessageType _last;

px4_ros2_cpp/include/px4_ros2/vehicle_state/battery.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ * Copyright (c) 2023-2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <Eigen/Eigen>
+#include <px4_msgs/msg/battery_status.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/utils/subscription.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup vehicle_state
+ *  @{
+ */
+
+/**
+ * @brief Provides access to the vehicle's battery status
+ *
+ * @ingroup vehicle_state
+ */
+class Battery : public Subscription<px4_msgs::msg::BatteryStatus>
+{
+public:
+  explicit Battery(Context & context)
+  : Subscription<px4_msgs::msg::BatteryStatus>(context, "fmu/out/battery_status") {}
+
+  /**
+   * @brief Get the vehicle's battery voltage.
+   *
+   * @return the voltage [V], or 0 if unknown
+   */
+  float voltageV() const
+  {
+    return last().voltage_v;
+  }
+
+  /**
+   * @brief Get the vehicle's battery current.
+   *
+   * @return the current [A], or -1 if unknown
+   */
+  float currentA() const
+  {
+    return last().current_a;
+  }
+
+  /**
+   * @brief Get the vehicle's battery cumulative discharged capacity.
+   *
+   * @return the discharged capacity [mAh], or -1 if unknown
+   */
+  float dischargedCapacityMah() const
+  {
+    return last().discharged_mah;
+  }
+
+  /**
+   * @brief Get the vehicle's battery remaining charge as a fraction.
+   *
+   * @return the remaining battery charge, 0.0 (empty) to 1.0 (full), or -1 if unknown.
+   */
+  float remaningFraction() const
+  {
+    return last().remaining;
+  }
+
+  /**
+   * @brief Get the vehicle's battery cell count.
+   *
+   * @return the number of cells, 0 if unknown
+   */
+  uint8_t cellCount() const
+  {
+    return last().cell_count;
+  }
+
+  /**
+   * @brief Get the voltages of each cell in the vehicle's battery.
+   *
+   * @return an Eigen vector containing the cell voltages. If the cell count is unknown, returns an empty vector.
+   */
+  Eigen::VectorXf cellVoltagesV() const
+  {
+    const px4_msgs::msg::BatteryStatus & battery = last();
+    const uint8_t cell_count = battery.cell_count;
+
+    if (cell_count == 0) {
+      RCLCPP_WARN_THROTTLE(
+        _node.get_logger(),
+        *_node.get_clock(), 1000, "Failed to retrieve battery cell voltage: cell count unknown.");
+      return Eigen::VectorXf();
+    }
+
+    Eigen::VectorXf cell_voltages(cell_count);
+    for (uint8_t i = 0; i < cell_count; ++i) {
+      cell_voltages(i) = battery.voltage_cell_v[i];
+    }
+
+    return cell_voltages;
+  }
+
+};
+
+/** @}*/
+} /* namespace px4_ros2 */

px4_ros2_cpp/include/px4_ros2/vehicle_state/groups.dox

@@ -0,0 +1,3 @@
+/*!
+\defgroup vehicle_state Vehicle State
+*/

px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ * Copyright (c) 2023-2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <Eigen/Eigen>
+#include <px4_msgs/msg/home_position.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/utils/subscription.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup vehicle_state
+ *  @{
+ */
+
+/**
+ * @brief Provides access to the vehicle's home position
+ *
+ * @ingroup vehicle_state
+ */
+class HomePosition : public Subscription<px4_msgs::msg::HomePosition>
+{
+public:
+  explicit HomePosition(Context & context)
+  : Subscription<px4_msgs::msg::HomePosition>(context, "fmu/out/home_position") {}
+
+  /**
+   * @brief Get the vehicle's home position in local coordinates.
+   *
+   * @return the local coordinates of the home position (NED) [m]
+   */
+  Eigen::Vector3f localPosition() const
+  {
+    const px4_msgs::msg::HomePosition & home = last();
+    return Eigen::Vector3f{home.x, home.y, home.z};
+  }
+
+  /**
+   * @brief Get the vehicle's home position in global coordinates.
+   *
+   * @return the global coordinates of the home position (latitude [°], longitude [°], altitude [m AMSL])
+   */
+  Eigen::Vector3d globalPosition() const
+  {
+    const px4_msgs::msg::HomePosition & home = last();
+    return Eigen::Vector3d{home.lat, home.lon, home.alt};
+  }
+
+  /**
+   * @brief Get the vehicle's home position yaw.
+   *
+   * @return the yaw of the home position (NED) [rad]
+   */
+  float yaw() const
+  {
+    return last().yaw;
+  }
+
+  /**
+   * @brief Check if vehicle's local home position is valid (xyz).
+   *
+   * @return true if the local position has been set, otherwise false
+   */
+  bool localPositionValid() const
+  {
+    return lastValid() && last().valid_lpos;
+  }
+
+  /**
+   * @brief Check if vehicle's global horizontal home position is valid (lat, lon).
+   *
+   * @return true if latitude and longitude have been set, otherwise false
+   */
+  bool globaHorizontalPositionValid() const
+  {
+    return lastValid() && last().valid_hpos;
+  }
+
+  /**
+   * @brief Check if vehicle's home position altitude is valid.
+   *
+   * @return true if altitude has been set, otherwise false
+   */
+  bool altitudeValid() const
+  {
+    return lastValid() && last().valid_alt;
+  }
+
+  /**
+   * @brief Check if home position has been set manually.
+   *
+   * @return true if set manually, otherwise false
+   */
+  bool manualHome() const
+  {
+    return last().manual_home;
+  }
+
+};
+
+/** @}*/
+} /* namespace px4_ros2 */

px4_ros2_cpp/include/px4_ros2/vehicle_state/land_detected.hpp

@@ -0,0 +1,42 @@
+/****************************************************************************
+ * Copyright (c) 2023-2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_msgs/msg/vehicle_land_detected.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/utils/subscription.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup vehicle_state
+ *  @{
+ */
+
+/**
+ * @brief Provides access to the vehicle's status
+ *
+ * @ingroup vehicle_state
+ */
+class LandDetected : public Subscription<px4_msgs::msg::VehicleLandDetected>
+{
+public:
+  explicit LandDetected(Context & context)
+  : Subscription<px4_msgs::msg::VehicleLandDetected>(context, "fmu/out/vehicle_land_detected") {}
+
+  /**
+   * @brief Check if vehicle is landed on the ground.
+   *
+   * @return true if landed, false otherwise
+   */
+  bool landed() const
+  {
+    return last().landed;
+  }
+
+};
+
+/** @}*/
+} /* namespace px4_ros2 */