diff --git a/examples/cpp/modes/rtl_replacement/include/mode.hpp b/examples/cpp/modes/rtl_replacement/include/mode.hpp index ab36a2d..f83af68 100644 --- a/examples/cpp/modes/rtl_replacement/include/mode.hpp +++ b/examples/cpp/modes/rtl_replacement/include/mode.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -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( - "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(*this); _trajectory_setpoint = std::make_shared(*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::SharedPtr _vehicle_land_detected_sub; + std::shared_ptr _land_detected; std::shared_ptr _trajectory_setpoint; }; diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index 874799f..efb2268 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/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 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 191ed61..86ebb66 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 @@ -27,6 +27,8 @@ 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"}, \ @@ -34,8 +36,10 @@ using namespace std::chrono_literals; // NOLINT {"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 diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp index e857755..dd33fd9 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/angular_velocity.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include namespace px4_ros2 { diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp index d4cd69e..351c796 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp @@ -8,8 +8,8 @@ #include #include #include -#include #include +#include namespace px4_ros2 { diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp index 8a2e00a..c1049b3 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include namespace px4_ros2 { diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp index 9b017e6..443e6b3 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include namespace px4_ros2 { diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp b/px4_ros2_cpp/include/px4_ros2/utils/subscription.hpp similarity index 99% rename from px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp rename to px4_ros2_cpp/include/px4_ros2/utils/subscription.hpp index 9b7289f..436e176 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp +++ b/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::SharedPtr _subscription{nullptr}; RosMessageType _last; diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/battery.hpp b/px4_ros2_cpp/include/px4_ros2/vehicle_state/battery.hpp new file mode 100644 index 0000000..96d44e9 --- /dev/null +++ b/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 +#include +#include +#include + +namespace px4_ros2 +{ +/** \ingroup vehicle_state + * @{ + */ + +/** + * @brief Provides access to the vehicle's battery status + * + * @ingroup vehicle_state + */ +class Battery : public Subscription +{ +public: + explicit Battery(Context & context) + : Subscription(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 */ diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/groups.dox b/px4_ros2_cpp/include/px4_ros2/vehicle_state/groups.dox new file mode 100644 index 0000000..a84e28b --- /dev/null +++ b/px4_ros2_cpp/include/px4_ros2/vehicle_state/groups.dox @@ -0,0 +1,3 @@ +/*! +\defgroup vehicle_state Vehicle State +*/ diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp b/px4_ros2_cpp/include/px4_ros2/vehicle_state/home_position.hpp new file mode 100644 index 0000000..de50733 --- /dev/null +++ b/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 +#include +#include +#include + +namespace px4_ros2 +{ +/** \ingroup vehicle_state + * @{ + */ + +/** + * @brief Provides access to the vehicle's home position + * + * @ingroup vehicle_state + */ +class HomePosition : public Subscription +{ +public: + explicit HomePosition(Context & context) + : Subscription(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 */ diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/land_detected.hpp b/px4_ros2_cpp/include/px4_ros2/vehicle_state/land_detected.hpp new file mode 100644 index 0000000..2329d39 --- /dev/null +++ b/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 +#include +#include + +namespace px4_ros2 +{ +/** \ingroup vehicle_state + * @{ + */ + +/** + * @brief Provides access to the vehicle's status + * + * @ingroup vehicle_state + */ +class LandDetected : public Subscription +{ +public: + explicit LandDetected(Context & context) + : Subscription(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 */ diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/vehicle_status.hpp b/px4_ros2_cpp/include/px4_ros2/vehicle_state/vehicle_status.hpp new file mode 100644 index 0000000..bf483a2 --- /dev/null +++ b/px4_ros2_cpp/include/px4_ros2/vehicle_state/vehicle_status.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * Copyright (c) 2023-2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace px4_ros2 +{ +/** \ingroup vehicle_state + * @{ + */ + +/** + * @brief Provides access to the vehicle's status + * + * @ingroup vehicle_state + */ +class VehicleStatus : public Subscription +{ +public: + explicit VehicleStatus(Context & context) + : Subscription(context, "fmu/out/vehicle_status") {} + + /** + * @brief Get the vehicle's arming status. + * + * @return true if the vehicle is armed, false otherwise + */ + bool armed() const + { + return last().arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED; + } + + /** + * @brief Get the vehicle's current active flight mode. + * + * @return the int value describing the mode + * @see navigation state values: https://github.com/PX4/PX4-Autopilot/blob/v1.15.0/msg/VehicleStatus.msg#L34-L65 + */ + uint8_t navState() const + { + return last().nav_state; + } + +}; + +/** @}*/ +} /* namespace px4_ros2 */ diff --git a/px4_ros2_cpp/include/px4_ros2/vehicle_state/vtol_status.hpp b/px4_ros2_cpp/include/px4_ros2/vehicle_state/vtol_status.hpp new file mode 100644 index 0000000..50ea559 --- /dev/null +++ b/px4_ros2_cpp/include/px4_ros2/vehicle_state/vtol_status.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * Copyright (c) 2023-2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace px4_ros2 +{ +/** \ingroup vehicle_state + * @{ + */ + +/** + * @brief Provides access to the vtol vehicle's status + * + * @ingroup vehicle_state + */ +class VtolStatus : public Subscription +{ +public: + explicit VtolStatus(Context & context) + : Subscription(context, "fmu/out/vtol_vehicle_status") {} + + /** + * @brief Check if vehicle is in an undefined state. This indicates the vehicle is not a VTOL. + * + * @return true if undefined state, false otherwise. + */ + bool isUndefined() const + { + return last().vehicle_vtol_state == + px4_msgs::msg::VtolVehicleStatus::VEHICLE_VTOL_STATE_UNDEFINED; + } + + /** + * @brief Check if VTOL is transitioning to fixed-wing. + * + * @return true if transitioning to FW, false otherwise. + */ + bool isTransitioningToFw() const + { + return last().vehicle_vtol_state == + px4_msgs::msg::VtolVehicleStatus::VEHICLE_VTOL_STATE_TRANSITION_TO_FW; + } + + /** + * @brief Check if VTOL is transitioning to multicopter. + * + * @return true if transitioning to MC, false otherwise. + */ + bool isTransitioningToMc() const + { + return last().vehicle_vtol_state == + px4_msgs::msg::VtolVehicleStatus::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; + } + + /** + * @brief Check if VTOL is in multicopter mode. + * + * @return true if in MC mode, false otherwise. + */ + bool isMcMode() const + { + return last().vehicle_vtol_state == px4_msgs::msg::VtolVehicleStatus::VEHICLE_VTOL_STATE_MC; + } + + /** + * @brief Check if VTOL is in fixed-wing mode. + * + * @return true if in FW mode, false otherwise. + */ + bool isFwMode() const + { + return last().vehicle_vtol_state == px4_msgs::msg::VtolVehicleStatus::VEHICLE_VTOL_STATE_FW; + } + +}; + +/** @}*/ +} /* namespace px4_ros2 */ diff --git a/scripts/check-used-topics.py b/scripts/check-used-topics.py index 72f8ad2..c0fd255 100755 --- a/scripts/check-used-topics.py +++ b/scripts/check-used-topics.py @@ -12,7 +12,7 @@ configs = [ # Tuples of (topics_list_file, topic define, source_dir list) ('px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp', 'ALL_PX4_ROS2_MESSAGES', [ - 'px4_ros2_cpp/src' + 'px4_ros2_cpp/src', 'px4_ros2_cpp/include' ]), ]