From 04e778c668bf19824af0ba7b69242e644ba07825 Mon Sep 17 00:00:00 2001 From: Damien SIX Date: Fri, 12 Jul 2024 19:44:57 +0200 Subject: [PATCH] refactor: use enum instead of multiple flags for activation --- .../include/px4_ros2/components/mode_executor.hpp | 9 +++++++-- px4_ros2_cpp/src/components/mode_executor.cpp | 9 ++++++--- px4_ros2_cpp/test/integration/mode_executor.cpp | 3 ++- px4_ros2_cpp/test/integration/overrides.cpp | 3 ++- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/components/mode_executor.hpp b/px4_ros2_cpp/include/px4_ros2/components/mode_executor.hpp index eda1ed47..1e534d27 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/mode_executor.hpp +++ b/px4_ros2_cpp/include/px4_ros2/components/mode_executor.hpp @@ -34,8 +34,13 @@ class ModeExecutorBase struct Settings { - bool activate_immediately{false}; ///< If set activate the mode (and executor) immediately. Only use this for fully autonomous executors that also arm the vehicle - bool is_allowed_to_arm{false}; ///< If set, the executor is allowed to arm the vehicle + enum class Activation + { + ActivateOnlyWhenArmed, ///< Only activate the executor when armed (and selected) + ActivateAlways, ///< Allow the executor to always be activated (so it can arm the vehicle) + ActivateImmediately, ///< Activate the mode and executor immediately after registration. Only use this for fully autonomous executors that also arm the vehicle + }; + Activation activation{Activation::ActivateOnlyWhenArmed}; }; enum class DeactivateReason diff --git a/px4_ros2_cpp/src/components/mode_executor.cpp b/px4_ros2_cpp/src/components/mode_executor.cpp index e3dabee8..a6cb2978 100644 --- a/px4_ros2_cpp/src/components/mode_executor.cpp +++ b/px4_ros2_cpp/src/components/mode_executor.cpp @@ -59,7 +59,8 @@ bool ModeExecutorBase::doRegister() _owned_mode.unsubscribeVehicleStatus(); RegistrationSettings settings = _owned_mode.getRegistrationSettings(); settings.register_mode_executor = true; - settings.activate_mode_immediately = _settings.activate_immediately; + settings.activate_mode_immediately = + (_settings.activation == Settings::Activation::ActivateImmediately); bool ret = _registration->doRegister(settings); if (ret) { @@ -289,9 +290,11 @@ void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus:: const bool was_armed = _is_armed; const ModeBase::ModeID current_mode = static_cast(msg->nav_state); _is_armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED; - const bool wants_to_activate_immediately = _settings.activate_immediately && _was_never_activated; + const bool wants_to_activate_immediately = + (_settings.activation == Settings::Activation::ActivateImmediately) && _was_never_activated; const bool is_in_charge = id() == msg->executor_in_charge && - (_is_armed || wants_to_activate_immediately || _settings.is_allowed_to_arm); + (_is_armed || wants_to_activate_immediately || + _settings.activation == Settings::Activation::ActivateAlways); const bool changed_to_armed = !was_armed && _is_armed; bool got_activated = false; diff --git a/px4_ros2_cpp/test/integration/mode_executor.cpp b/px4_ros2_cpp/test/integration/mode_executor.cpp index 144e7474..277d086f 100644 --- a/px4_ros2_cpp/test/integration/mode_executor.cpp +++ b/px4_ros2_cpp/test/integration/mode_executor.cpp @@ -80,7 +80,8 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase { public: ModeExecutorTest(rclcpp::Node & node, FlightModeTest & owned_mode, bool activate_immediately) - : ModeExecutorBase(node, ModeExecutorBase::Settings{activate_immediately}, owned_mode), + : ModeExecutorBase(node, ModeExecutorBase::Settings{Settings::Activation::ActivateImmediately}, + owned_mode), _node(node) {} diff --git a/px4_ros2_cpp/test/integration/overrides.cpp b/px4_ros2_cpp/test/integration/overrides.cpp index d9a2dc93..a629735a 100644 --- a/px4_ros2_cpp/test/integration/overrides.cpp +++ b/px4_ros2_cpp/test/integration/overrides.cpp @@ -79,7 +79,8 @@ class ModeExecutorTest : public px4_ros2::ModeExecutorBase { public: ModeExecutorTest(rclcpp::Node & node, FlightModeTest & owned_mode, bool activate_immediately) - : ModeExecutorBase(node, ModeExecutorBase::Settings{activate_immediately}, owned_mode), + : ModeExecutorBase(node, ModeExecutorBase::Settings{Settings::Activation::ActivateImmediately}, + owned_mode), _node(node) {}