From 8841ffd3cdff25a5c7e6eb1ddfb05448d9b28822 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 13:22:37 +0200 Subject: [PATCH 1/6] overrides: check for confirmation from PX4 And resend request on timeout. --- .../message_compatibility_check.hpp | 1 + .../include/px4_ros2/components/overrides.hpp | 5 ++++ px4_ros2_cpp/src/components/mode_executor.cpp | 17 ++++++------ px4_ros2_cpp/src/components/overrides.cpp | 26 +++++++++++++++++++ 4 files changed, 41 insertions(+), 8 deletions(-) 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 4e2f1da0..500fa301 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 @@ -40,6 +40,7 @@ using namespace std::chrono_literals; // NOLINT {"fmu/out/airspeed_validated"}, \ {"fmu/out/arming_check_request"}, \ {"fmu/out/battery_status"}, \ + {"fmu/out/config_overrides_confirm", "ConfigOverrides"}, \ {"fmu/out/home_position"}, \ {"fmu/out/manual_control_setpoint"}, \ {"fmu/out/mode_completed"}, \ diff --git a/px4_ros2_cpp/include/px4_ros2/components/overrides.hpp b/px4_ros2_cpp/include/px4_ros2/components/overrides.hpp index 4cfaa5ee..51c51a77 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/overrides.hpp +++ b/px4_ros2_cpp/include/px4_ros2/components/overrides.hpp @@ -8,6 +8,8 @@ #include #include +#include "shared_subscription.hpp" + namespace px4_ros2 { class ModeBase; @@ -34,6 +36,9 @@ class ConfigOverrides { rclcpp::Publisher::SharedPtr _config_overrides_pub; bool _is_setup{false}; bool _require_update_after_setup{false}; + + rclcpp::TimerBase::SharedPtr _confirm_timer; + SharedSubscriptionCallbackInstance _config_overrides_confirm_sub; }; } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/components/mode_executor.cpp b/px4_ros2_cpp/src/components/mode_executor.cpp index b5cfa91a..9a374bb3 100644 --- a/px4_ros2_cpp/src/components/mode_executor.cpp +++ b/px4_ros2_cpp/src/components/mode_executor.cpp @@ -330,11 +330,10 @@ bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s) if (enabled && _is_in_charge && _registration->registered() && _prev_failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) { rclcpp::WaitSet wait_set; - const auto vehicle_status_sub = - SharedSubscription::instance( - _node, _topic_namespace_prefix + "fmu/out/vehicle_status" + - px4_ros2::getMessageNameVersion()) - .getSubscription(); + const auto vehicle_status_sub = _node.create_subscription( + _topic_namespace_prefix + "fmu/out/vehicle_status" + + px4_ros2::getMessageNameVersion(), + rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::VehicleStatus::UniquePtr) {}); wait_set.add_subscription(vehicle_status_sub); bool got_message = false; @@ -348,8 +347,7 @@ bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s) break; } - auto wait_ret = - wait_set.wait((timeout - (now - start_time)).to_chrono()); + const auto wait_ret = wait_set.wait(100ms); if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) { px4_msgs::msg::VehicleStatus msg; @@ -366,8 +364,11 @@ bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s) } } else { - RCLCPP_DEBUG(_node.get_logger(), "timeout"); + RCLCPP_DEBUG(_node.get_logger(), "deferFailsafesSync timeout"); } + + // Resend request + _config_overrides.deferFailsafes(enabled, timeout_s); } wait_set.remove_subscription(vehicle_status_sub); diff --git a/px4_ros2_cpp/src/components/overrides.cpp b/px4_ros2_cpp/src/components/overrides.cpp index 1e5b2a4d..25d59b62 100644 --- a/px4_ros2_cpp/src/components/overrides.cpp +++ b/px4_ros2_cpp/src/components/overrides.cpp @@ -18,6 +18,22 @@ ConfigOverrides::ConfigOverrides(rclcpp::Node& node, const std::string& topic_na topic_namespace_prefix + "fmu/in/config_overrides_request" + px4_ros2::getMessageNameVersion(), 1); + + // Handle confirmations from PX4 + _config_overrides_confirm_sub = SharedSubscription::create( + _node, + topic_namespace_prefix + "fmu/out/config_overrides_confirm" + + px4_ros2::getMessageNameVersion(), + [this](const px4_msgs::msg::ConfigOverrides::UniquePtr& msg) { + // Compare the whole struct without the timestamp field (which comes first) + const auto offset_after_timestamp = offsetof(px4_msgs::msg::ConfigOverrides, timestamp) + + sizeof(px4_msgs::msg::ConfigOverrides::timestamp); + if (memcmp(reinterpret_cast(&_current_overrides) + offset_after_timestamp, + reinterpret_cast(msg.get()) + offset_after_timestamp, + sizeof(px4_msgs::msg::ConfigOverrides) - offset_after_timestamp) == 0) { + _confirm_timer = nullptr; + } + }); } void ConfigOverrides::controlAutoDisarm(bool enabled) @@ -45,6 +61,16 @@ void ConfigOverrides::update() _current_overrides.timestamp = 0; // Let PX4 set the timestamp _config_overrides_pub->publish(_current_overrides); + // Start/reset confirmation timer: in case we do not get a confirmation until the timeout, it + // will resend the topic + _confirm_timer = rclcpp::create_timer( + &_node, _node.get_clock(), rclcpp::Duration::from_seconds(0.1), [this]() { + RCLCPP_WARN_ONCE( + _node.get_logger(), + "Config overrides confirmation timed out. Resending request (only printed once)"); + update(); + }); + } else { _require_update_after_setup = true; } From 3a17c46920316620f43ae7b496ad2f24a669d3a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 13:23:17 +0200 Subject: [PATCH 2/6] mode: continue to publish mode_completed even when already sent In case the previous topic got lost. --- px4_ros2_cpp/src/components/mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp index 859ceb7f..8a28e7af 100644 --- a/px4_ros2_cpp/src/components/mode.cpp +++ b/px4_ros2_cpp/src/components/mode.cpp @@ -198,7 +198,7 @@ void ModeBase::completed(Result result) if (_completed) { RCLCPP_DEBUG_ONCE(node().get_logger(), "Mode '%s': completed was already called", _registration->name().c_str()); - return; + // Continue to publish the topic, in case the previous one got lost } px4_msgs::msg::ModeCompleted mode_completed{}; From 71623225c13bd1bf409001c36bb65f8d0074bdba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 13:24:17 +0200 Subject: [PATCH 3/6] test: add check for mode_completed topic drop --- .../test/integration/mode_executor.cpp | 43 +++++++++++++++++-- 1 file changed, 40 insertions(+), 3 deletions(-) diff --git a/px4_ros2_cpp/test/integration/mode_executor.cpp b/px4_ros2_cpp/test/integration/mode_executor.cpp index 1cb16dda..b3804a2b 100644 --- a/px4_ros2_cpp/test/integration/mode_executor.cpp +++ b/px4_ros2_cpp/test/integration/mode_executor.cpp @@ -74,6 +74,18 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase { ModeExecutorTest(rclcpp::Node& node, FlightModeTest& owned_mode, Activation activation) : ModeExecutorBase(ModeExecutorBase::Settings{}.activate(activation), owned_mode), _node(node) { + _mode_completed_sub = node.create_subscription( + owned_mode.topicNamespacePrefix() + "fmu/out/mode_completed" + + px4_ros2::getMessageNameVersion(), + rclcpp::QoS(1).best_effort(), [this, &node](px4_msgs::msg::ModeCompleted::UniquePtr msg) { + if (_mode_completion_next_state.has_value()) { + EXPECT_EQ(msg->nav_state, _mode_completion_expected_nav_state); + RCLCPP_DEBUG(_node.get_logger(), "Received extra mode completion, continuing"); + const auto next_state = _mode_completion_next_state.value(); + _mode_completion_next_state.reset(); + runState(next_state, px4_ros2::Result::Success); + } + }); } enum class State { @@ -120,12 +132,29 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase { break; case State::TakingOff: - takeoff([this](px4_ros2::Result result) { runState(State::MyMode, result); }); + takeoff([this](px4_ros2::Result result) { + if (simulate_mode_completion_message_drop) { + RCLCPP_DEBUG(_node.get_logger(), + "Waiting for extra mode completion topic update from Takeoff mode"); + _mode_completion_next_state = State::MyMode; + _mode_completion_expected_nav_state = px4_ros2::ModeBase::kModeIDTakeoff; + } else { + runState(State::MyMode, result); + } + }); break; case State::MyMode: - scheduleMode(ownedMode().id(), - [this](px4_ros2::Result result) { runState(State::RTL, result); }); + scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { + if (simulate_mode_completion_message_drop) { + RCLCPP_DEBUG(_node.get_logger(), + "Waiting for extra mode completion topic update from the custom mode"); + _mode_completion_next_state = State::RTL; + _mode_completion_expected_nav_state = ownedMode().id(); + } else { + runState(State::RTL, result); + } + }); break; case State::RTL: @@ -148,8 +177,13 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase { std::function on_completed; std::function on_state_completed; + bool simulate_mode_completion_message_drop{false}; private: + rclcpp::Subscription::SharedPtr _mode_completed_sub; + std::optional _mode_completion_next_state; + uint8_t _mode_completion_expected_nav_state{}; + rclcpp::Node& _node; }; @@ -180,6 +214,9 @@ void TestExecutionAutonomous::run() // The executor is expected to be activated and then run through its states (successfully) + // Ensure mode completion is resent when dropped + _mode_executor->simulate_mode_completion_message_drop = true; + _mode_executor->on_completed = [this]() { EXPECT_EQ(_mode_executor->num_activations, 1); EXPECT_EQ(_mode_executor->num_deactivations, 0); From 7b9d041b284186f904cbad2975bce665b3de9ffc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 13:36:13 +0200 Subject: [PATCH 4/6] feat: use setpoint types instead of VehicleControlMode Motivations: - allows to check if the vehicle type supports a requested setpoint type - reliability: previously there was no confirmation to VehicleControlMode - a cleaner interface: no need to set controller flags This is a breaking change and requires a corresponding update on the PX4 side. This also changes the API for setpoint types. Functional change: an exception is thrown if there is no response from PX4 for a setpoint type, or if there is an error (e.g. due to vehicle incompatibility) --- .../include/px4_ros2/common/setpoint_base.hpp | 44 ++-- .../message_compatibility_check.hpp | 3 +- .../include/px4_ros2/components/mode.hpp | 12 +- .../setpoint_types/direct_actuators.hpp | 6 +- .../setpoint_types/experimental/attitude.hpp | 3 +- .../setpoint_types/experimental/rates.hpp | 3 +- .../experimental/rover/position.hpp | 6 +- .../experimental/rover/speed_attitude.hpp | 6 +- .../experimental/rover/speed_rate.hpp | 6 +- .../experimental/rover/speed_steering.hpp | 6 +- .../experimental/rover/throttle_attitude.hpp | 6 +- .../experimental/rover/throttle_rate.hpp | 6 +- .../experimental/rover/throttle_steering.hpp | 6 +- .../experimental/trajectory.hpp | 5 +- .../fixedwing/lateral_longitudinal.hpp | 8 +- .../setpoint_types/multicopter/goto.hpp | 5 +- px4_ros2_cpp/src/components/mode.cpp | 194 +++++++++++++----- px4_ros2_cpp/src/components/registration.cpp | 5 +- .../setpoint_types/direct_actuators.cpp | 13 -- .../setpoint_types/experimental/attitude.cpp | 10 - .../setpoint_types/experimental/rates.cpp | 11 - .../experimental/rover/position.cpp | 10 - .../experimental/rover/speed_attitude.cpp | 10 - .../experimental/rover/speed_rate.cpp | 10 - .../experimental/rover/speed_steering.cpp | 10 - .../experimental/rover/throttle_attitude.cpp | 10 - .../experimental/rover/throttle_rate.cpp | 10 - .../experimental/rover/throttle_steering.cpp | 10 - .../experimental/trajectory.cpp | 21 +- .../fixedwing/lateral_longitudinal.cpp | 22 +- .../setpoint_types/multicopter/goto.cpp | 14 -- 31 files changed, 251 insertions(+), 240 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/common/setpoint_base.hpp b/px4_ros2_cpp/include/px4_ros2/common/setpoint_base.hpp index aa73511b..f69ca517 100644 --- a/px4_ros2_cpp/include/px4_ros2/common/setpoint_base.hpp +++ b/px4_ros2_cpp/include/px4_ros2/common/setpoint_base.hpp @@ -8,7 +8,8 @@ #include #include #include -#include +#include +#include #include #include "context.hpp" @@ -19,30 +20,7 @@ namespace px4_ros2 { class SetpointBase : public std::enable_shared_from_this { public: using ShouldActivateCB = std::function; - - struct Configuration { - void fillControlMode(px4_msgs::msg::VehicleControlMode& control_mode) - { - control_mode.flag_control_rates_enabled = rates_enabled; - control_mode.flag_control_attitude_enabled = attitude_enabled; - control_mode.flag_control_acceleration_enabled = acceleration_enabled; - control_mode.flag_control_velocity_enabled = velocity_enabled; - control_mode.flag_control_position_enabled = position_enabled; - control_mode.flag_control_altitude_enabled = altitude_enabled; - control_mode.flag_control_allocation_enabled = control_allocation_enabled; - control_mode.flag_control_climb_rate_enabled = climb_rate_enabled; - } - - bool control_allocation_enabled{true}; - bool rates_enabled{true}; - bool attitude_enabled{true}; - bool altitude_enabled{true}; - bool acceleration_enabled{true}; - bool velocity_enabled{true}; - bool position_enabled{true}; - bool local_position_is_optional{false}; - bool climb_rate_enabled{false}; - }; + using SetpointType = decltype(px4_msgs::msg::SetpointConfig::type); explicit SetpointBase(Context& context) { context.addSetpointType(this); } @@ -58,7 +36,21 @@ class SetpointBase : public std::enable_shared_from_this { return {}; } - virtual Configuration getConfiguration() = 0; + /** + * Returns one of px4_msgs::msg::SetpointType::TYPE_* + */ + virtual SetpointType getSetpointType() = 0; + + /** + * Allows a setpoint class to clear an optional requirement. This is for setpoint types that + * support multiple variations, for example some that require local position and others that do + * not. + * + * @param setpoint_config_reply input and output config + */ + virtual void clearOptionalRequirements(px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) + { + } virtual float desiredUpdateRateHz() { return 50.f; } 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 500fa301..31daa4da 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 @@ -15,7 +15,6 @@ using namespace std::chrono_literals; // NOLINT {"fmu/in/actuator_servos"}, \ {"fmu/in/arming_check_reply"}, \ {"fmu/in/aux_global_position"}, \ - {"fmu/in/config_control_setpoints", "VehicleControlMode"}, \ {"fmu/in/config_overrides_request", "ConfigOverrides"}, \ {"fmu/in/fixed_wing_lateral_setpoint"}, \ {"fmu/in/fixed_wing_longitudinal_setpoint"}, \ @@ -30,6 +29,7 @@ using namespace std::chrono_literals; // NOLINT {"fmu/in/rover_speed_setpoint"}, \ {"fmu/in/rover_steering_setpoint"}, \ {"fmu/in/rover_throttle_setpoint"}, \ + {"fmu/in/setpoint_config"}, \ {"fmu/in/trajectory_setpoint"}, \ {"fmu/in/unregister_ext_component"}, \ {"fmu/in/vehicle_attitude_setpoint"}, \ @@ -45,6 +45,7 @@ using namespace std::chrono_literals; // NOLINT {"fmu/out/manual_control_setpoint"}, \ {"fmu/out/mode_completed"}, \ {"fmu/out/register_ext_component_reply"}, \ + {"fmu/out/setpoint_config_reply"}, \ {"fmu/out/vehicle_attitude"}, \ {"fmu/out/vehicle_angular_velocity"}, \ {"fmu/out/vehicle_command_ack"}, \ diff --git a/px4_ros2_cpp/include/px4_ros2/components/mode.hpp b/px4_ros2_cpp/include/px4_ros2/components/mode.hpp index 1b860726..f060fa69 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/mode.hpp +++ b/px4_ros2_cpp/include/px4_ros2/components/mode.hpp @@ -7,7 +7,8 @@ #include #include -#include +#include +#include #include #include #include @@ -203,10 +204,9 @@ class ModeBase : public Context { void updateSetpointUpdateTimer(); - void updateModeRequirementsFromSetpoints(); + void checkSetpointCompatibilityAndRequirements(); void setSetpointUpdateRateFromSetpointTypes(); - void publishSetpointConfig(SetpointBase& setpoint); - void activateSetpointType(SetpointBase& setpoint); + void activateSetpointType(const std::shared_ptr& setpoint); void deactivateAllSetpointTypes(); std::shared_ptr _registration; @@ -217,7 +217,8 @@ class ModeBase : public Context { HealthAndArmingChecks _health_and_arming_checks; rclcpp::Publisher::SharedPtr _mode_completed_pub; - rclcpp::Publisher::SharedPtr _config_control_setpoints_pub; + rclcpp::Publisher::SharedPtr _setpoint_config_pub; + SharedSubscriptionCallbackInstance _setpoint_config_reply_sub_cb; SharedSubscriptionCallbackInstance _vehicle_status_sub_cb; @@ -234,6 +235,7 @@ class ModeBase : public Context { std::vector> _setpoint_types; std::vector _new_setpoint_types; ///< This stores new setpoints during initialization, until registration + std::shared_ptr _current_activating_setpoint; ///< Setpoint waiting for a reply }; /** @}*/ diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp index 25c9e553..27dc5836 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp @@ -27,7 +27,11 @@ class DirectActuatorsSetpointType : public SetpointBase { ~DirectActuatorsSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_DIRECT_ACTUATORS; + } + float desiredUpdateRateHz() override { return 200.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp index 420be871..342aecf6 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp @@ -23,7 +23,8 @@ class AttitudeSetpointType : public SetpointBase { ~AttitudeSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_ATTITUDE; } + float desiredUpdateRateHz() override { return 100.f; } void update(const Eigen::Quaternionf& attitude_setpoint, diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp index 79741a94..185cd374 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp @@ -23,7 +23,8 @@ class RatesSetpointType : public SetpointBase { ~RatesSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_RATES; } + float desiredUpdateRateHz() override { return 200.f; } void update(const Eigen::Vector3f& rate_setpoints_frd_rad, diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/position.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/position.hpp index c7e52b3a..cc9c460d 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/position.hpp @@ -23,7 +23,11 @@ class RoverPositionSetpointType : public SetpointBase { ~RoverPositionSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_POSITION; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_attitude.hpp index 2de5d7c4..3477a8b4 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_attitude.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_attitude.hpp @@ -23,7 +23,11 @@ class RoverSpeedAttitudeSetpointType : public SetpointBase { ~RoverSpeedAttitudeSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_ATTITUDE; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_rate.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_rate.hpp index 69b9cfff..7cc43458 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_rate.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_rate.hpp @@ -23,7 +23,11 @@ class RoverSpeedRateSetpointType : public SetpointBase { ~RoverSpeedRateSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_RATE; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_steering.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_steering.hpp index 5a317935..ecc7d618 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_steering.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/speed_steering.hpp @@ -23,7 +23,11 @@ class RoverSpeedSteeringSetpointType : public SetpointBase { ~RoverSpeedSteeringSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_SPEED_STEERING; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_attitude.hpp index 862136fc..1ef4a0bb 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_attitude.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_attitude.hpp @@ -23,7 +23,11 @@ class RoverThrottleAttitudeSetpointType : public SetpointBase { ~RoverThrottleAttitudeSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_ATTITUDE; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_rate.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_rate.hpp index 7aa82459..73630326 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_rate.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_rate.hpp @@ -23,7 +23,11 @@ class RoverThrottleRateSetpointType : public SetpointBase { ~RoverThrottleRateSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_RATE; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_steering.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_steering.hpp index 509f0ab6..cc4155d0 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_steering.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rover/throttle_steering.hpp @@ -23,7 +23,11 @@ class RoverThrottleSteeringSetpointType : public SetpointBase { ~RoverThrottleSteeringSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_ROVER_THROTTLE_STEERING; + } + float desiredUpdateRateHz() override { return 30.f; } /** diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp index 370156f3..4f5f57ce 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp @@ -32,7 +32,10 @@ class TrajectorySetpointType : public SetpointBase { ~TrajectorySetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_TRAJECTORY; } + + void clearOptionalRequirements( + px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override; void update(const Eigen::Vector3f& velocity_ned_m_s, const std::optional& acceleration_ned_m_s2 = {}, diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp index 1a46e8f6..d7918ca8 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp @@ -37,7 +37,13 @@ class FwLateralLongitudinalSetpointType : public SetpointBase { ~FwLateralLongitudinalSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_FIXEDWING_LATERAL_LONGITUDINAL; + } + + void clearOptionalRequirements( + px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override; /** * @brief Update the setpoint with full flexibility by passing a FwLateralLongitudinalSetpoint diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/multicopter/goto.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/multicopter/goto.hpp index 698672e4..f4f2c670 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/multicopter/goto.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/multicopter/goto.hpp @@ -25,7 +25,10 @@ class MulticopterGotoSetpointType : public SetpointBase { ~MulticopterGotoSetpointType() override = default; - Configuration getConfiguration() override; + SetpointType getSetpointType() override + { + return px4_msgs::msg::SetpointConfig::TYPE_MULTICOPTER_GOTO; + } /** * @brief Go-to setpoint update diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp index 8a28e7af..96bbdeea 100644 --- a/px4_ros2_cpp/src/components/mode.cpp +++ b/px4_ros2_cpp/src/components/mode.cpp @@ -48,10 +48,24 @@ ModeBase::ModeBase(rclcpp::Node& node, ModeBase::Settings settings, topic_namespace_prefix + "fmu/in/mode_completed" + px4_ros2::getMessageNameVersion(), 1); - _config_control_setpoints_pub = node.create_publisher( - topic_namespace_prefix + "fmu/in/config_control_setpoints" + - px4_ros2::getMessageNameVersion(), + _setpoint_config_pub = node.create_publisher( + topic_namespace_prefix + "fmu/in/setpoint_config" + + px4_ros2::getMessageNameVersion(), 1); + + _setpoint_config_reply_sub_cb = SharedSubscription::create( + node, + topic_namespace_prefix + "fmu/out/setpoint_config_reply" + + px4_ros2::getMessageNameVersion(), + [this](const px4_msgs::msg::SetpointConfigReply::UniquePtr& msg) { + if (msg->source_id == id()) { + if (_current_activating_setpoint && + _current_activating_setpoint->getSetpointType() == msg->type) { + _current_activating_setpoint->setActive(true); + _current_activating_setpoint.reset(); + } + } + }); } ModeBase::ModeID ModeBase::id() const @@ -118,7 +132,6 @@ void ModeBase::callOnActivate() _is_active = true; _completed = false; _last_setpoint_update = node().get_clock()->now(); - activateSetpointType(*_setpoint_types[0]); onActivate(); if (_setpoint_update_rate_hz > FLT_EPSILON) { @@ -161,6 +174,123 @@ void ModeBase::updateSetpointUpdateTimer() } } +void ModeBase::checkSetpointCompatibilityAndRequirements() +{ + // Check setpoint types compatibility with current vehicle type + + // Create a fresh subscription to avoid ROS Jazzy WaitSet conflicts + const auto setpoint_config_reply_sub = + node().create_subscription( + topicNamespacePrefix() + "fmu/out/setpoint_config_reply" + + px4_ros2::getMessageNameVersion(), + rclcpp::QoS(1).best_effort(), [](px4_msgs::msg::SetpointConfigReply::UniquePtr) {}); + + // Wait until DDS discovery has matched both directions so that the very + // first publish is not silently dropped + const auto discovery_start = std::chrono::steady_clock::now(); + const auto discovery_timeout = 3000ms; + while (setpoint_config_reply_sub->get_publisher_count() == 0 || + _setpoint_config_pub->get_subscription_count() == 0) { + if (std::chrono::steady_clock::now() >= discovery_start + discovery_timeout) { + RCLCPP_WARN(node().get_logger(), + "Timeout waiting for setpoint config discovery " + "(reply publishers=%zu, config subscribers=%zu)", + setpoint_config_reply_sub->get_publisher_count(), + _setpoint_config_pub->get_subscription_count()); + break; + } + std::this_thread::sleep_for(50ms); + } + rclcpp::WaitSet wait_set; + wait_set.add_subscription(setpoint_config_reply_sub); + + unsigned setpoint_index = 0; + for (const auto& setpoint : _setpoint_types) { + px4_msgs::msg::SetpointConfig setpoint_config{}; + setpoint_config.source_id = static_cast(id()); + setpoint_config.should_apply = false; + setpoint_config.type = setpoint->getSetpointType(); + setpoint_config.timestamp = 0; // Let PX4 set the timestamp + + bool got_reply = false; + + for (int retries = 0; retries < 5 && !got_reply; ++retries) { + _setpoint_config_pub->publish(setpoint_config); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = 300ms; + while (!got_reply) { + auto now = std::chrono::steady_clock::now(); + + if (now >= start_time + timeout) { + break; + } + + auto wait_ret = wait_set.wait(timeout - (now - start_time)); + + if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) { + px4_msgs::msg::SetpointConfigReply reply; + rclcpp::MessageInfo info; + + if (setpoint_config_reply_sub->take(reply, info)) { + if (reply.source_id == id() && reply.type == setpoint_config.type) { + if (reply.result != px4_msgs::msg::SetpointConfigReply::RESULT_SUCCESS) { + // This is fatal, the setpoint cannot be used. + // We could extend the API and allow for optional setpoint types, so that a mode + // could fall back to another type (to e.g. support multiple vehicle types). + switch (reply.result) { + case px4_msgs::msg::SetpointConfigReply::RESULT_UNSUPPORTED: + throw Exception("Setpoint type " + std::to_string(setpoint_config.type) + + " with index " + std::to_string(setpoint_index) + + " is not supported by the current vehicle type"); + case px4_msgs::msg::SetpointConfigReply::RESULT_UNKNOWN_SETPOINT_TYPE: + throw Exception("Setpoint type " + std::to_string(setpoint_config.type) + + " with index " + std::to_string(setpoint_index) + + " is not known by the FMU"); + case px4_msgs::msg::SetpointConfigReply::RESULT_FAILURE_OTHER: + default: + throw Exception("Setpoint type " + std::to_string(setpoint_config.type) + + " with index " + std::to_string(setpoint_index) + + " was rejected by the FMU"); + } + } + + // Apply mode requirement flags + setpoint->clearOptionalRequirements(reply); + RequirementFlags& requirements = modeRequirements(); + requirements.angular_velocity |= reply.mode_req_angular_velocity; + requirements.attitude |= reply.mode_req_attitude; + requirements.local_alt |= reply.mode_req_local_alt; + requirements.local_position |= reply.mode_req_local_position; + + if (requirements.manual_control) { + // Use relaxed local position accuracy if a manual mode + if (requirements.local_position) { + requirements.local_position = false; + requirements.local_position_relaxed = true; + } + } + + got_reply = true; + } + } else { + RCLCPP_DEBUG(node().get_logger(), "No SetpointConfigReply message received"); + } + + } else { + RCLCPP_DEBUG(node().get_logger(), "timeout"); + } + } + } + + if (!got_reply) { + // If we did not get a reply, something is very wrong + throw Exception("Did not get a reply from FMU for setpoint configuration"); + } + ++setpoint_index; + } + wait_set.remove_subscription(setpoint_config_reply_sub); +} + void ModeBase::setSetpointUpdateRate(float rate_hz) { _setpoint_update_timer = nullptr; @@ -219,7 +349,7 @@ void ModeBase::onAboutToRegister() setpoint->setShouldActivateCallback([this, setpoint]() { for (auto& setpoint_type : _setpoint_types) { if (setpoint_type.get() == setpoint) { - activateSetpointType(*setpoint); + activateSetpointType(setpoint_type); RCLCPP_DEBUG(node().get_logger(), "Mode '%s': changing setpoint type", _registration->name().c_str()); } else { @@ -229,8 +359,6 @@ void ModeBase::onAboutToRegister() }); } _new_setpoint_types.clear(); - - updateModeRequirementsFromSetpoints(); } bool ModeBase::onRegistered() @@ -243,9 +371,8 @@ bool ModeBase::onRegistered() return false; } - // TODO: check setpoint types compatibility with current vehicle type + checkSetpointCompatibilityAndRequirements(); - publishSetpointConfig(*_setpoint_types[0]); if (_setpoint_update_rate_hz < FLT_EPSILON) { // Do not use default setpoint rate if rate was already set by user setSetpointUpdateRateFromSetpointTypes(); @@ -254,33 +381,6 @@ bool ModeBase::onRegistered() return true; } -void ModeBase::updateModeRequirementsFromSetpoints() -{ - // Set a mode requirement if at least one setypoint type requires it - RequirementFlags& requirements = modeRequirements(); - for (const auto& setpoint_type : _setpoint_types) { - const auto config = setpoint_type->getConfiguration(); - - requirements.angular_velocity |= config.rates_enabled; - requirements.attitude |= config.attitude_enabled; - requirements.local_alt |= config.altitude_enabled; - requirements.local_alt |= config.climb_rate_enabled; - - if (!config.local_position_is_optional) { - requirements.local_position |= config.velocity_enabled; - requirements.local_position |= config.position_enabled; - } - } - - if (requirements.manual_control) { - // Use relaxed local position accuracy if a manual mode - if (requirements.local_position) { - requirements.local_position = false; - requirements.local_position_relaxed = true; - } - } -} - void ModeBase::setSetpointUpdateRateFromSetpointTypes() { // Set update rate based on setpoint types @@ -293,19 +393,17 @@ void ModeBase::setSetpointUpdateRateFromSetpointTypes() } } -void ModeBase::publishSetpointConfig(SetpointBase& setpoint) -{ - px4_msgs::msg::VehicleControlMode control_mode{}; - control_mode.source_id = static_cast(id()); - setpoint.getConfiguration().fillControlMode(control_mode); - control_mode.timestamp = 0; // Let PX4 set the timestamp - _config_control_setpoints_pub->publish(control_mode); -} - -void ModeBase::activateSetpointType(SetpointBase& setpoint) +void ModeBase::activateSetpointType(const std::shared_ptr& setpoint) { - setpoint.setActive(true); - publishSetpointConfig(setpoint); + _current_activating_setpoint = setpoint; + px4_msgs::msg::SetpointConfig setpoint_config{}; + setpoint_config.source_id = static_cast(id()); + setpoint_config.should_apply = true; + setpoint_config.type = setpoint->getSetpointType(); + setpoint_config.timestamp = 0; // Let PX4 set the timestamp + _setpoint_config_pub->publish(setpoint_config); + // setActive() will be called when we get a matching reply from PX4. If not (e.g. on message + // drop), the next setpoint update triggers another setpoint activation request. } void ModeBase::deactivateAllSetpointTypes() diff --git a/px4_ros2_cpp/src/components/registration.cpp b/px4_ros2_cpp/src/components/registration.cpp index 0a594857..fb98b08a 100644 --- a/px4_ros2_cpp/src/components/registration.cpp +++ b/px4_ros2_cpp/src/components/registration.cpp @@ -11,7 +11,7 @@ #include #include -static constexpr uint16_t kLatestPX4ROS2ApiVersion = 1; +static constexpr uint16_t kLatestPX4ROS2ApiVersion = 2; using namespace std::chrono_literals; @@ -162,7 +162,8 @@ bool Registration::doRegister(const RegistrationSettings& settings) _registered = true; } else { RCLCPP_FATAL(_node.get_logger(), - "Incompatible ROS2 library API version: got %i, expected %i", + "Incompatible ROS2 library API version: got %i, expected %i (update " + "PX4 or the PX4 ROS library)", reply.px4_ros2_api_version, kLatestPX4ROS2ApiVersion); } diff --git a/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp b/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp index 04c6431c..e7f96a9a 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/direct_actuators.cpp @@ -47,17 +47,4 @@ void DirectActuatorsSetpointType::updateServos( _actuator_servos_pub->publish(sp_servos); } -SetpointBase::Configuration DirectActuatorsSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = false; - config.rates_enabled = false; - config.attitude_enabled = false; - config.altitude_enabled = false; - config.climb_rate_enabled = false; - config.acceleration_enabled = false; - config.velocity_enabled = false; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp index 7afd31ab..ac64ed41 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp @@ -64,14 +64,4 @@ void AttitudeSetpointType::update(const float roll, const float pitch, const flo _vehicle_attitude_setpoint_pub->publish(sp); } -SetpointBase::Configuration AttitudeSetpointType::getConfiguration() -{ - Configuration config{}; - config.altitude_enabled = false; - config.climb_rate_enabled = false; - config.acceleration_enabled = false; - config.velocity_enabled = false; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp index bddd9bdf..0b9e0e8b 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp @@ -34,15 +34,4 @@ void RatesSetpointType::update(const Eigen::Vector3f& rate_setpoints_frd_rad, _vehicle_rates_setpoint_pub->publish(sp); } -SetpointBase::Configuration RatesSetpointType::getConfiguration() -{ - Configuration config{}; - config.attitude_enabled = false; - config.altitude_enabled = false; - config.climb_rate_enabled = false; - config.acceleration_enabled = false; - config.velocity_enabled = false; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/position.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/position.cpp index 2625ac3e..98739fb5 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/position.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/position.cpp @@ -39,14 +39,4 @@ void RoverPositionSetpointType::update(const Eigen::Vector2f& position_ned, _rover_position_setpoint_pub->publish(sp); } -SetpointBase::Configuration RoverPositionSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = true; - config.velocity_enabled = true; - config.position_enabled = true; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_attitude.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_attitude.cpp index 710fe711..f67dcacf 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_attitude.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_attitude.cpp @@ -40,14 +40,4 @@ void RoverSpeedAttitudeSetpointType::update(const float speed_body_x, const floa _rover_attitude_setpoint_pub->publish(sp_att); } -SetpointBase::Configuration RoverSpeedAttitudeSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = true; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_rate.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_rate.cpp index 1bae7002..53476ae9 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_rate.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_rate.cpp @@ -39,14 +39,4 @@ void RoverSpeedRateSetpointType::update(const float speed_body_x, const float ya _rover_rate_setpoint_pub->publish(sp_rate); } -SetpointBase::Configuration RoverSpeedRateSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = false; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_steering.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_steering.cpp index bbb4303c..87578188 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_steering.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/speed_steering.cpp @@ -41,14 +41,4 @@ void RoverSpeedSteeringSetpointType::update(const float speed_body_x, _rover_steering_setpoint_pub->publish(sp_steering); } -SetpointBase::Configuration RoverSpeedSteeringSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = false; - config.attitude_enabled = false; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_attitude.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_attitude.cpp index c284f880..a9b39f00 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_attitude.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_attitude.cpp @@ -42,14 +42,4 @@ void RoverThrottleAttitudeSetpointType::update(const float throttle_body_x, _rover_attitude_setpoint_pub->publish(sp_att); } -SetpointBase::Configuration RoverThrottleAttitudeSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = true; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_rate.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_rate.cpp index ab50eb4e..268e38a0 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_rate.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_rate.cpp @@ -41,14 +41,4 @@ void RoverThrottleRateSetpointType::update(const float throttle_body_x, _rover_rate_setpoint_pub->publish(sp_rate); } -SetpointBase::Configuration RoverThrottleRateSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = false; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_steering.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_steering.cpp index 2e53d7df..73ed16f4 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_steering.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rover/throttle_steering.cpp @@ -42,14 +42,4 @@ void RoverThrottleSteeringSetpointType::update(const float throttle_body_x, _rover_steering_setpoint_pub->publish(sp_steering); } -SetpointBase::Configuration RoverThrottleSteeringSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = false; - config.attitude_enabled = false; - config.velocity_enabled = true; - config.position_enabled = false; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp index 341d3356..cb2b6aa3 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp @@ -19,6 +19,14 @@ TrajectorySetpointType::TrajectorySetpointType(Context& context, bool local_posi 1); } +void TrajectorySetpointType::clearOptionalRequirements( + px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) +{ + if (_local_position_is_optional) { + setpoint_config_reply.mode_req_local_position = false; + } +} + void TrajectorySetpointType::update(const Eigen::Vector3f& velocity_ned_m_s, const std::optional& acceleration_ned_m_s2, std::optional yaw_ned_rad, @@ -83,17 +91,4 @@ void TrajectorySetpointType::updatePosition(const Eigen::Vector3f& position_ned_ _trajectory_setpoint_pub->publish(sp); } -SetpointBase::Configuration TrajectorySetpointType::getConfiguration() -{ - Configuration config{}; - config.rates_enabled = true; - config.attitude_enabled = true; - config.acceleration_enabled = true; - config.position_enabled = true; - config.velocity_enabled = true; - config.altitude_enabled = true; - config.climb_rate_enabled = true; - config.local_position_is_optional = _local_position_is_optional; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp b/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp index 347a1d89..8a59a309 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp @@ -82,6 +82,14 @@ void FwLateralLongitudinalSetpointType::updateWithHeightRate( _fw_longitudinal_sp_pub->publish(longitudinal_sp); } +void FwLateralLongitudinalSetpointType::clearOptionalRequirements( + px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) +{ + if (_local_position_is_optional) { + setpoint_config_reply.mode_req_local_position = false; + } +} + void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoint& setpoint, const FwControlConfiguration& config) { @@ -127,18 +135,4 @@ void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoi _fw_longitudinal_sp_pub->publish(longitudinal_sp); } -SetpointBase::Configuration FwLateralLongitudinalSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = true; - config.altitude_enabled = true; - config.acceleration_enabled = true; - config.velocity_enabled = true; - config.position_enabled = true; - config.climb_rate_enabled = true; - config.local_position_is_optional = _local_position_is_optional; - return config; -} } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/control/setpoint_types/multicopter/goto.cpp b/px4_ros2_cpp/src/control/setpoint_types/multicopter/goto.cpp index d9047905..3ddb7102 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/multicopter/goto.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/multicopter/goto.cpp @@ -50,20 +50,6 @@ void MulticopterGotoSetpointType::update(const Eigen::Vector3f& position, _goto_setpoint_pub->publish(sp); } -SetpointBase::Configuration MulticopterGotoSetpointType::getConfiguration() -{ - Configuration config{}; - config.control_allocation_enabled = true; - config.rates_enabled = true; - config.attitude_enabled = true; - config.altitude_enabled = true; - config.acceleration_enabled = true; - config.velocity_enabled = true; - config.position_enabled = true; - config.climb_rate_enabled = true; - return config; -} - MulticopterGotoGlobalSetpointType::MulticopterGotoGlobalSetpointType(Context& context) : _node(context.node()), _map_projection(std::make_unique(context)), From f0253f51e04606dd2b112a1d9b3921b225b38d6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 13:44:10 +0200 Subject: [PATCH 5/6] fw lateral longitudinal sp: republish configuration if needed Mainly to ensure it is sent in case a topic is dropped, and then an update method is used that does not update the config. --- .../fixedwing/lateral_longitudinal.hpp | 7 ++++ .../fixedwing/lateral_longitudinal.cpp | 33 +++++++++++++++++-- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp index d7918ca8..98e85286 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/fixedwing/lateral_longitudinal.hpp @@ -109,6 +109,8 @@ class FwLateralLongitudinalSetpointType : public SetpointBase { float desiredUpdateRateHz() override { return 30.f; } private: + void publishConfigurationIfNeeded(); + rclcpp::Node& _node; bool _local_position_is_optional; rclcpp::Publisher::SharedPtr _fw_lateral_sp_pub; @@ -119,6 +121,11 @@ class FwLateralLongitudinalSetpointType : public SetpointBase { _lateral_control_configuration_pub; rclcpp::Publisher::SharedPtr _longitudinal_control_configuration_pub; + + rclcpp::Time _last_config_published_time{}; + std::optional _current_lateral_configuration; + std::optional + _current_longitudinal_configuration; }; struct FwLateralLongitudinalSetpoint { diff --git a/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp b/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp index 8a59a309..41bc87f9 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/fixedwing/lateral_longitudinal.cpp @@ -6,6 +6,8 @@ #include #include +using namespace std::chrono_literals; // NOLINT + namespace px4_ros2 { FwLateralLongitudinalSetpointType::FwLateralLongitudinalSetpointType(Context& context, @@ -34,6 +36,7 @@ FwLateralLongitudinalSetpointType::FwLateralLongitudinalSetpointType(Context& co context.topicNamespacePrefix() + "fmu/in/longitudinal_control_configuration" + px4_ros2::getMessageNameVersion(), 1); + _last_config_published_time = _node.now(); } void FwLateralLongitudinalSetpointType::updateWithAltitude( @@ -57,6 +60,8 @@ void FwLateralLongitudinalSetpointType::updateWithAltitude( longitudinal_sp.throttle_direct = NAN; _fw_longitudinal_sp_pub->publish(longitudinal_sp); + + publishConfigurationIfNeeded(); } void FwLateralLongitudinalSetpointType::updateWithHeightRate( @@ -80,6 +85,25 @@ void FwLateralLongitudinalSetpointType::updateWithHeightRate( longitudinal_sp.throttle_direct = NAN; _fw_longitudinal_sp_pub->publish(longitudinal_sp); + + publishConfigurationIfNeeded(); +} + +void FwLateralLongitudinalSetpointType::publishConfigurationIfNeeded() +{ + // In case a configuration was sent once, we regularly send it again at a lower rate, in case of a + // mode switch, or if a message got lost. + const auto now = _node.now(); + if (now > _last_config_published_time + 100ms) { + if (_current_lateral_configuration) { + _lateral_control_configuration_pub->publish(*_current_lateral_configuration); + } + if (_current_longitudinal_configuration) { + _longitudinal_control_configuration_pub->publish(*_current_longitudinal_configuration); + } + + _last_config_published_time = now; + } } void FwLateralLongitudinalSetpointType::clearOptionalRequirements( @@ -95,10 +119,9 @@ void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoi { onUpdate(); - update(setpoint); - px4_msgs::msg::LateralControlConfiguration lateral_configuration{}; lateral_configuration.lateral_accel_max = config.max_lateral_acceleration.value_or(NAN); + _current_lateral_configuration = lateral_configuration; _lateral_control_configuration_pub->publish(lateral_configuration); @@ -110,8 +133,12 @@ void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoi longitudinal_configuration.climb_rate_target = config.target_climb_rate.value_or(NAN); longitudinal_configuration.sink_rate_target = config.target_sink_rate.value_or(NAN); longitudinal_configuration.speed_weight = config.speed_weight.value_or(NAN); + _current_longitudinal_configuration = longitudinal_configuration; _longitudinal_control_configuration_pub->publish(longitudinal_configuration); + _last_config_published_time = _node.now(); + + update(setpoint); } void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoint& setpoint) @@ -133,6 +160,8 @@ void FwLateralLongitudinalSetpointType::update(const FwLateralLongitudinalSetpoi longitudinal_sp.throttle_direct = setpoint.throttle_direct.value_or(NAN); _fw_longitudinal_sp_pub->publish(longitudinal_sp); + + publishConfigurationIfNeeded(); } } // namespace px4_ros2 From 42e9e7c626abfd1938c142d0186de50e72e7324f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 16 Jun 2026 14:35:51 +0200 Subject: [PATCH 6/6] tests: skip setpoint config check on registration because there is no (fake) autopilot --- px4_ros2_cpp/include/px4_ros2/components/mode.hpp | 2 ++ .../include/px4_ros2/mission/mission_executor.hpp | 5 +++++ px4_ros2_cpp/src/components/mode.cpp | 4 +++- px4_ros2_cpp/test/unit/mission_execution.hpp | 1 + px4_ros2_cpp/test/unit/modes.cpp | 13 +++++++++---- 5 files changed, 20 insertions(+), 5 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/components/mode.hpp b/px4_ros2_cpp/include/px4_ros2/components/mode.hpp index f060fa69..6a2146d7 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/mode.hpp +++ b/px4_ros2_cpp/include/px4_ros2/components/mode.hpp @@ -180,6 +180,7 @@ class ModeBase : public Context { protected: void setSkipMessageCompatibilityCheck() { _skip_message_compatibility_check = true; } + void setSkipSetpointCheck() { _skip_setpoint_check = true; } void overrideRegistration(const std::shared_ptr& registration); void disableWatchdogTimer() { _health_and_arming_checks.disableWatchdogTimer(); } @@ -213,6 +214,7 @@ class ModeBase : public Context { const Settings _settings; bool _skip_message_compatibility_check{false}; + bool _skip_setpoint_check{false}; ///< Skip setpoint checks on startup. Only for unit tests. HealthAndArmingChecks _health_and_arming_checks; diff --git a/px4_ros2_cpp/include/px4_ros2/mission/mission_executor.hpp b/px4_ros2_cpp/include/px4_ros2/mission/mission_executor.hpp index f9d05280..04967be6 100644 --- a/px4_ros2_cpp/include/px4_ros2/mission/mission_executor.hpp +++ b/px4_ros2_cpp/include/px4_ros2/mission/mission_executor.hpp @@ -167,6 +167,11 @@ class MissionExecutor { ModeBase::disableWatchdogTimer(); } + void setSkipSetpointCheck() // NOLINT we just want to change the methods visibility + { + ModeBase::setSkipSetpointCheck(); + } + private: MissionExecutor& _mission_executor; }; diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp index 96bbdeea..b8736cd1 100644 --- a/px4_ros2_cpp/src/components/mode.cpp +++ b/px4_ros2_cpp/src/components/mode.cpp @@ -371,7 +371,9 @@ bool ModeBase::onRegistered() return false; } - checkSetpointCompatibilityAndRequirements(); + if (!_skip_setpoint_check) { + checkSetpointCompatibilityAndRequirements(); + } if (_setpoint_update_rate_hz < FLT_EPSILON) { // Do not use default setpoint rate if rate was already set by user diff --git a/px4_ros2_cpp/test/unit/mission_execution.hpp b/px4_ros2_cpp/test/unit/mission_execution.hpp index 8bd291b5..43ce47ae 100644 --- a/px4_ros2_cpp/test/unit/mission_execution.hpp +++ b/px4_ros2_cpp/test/unit/mission_execution.hpp @@ -143,6 +143,7 @@ class MissionExecutorTest : public px4_ros2::MissionExecutor { bool doRegisterImpl(MissionMode& mode, MissionModeExecutor& executor) override { mode.disableWatchdogTimer(); + mode.setSkipSetpointCheck(); executor.setRegistration(std::make_shared(_node)); const bool ret = MissionExecutor::doRegisterImpl(mode, executor); _mode_id = executor.ownedMode().id(); diff --git a/px4_ros2_cpp/test/unit/modes.cpp b/px4_ros2_cpp/test/unit/modes.cpp index c3d3c82a..e251809c 100644 --- a/px4_ros2_cpp/test/unit/modes.cpp +++ b/px4_ros2_cpp/test/unit/modes.cpp @@ -30,6 +30,7 @@ class TestMode : public px4_ros2::ModeBase { setSkipMessageCompatibilityCheck(); overrideRegistration(std::make_shared(node)); + setSkipSetpointCheck(); } private: @@ -43,17 +44,21 @@ TEST(modes, modeRequirements) rclcpp::Node node("test_node"); auto mode = std::make_shared(node); EXPECT_TRUE(mode->doRegister()); - EXPECT_TRUE(mode->modeRequirements().angular_velocity); + EXPECT_TRUE(mode->modeRequirements().manual_control); + EXPECT_TRUE(mode->modeRequirements().global_position); + // Requirements from the setpoint are not set as we do not simulate the setpoint reply + // (setSkipSetpointCheck()) mode->modeRequirements().clearAll(); - EXPECT_FALSE(mode->modeRequirements().angular_velocity); + EXPECT_FALSE(mode->modeRequirements().manual_control); + EXPECT_FALSE(mode->modeRequirements().global_position); } TEST(modes, nodeWithMode) { auto node_with_mode = std::make_shared>("test_node"); - EXPECT_TRUE(node_with_mode->getMode().modeRequirements().angular_velocity); + EXPECT_TRUE(node_with_mode->getMode().modeRequirements().manual_control); node_with_mode->getMode().modeRequirements().clearAll(); - EXPECT_FALSE(node_with_mode->getMode().modeRequirements().angular_velocity); + EXPECT_FALSE(node_with_mode->getMode().modeRequirements().manual_control); }