diff --git a/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt b/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt index 69568d4e06..838331062a 100644 --- a/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt +++ b/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt @@ -22,6 +22,7 @@ set(HEADER_FILES include/px4_sdk/components/health_and_arming_checks.h include/px4_sdk/components/mode.h include/px4_sdk/components/mode_executor.h + include/px4_sdk/components/overrides.h include/px4_sdk/components/setpoints.h include/px4_sdk/components/wait_for_fmu.h ) @@ -30,6 +31,7 @@ add_library(px4_sdk_lib src/components/health_and_arming_checks.cpp src/components/mode.cpp src/components/mode_executor.cpp + src/components/overrides.cpp src/components/registration.cpp src/components/setpoints.cpp src/components/wait_for_fmu.cpp @@ -86,6 +88,12 @@ if(BUILD_TESTING) target_include_directories(test_mode_executor PRIVATE include) endif() + ament_add_gtest(test_overrides test/integration/overrides.cpp) + if(TARGET test_overrides) + target_link_libraries(test_overrides ${PROJECT_NAME} integration_utils) + target_include_directories(test_overrides PRIVATE include) + endif() + endif() ament_package() diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h index 1654e5758d..72b723a04f 100644 --- a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h +++ b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h @@ -40,6 +40,7 @@ #include #include "health_and_arming_checks.h" #include "setpoints.h" +#include "overrides.h" class Registration; struct RegistrationSettings; @@ -153,10 +154,13 @@ public: SetpointSender &setpoints() { return _setpoint_sender; } + ConfigOverrides &configOverrides() { return _config_overrides; } + private: friend class ModeExecutorBase; void overrideRegistration(const std::shared_ptr ®istration); RegistrationSettings getRegistrationSettings() const; + void onRegistered(); void unsubscribeVehicleStatus(); void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate = false); @@ -183,6 +187,8 @@ private: float _setpoint_update_rate_hz{0.f}; rclcpp::TimerBase::SharedPtr _setpoint_update_timer; SetpointSender _setpoint_sender; + + ConfigOverrides _config_overrides; }; } /* namespace px4_sdk */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h index 36ae6af039..c5978bb5fe 100644 --- a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h +++ b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h @@ -34,6 +34,7 @@ #pragma once #include "mode.h" +#include "overrides.h" #include #include @@ -84,6 +85,11 @@ public: */ virtual void onDeactivate(DeactivateReason reason) = 0; + /** + * Called when failsafes are currently being deferred (see @deferFailsafes), and the FMU wants to trigger + * a failsafe. + */ + virtual void onFailsafeDeferred() {} /** * Send command and wait for ack/nack @@ -116,6 +122,22 @@ public: rclcpp::Node &node() { return _node; } + ConfigOverrides &configOverrides() { return _config_overrides; } + + /** + * Enable/disable deferring failsafes. While enabled (and the executor is in charge), + * most failsafes are prevented from being triggered until the given timeout is exceeded. + * Some failsafes that cannot be prevented: + * - vehicle exceeds attitude limits (can be disabled via parameters) + * - the mode cannot run (some mode requirements are not met, such as no position estimate) + * + * If the executor is in charge, this method will wait for the FMU for acknowledgement (to avoid race conditions) + * @param enabled + * @param timeout_s 0=system default, -1=no timeout + * @return true on success + */ + bool deferFailsafesSync(bool enabled, int timeout_s = 0); + private: class ScheduledMode @@ -151,6 +173,8 @@ private: RunCheckCallback _run_check_callback; }; + void onRegistered(); + void callOnActivate(); void callOnDeactivate(DeactivateReason reason); @@ -176,6 +200,9 @@ private: bool _is_armed{false}; bool _was_never_activated{true}; ModeBase::ID_t _prev_nav_state{ModeBase::ID_INVALID}; + uint8_t _prev_failsafe_defer_state{px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED}; + + ConfigOverrides _config_overrides; }; } /* namespace px4_sdk */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/overrides.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/overrides.h new file mode 100644 index 0000000000..91dd10878c --- /dev/null +++ b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/overrides.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +namespace px4_sdk +{ + +class ModeBase; +class ModeExecutorBase; + +class ConfigOverrides +{ +public: + ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix = ""); + + void controlAutoDisarm(bool enabled); + + +private: + void update(); + + friend class ModeBase; + friend class ModeExecutorBase; + void deferFailsafes(bool enabled, int timeout_s = 0); + void setup(uint8_t type, uint8_t id); + + rclcpp::Node &_node; + + px4_msgs::msg::ConfigOverrides _current_overrides{}; + rclcpp::Publisher::SharedPtr _config_overrides_pub; + bool _is_setup{false}; + bool _require_update_after_setup{false}; +}; + +} /* namespace px4_sdk */ diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp index 0347945960..4abc512b0c 100644 --- a/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp +++ b/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp @@ -45,7 +45,7 @@ ModeBase::ModeBase(rclcpp::Node &node, const ModeBase::Settings &settings, : _node(node), _registration(std::make_shared(node, topic_namespace_prefix)), _settings(settings), _health_and_arming_checks(node, std::bind(&ModeBase::checkArmingAndRunConditions, this, std::placeholders::_1), topic_namespace_prefix), - _setpoint_sender(node, *this, topic_namespace_prefix) + _setpoint_sender(node, *this, topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix) { _vehicle_status_sub = _node.create_subscription( topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), @@ -77,7 +77,13 @@ bool ModeBase::doRegister() assert(!_registration->registered()); _health_and_arming_checks.overrideRegistration(_registration); RegistrationSettings settings = getRegistrationSettings(); - return _registration->doRegister(settings); + bool ret = _registration->doRegister(settings); + + if (ret) { + onRegistered(); + } + + return ret; } RegistrationSettings ModeBase::getRegistrationSettings() const @@ -178,3 +184,8 @@ void ModeBase::completed(Result result) _mode_completed_pub->publish(mode_completed); _completed = true; } + +void ModeBase::onRegistered() +{ + _config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE, _registration->modeId()); +} diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp index a36f6b6c61..93a6a9f327 100644 --- a/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp +++ b/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp @@ -45,7 +45,8 @@ ModeExecutorBase::ModeExecutorBase(rclcpp::Node &node, const ModeExecutorBase::S ModeBase &owned_mode, const std::string &topic_namespace_prefix) : _node(node), _settings(settings), _owned_mode(owned_mode), _registration(std::make_shared(node, topic_namespace_prefix)), - _current_scheduled_mode(node, topic_namespace_prefix) + _current_scheduled_mode(node, topic_namespace_prefix), + _config_overrides(node, topic_namespace_prefix) { _vehicle_status_sub = _node.create_subscription( topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(), @@ -75,7 +76,19 @@ bool ModeExecutorBase::doRegister() RegistrationSettings settings = _owned_mode.getRegistrationSettings(); settings.register_mode_executor = true; settings.activate_mode_immediately = _settings.activate_immediately; - return _registration->doRegister(settings); + bool ret = _registration->doRegister(settings); + + if (ret) { + _owned_mode.onRegistered(); + onRegistered(); + } + + return ret; +} + +void ModeExecutorBase::onRegistered() +{ + _config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE_EXECUTOR, _registration->modeExecutorId()); } void ModeExecutorBase::callOnActivate() @@ -297,6 +310,15 @@ void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus:: callOnActivate(); } + + if (_is_in_charge && _prev_failsafe_defer_state != msg->failsafe_defer_state + && msg->failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE) { + // FMU wants to failsafe, but got deferred -> notify the executor + onFailsafeDeferred(); + } + + _prev_failsafe_defer_state = msg->failsafe_defer_state; + // Do not activate the mode if we're scheduling another mode. This is only expected to happen for a brief moment, // e.g. when the executor gets activated or right after arming. It thus prevents unnecessary mode activation toggling. bool do_not_activate_mode = (_current_scheduled_mode.active() && _owned_mode.id() != _current_scheduled_mode.modeId()) @@ -309,6 +331,55 @@ void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus:: _current_wait_vehicle_status.update(msg); } +bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s) +{ + _config_overrides.deferFailsafes(enabled, timeout_s); + + // To avoid race conditions we wait until the FMU sets it if the executor is in charge + if (enabled && _is_in_charge && _registration->registered() + && _prev_failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(_vehicle_status_sub); + + bool got_message = false; + auto start_time = _node.now(); + const rclcpp::Duration timeout = 1s; + + while (!got_message) { + auto now = _node.now(); + + if (now >= start_time + timeout) { + break; + } + + auto wait_ret = wait_set.wait((timeout - (now - start_time)).to_chrono()); + + if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) { + px4_msgs::msg::VehicleStatus msg; + rclcpp::MessageInfo info; + + if (_vehicle_status_sub->take(msg, info)) { + if (msg.failsafe_defer_state != px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) { + got_message = true; + } + + } else { + RCLCPP_DEBUG(_node.get_logger(), "no message received"); + } + + } else { + RCLCPP_DEBUG(_node.get_logger(), "timeout"); + } + } + + wait_set.remove_subscription(_vehicle_status_sub); + + return got_message; + } + + return true; +} + ModeExecutorBase::ScheduledMode::ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix) { _mode_completed_sub = node.create_subscription( diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/overrides.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/overrides.cpp new file mode 100644 index 0000000000..8d676b2d5d --- /dev/null +++ b/src/ros2/cpp/px4_sdk_lib/src/components/overrides.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "px4_sdk/components/overrides.h" + +#include + +using namespace px4_sdk; + +ConfigOverrides::ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix) + : _node(node) +{ + _config_overrides_pub = _node.create_publisher( + topic_namespace_prefix + "/fmu/in/config_overrides_request", 1); +} + +void ConfigOverrides::controlAutoDisarm(bool enabled) +{ + _current_overrides.disable_auto_disarm = !enabled; + update(); +} + +void ConfigOverrides::deferFailsafes(bool enabled, int timeout_s) +{ + _current_overrides.defer_failsafes = enabled; + _current_overrides.defer_failsafes_timeout_s = timeout_s; + update(); +} + +void ConfigOverrides::update() +{ + if (_is_setup) { + _current_overrides.timestamp = _node.get_clock()->now().nanoseconds() / 1000; + _config_overrides_pub->publish(_current_overrides); + + } else { + _require_update_after_setup = true; + } +} + +void ConfigOverrides::setup(uint8_t type, uint8_t id) +{ + assert(!_is_setup); + _current_overrides.source_type = type; + _current_overrides.source_id = id; + _is_setup = true; + + if (_require_update_after_setup) { + update(); + _require_update_after_setup = false; + } +} diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/overrides.cpp b/src/ros2/cpp/px4_sdk_lib/test/integration/overrides.cpp new file mode 100644 index 0000000000..8974f19c12 --- /dev/null +++ b/src/ros2/cpp/px4_sdk_lib/test/integration/overrides.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "util.h" + +#include +#include + +#include + +#include + +using namespace px4_sdk; +using namespace std; +using namespace std::chrono_literals; + +static const char *name = "Test Flight Mode"; + + +class Tester : public ::testing::Test +{ +public: + static void SetUpTestSuite() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestSuite() + { + rclcpp::shutdown(); + } +}; + +class FlightModeTest : public ModeBase +{ +public: + FlightModeTest(rclcpp::Node &node) + : ModeBase(node, Settings{name, false}, ModeRequirements::autonomous()) + { + setSetpointUpdateRate(30.f); + } + + virtual ~FlightModeTest() = default; + + void onActivate() override + { + _activation_time = node().get_clock()->now(); + setpoints().configureSetpointsSync(SetpointSender::SetpointConfiguration{}); + ++num_activations; + } + + void checkArmingAndRunConditions(HealthAndArmingCheckReporter &reporter) override + { + ++num_arming_check_updates; + } + + void onDeactivate() override + { + ++num_deactivations; + } + + void updateSetpoint() override + { + ++num_setpoint_updates; + rclcpp::Time now = node().get_clock()->now(); + + if (now - _activation_time > rclcpp::Duration::from_seconds(8)) { + completed(Result::Success); + return; + } + + // Send some random setpoints + float elapsed_s = (now - _activation_time).seconds(); + Eigen::Vector3f velocity{5.f, elapsed_s, 0.f }; + setpoints().sendTrajectorySetpoint(velocity); + } + + int num_activations{0}; + int num_deactivations{0}; + int num_setpoint_updates{0}; + int num_arming_check_updates{0}; +private: + rclcpp::Time _activation_time{}; +}; + +class ModeExecutorTest : public ModeExecutorBase +{ +public: + ModeExecutorTest(rclcpp::Node &node, FlightModeTest &owned_mode, bool activate_immediately) + : ModeExecutorBase(node, ModeExecutorBase::Settings{activate_immediately}, owned_mode), + _node(node) + {} + + enum class State { + Reset = 0, + WaitForArming = 1, + Arming = 2, + TakingOff = 3, + Landing = 4, + Wait = 5, + TakingOff2 = 6, + WaitForFailsafe = 7, + WaitForDisarming = 8, + }; + + void onActivate() override + { + ++num_activations; + runState(State::WaitForArming, Result::Success); + } + + void onDeactivate(DeactivateReason reason) override + { + EXPECT_EQ(_state, State::WaitForDisarming); + ++num_deactivations; + } + + void onFailsafeDeferred() override + { + ++num_failsafe_deferred; + EXPECT_EQ(_state, State::WaitForFailsafe); + // Re-enable failsafes -> failsafe will trigger and executor deactivated + deferFailsafesSync(false); + runState(State::WaitForDisarming, Result::Success); + } + + void runState(State state, Result previous_result) + { + on_state_completed(state, previous_result); + + if (previous_result != Result::Success) { + RCLCPP_ERROR(_node.get_logger(), "State %i: previous state failed: %s", (int)state, resultToString(previous_result)); + return; + } + + RCLCPP_DEBUG(_node.get_logger(), "Executing state %i", (int)state); + + _state = state; + + switch (state) { + case State::Reset: + break; + + case State::WaitForArming: + waitReadyToArm([this](Result result) { runState(State::Arming, result); }); + break; + + case State::Arming: + arm([this](Result result) { runState(State::TakingOff, result); }); + break; + + case State::TakingOff: + takeoff([this](Result result) { runState(State::Landing, result); }); + break; + + case State::Landing: + land([this](Result result) { runState(State::Wait, result); }); + break; + + case State::Wait: + _wait_timer = _node.create_wall_timer(10s, [this] { + _wait_timer.reset(); + EXPECT_TRUE(isArmed()); + runState(State::TakingOff2, Result::Success); + }); + break; + + case State::TakingOff2: + takeoff([this](Result result) { + EXPECT_TRUE(deferFailsafesSync(true, 60)); + runState(State::WaitForFailsafe, result); + }); + break; + + case State::WaitForFailsafe: + // Nothing to do + break; + + case State::WaitForDisarming: + // Nothing to do + break; + + } + } + + State getState() const { return _state; } + + int num_activations{0}; + int num_deactivations{0}; + int num_failsafe_deferred{0}; + + std::function on_completed; + std::function on_state_completed; + +private: + State _state{}; + rclcpp::Node &_node; + rclcpp::TimerBase::SharedPtr _wait_timer; +}; + + +class TestExecutionOverrides +{ +public: + TestExecutionOverrides(rclcpp::Node &node) + : _node(node), _vehicle_state(node) {} + + void run(); +private: + rclcpp::Node &_node; + rclcpp::TimerBase::SharedPtr _test_timeout; + + std::unique_ptr _mode; + std::unique_ptr _mode_executor; + VehicleState _vehicle_state; + bool _was_armed{false}; +}; + +void TestExecutionOverrides::run() +{ + _test_timeout = _node.create_wall_timer(80s, [] { + EXPECT_TRUE(false); // Timed out + rclcpp::shutdown(); + }); + + _mode = std::make_unique(_node); + _mode_executor = std::make_unique(_node, *_mode.get(), true); + + + // Testing steps: + // - disable auto-disarm + // - register & activate executor + // - takeoff + // - land & ensure no auto-disarm triggered + // - takeoff + // - defer failsafes & trigger failsafe + // - wait for callback & enable failsafes + // - wait for failsafe triggering, landing and disarming + + _mode_executor->on_state_completed = [this](ModeExecutorTest::State next_state, Result result) { + if (next_state == ModeExecutorTest::State::WaitForFailsafe) { + EXPECT_EQ(result, Result::Success); + // trigger failsafe (one that can be deferred) + _vehicle_state.setForceLowBattery(true); + + } else { + EXPECT_EQ(result, Result::Success); + } + }; + + _vehicle_state.setOnVehicleStatusUpdate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { + bool armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED; + + if (_was_armed && !armed) { + // Finish the test + _vehicle_state.setForceLowBattery(false); + + EXPECT_EQ(_mode_executor->getState(), ModeExecutorTest::State::WaitForDisarming); + EXPECT_EQ(_mode_executor->num_activations, 1); + EXPECT_EQ(_mode_executor->num_deactivations, 1); + EXPECT_EQ(_mode_executor->num_failsafe_deferred, 1); + + rclcpp::shutdown(); + } + + _was_armed = armed; + }); + + _mode_executor->configOverrides().controlAutoDisarm(false); + ASSERT_TRUE(_mode_executor->doRegister()); +} + +TEST_F(Tester, run_executor_overrides) +{ + auto test_node = initNode(); + ASSERT_TRUE(waitForFMU(*test_node, 10s)); + TestExecutionOverrides test_execution{*test_node}; + test_execution.run(); + rclcpp::spin(test_node); +} +