feat: new setting to allow arming with executor (#45)

Auterion · Jul 16, 2024 · 0dc23b3 · 0dc23b3
1 parent 1ae6a38
commit 0dc23b3
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 6 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
@@ -34,7 +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
+    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
@@ -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<ModeBase::ModeID>(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);
+    (_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
@@ -80,7 +80,9 @@ 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{activate_immediately ? Settings::Activation::ActivateImmediately : Settings::Activation::ActivateOnlyWhenArmed},
+      owned_mode),
     _node(node)
   {}
 

diff --git a/px4_ros2_cpp/test/integration/overrides.cpp b/px4_ros2_cpp/test/integration/overrides.cpp
@@ -79,7 +79,9 @@ 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{activate_immediately ? Settings::Activation::ActivateImmediately : Settings::Activation::ActivateOnlyWhenArmed},
+      owned_mode),
     _node(node)
   {}