Extend default action for takeoff with a takeoff altitude

examples/cpp/modes/mission/include/actions/basic.hpp

@@ -0,0 +1,36 @@
+/****************************************************************************
+* Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/mission/mission_executor.hpp>
+#include <px4_ros2/third_party/nlohmann/json.hpp>
+
+class BasicCustomAction : public px4_ros2::ActionInterface
+{
+public:
+  explicit BasicCustomAction(px4_ros2::ModeBase & mode)
+  : _node(mode.node())
+  {
+  }
+  std::string name() const override {return "basicCustomAction";}
+
+  void run(
+    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
+    const px4_ros2::ActionArguments & arguments,
+    const std::function<void()> & on_completed) override
+  {
+    RCLCPP_INFO(_node.get_logger(), "Running custom action");
+    if (arguments.contains("customArgument")) {
+      RCLCPP_INFO(
+        _node.get_logger(), "Custom argument: %s",
+        arguments.at<std::string>("customArgument").c_str());
+    }
+    // Directly continue
+    on_completed();
+  }
+
+private:
+  rclcpp::Node & _node;
+};

examples/cpp/modes/mission/include/actions/camera_trigger.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+* Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/mission/mission_executor.hpp>
+#include <px4_ros2/third_party/nlohmann/json.hpp>
+
+/**
+ * @brief Example camera trigger action
+ *
+ * This is an example for a continuous action, i.e. an action that is ongoing in the background while the mission
+ * continues.
+ * By storing the state of the action while it is running, the state is restored when the mission is interrupted
+ * and resumed later on.
+ */
+class CameraTriggerAction : public px4_ros2::ActionInterface
+{
+public:
+  explicit CameraTriggerAction(px4_ros2::ModeBase & mode)
+  : _node(mode.node()) {}
+  std::string name() const override {return "cameraTrigger";}
+
+  bool canRun(
+    const px4_ros2::ActionArguments & arguments,
+    std::vector<std::string> & errors) override
+  {
+    // Require an 'action' argument
+    if (!arguments.contains("action")) {
+      errors.emplace_back("cameraTrigger action must have 'action' argument");
+      return false;
+    }
+    return true;
+  }
+
+  void run(
+    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
+    const px4_ros2::ActionArguments & arguments,
+    const std::function<void()> & on_completed) override
+  {
+    const auto action = arguments.at<std::string>("action");
+    if (action == "start") {
+      _state = handler->storeState(name(), arguments);
+      startCameraTrigger();
+    } else {
+      _state.reset();
+      stopCameraTrigger();
+    }
+
+    on_completed();
+  }
+
+  void deactivate() override
+  {
+    if (_state != nullptr) {
+      _state.reset();
+      stopCameraTrigger();
+    }
+  }
+
+private:
+  void startCameraTrigger()
+  {
+    RCLCPP_INFO(_node.get_logger(), "Starting camera trigger");
+  }
+  void stopCameraTrigger()
+  {
+    RCLCPP_INFO(_node.get_logger(), "Stopping camera trigger");
+  }
+  std::unique_ptr<px4_ros2::ActionStateKeeper> _state;
+  rclcpp::Node & _node;
+};

examples/cpp/modes/mission/include/actions/mode.hpp

@@ -0,0 +1,69 @@
+/****************************************************************************
+* Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
+#include <px4_ros2/mission/mission_executor.hpp>
+#include <px4_ros2/third_party/nlohmann/json.hpp>
+
+class FlightModeTest : public px4_ros2::ModeBase
+{
+public:
+  explicit FlightModeTest(rclcpp::Node & node)
+  : ModeBase(node, Settings{"My Mode"})
+  {
+    _trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
+  }
+
+  void onActivate() override
+  {
+    _duration = 0.0f;
+  }
+
+  void onDeactivate() override {}
+
+  void updateSetpoint(float dt_s) override
+  {
+    _duration += dt_s;
+    if (_duration > 5.0f) {
+      completed(px4_ros2::Result::Success);
+    }
+    // Send a fixed velocity setpoint
+    _trajectory_setpoint->update(Eigen::Vector3f{2.0f, 3.0f, 0.0f});
+  }
+
+private:
+  std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+  float _duration{0.0f};
+};
+
+class ModeAction : public px4_ros2::ActionInterface
+{
+public:
+  explicit ModeAction(px4_ros2::ModeBase & mode)
+  : _node(mode.node())
+  {
+    _mode = std::make_shared<FlightModeTest>(_node);
+
+    if (!_mode->doRegister()) {
+      throw std::runtime_error("Registration failed");
+    }
+  }
+  std::string name() const override {return "myMode";}
+
+  void run(
+    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
+    const px4_ros2::ActionArguments & arguments,
+    const std::function<void()> & on_completed) override
+  {
+    RCLCPP_INFO(_node.get_logger(), "Running custom mode");
+    handler->runMode(_mode->id(), on_completed);
+  }
+
+private:
+  rclcpp::Node & _node;
+  std::shared_ptr<px4_ros2::ModeBase> _mode;
+};

examples/cpp/modes/mission/include/actions/pickup.hpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+* Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/mission/mission_executor.hpp>
+#include <px4_ros2/vehicle_state/land_detected.hpp>
+#include <px4_ros2/third_party/nlohmann/json.hpp>
+
+class PickupAction : public px4_ros2::ActionInterface
+{
+public:
+  explicit PickupAction(px4_ros2::ModeBase & mode)
+  : _node(mode.node())
+  {
+    _land_detected = std::make_shared<px4_ros2::LandDetected>(mode);
+  }
+  std::string name() const override {return "pickup";}
+
+  bool supportsResumeFromLanded() override {return true;}
+
+  void run(
+    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
+    const px4_ros2::ActionArguments & arguments,
+    const std::function<void()> & on_completed) override
+  {
+    // If currently landed, we proceed with the takeoff.
+    // If that is interrupted, we don't want to land again, so we keep track of this state.
+    // If landing gets interrupted, we still want to land again when resuming.
+    const bool landed = _land_detected->lastValid(3s) && _land_detected->landed();
+    const bool resuming_from_landed = arguments.resuming() && _continuing_from_landed;
+
+    if (landed || resuming_from_landed) {
+      _continuing_from_landed = true;
+
+      const auto continue_after_takeoff = [handler, on_completed] {
+          handler->controlAutoSetHome(true);
+          handler->runAction("hold", px4_ros2::ActionArguments{{{"delay", 5}}}, on_completed);
+        };
+
+      if (resuming_from_landed) {
+        RCLCPP_INFO(_node.get_logger(), "Resuming pickup action");
+        continue_after_takeoff();
+      } else {
+        RCLCPP_INFO(_node.get_logger(), "Running pickup action from landed");
+        const float altitude =
+          arguments.contains("altitude") ? arguments.at<float>("altitude") : NAN;
+        handler->runModeTakeoff(altitude, NAN, continue_after_takeoff);
+      }
+
+    } else {
+      _continuing_from_landed = false;
+      RCLCPP_INFO(_node.get_logger(), "Running pickup action (in-air)");
+
+      // Go to Hold for a few seconds, then land
+      handler->runAction(
+        "hold", px4_ros2::ActionArguments{{{"delay", 5}}}, [handler]
+        {
+          // Prevent PX4 from updating the home position on the next takeoff
+          handler->controlAutoSetHome(false);
+          // The vehicle disarms after landing, and we don't want to continue with the mission immediately
+          handler->runMode(px4_ros2::ModeBase::kModeIDLand, [] {});
+        });
+    }
+  }
+
+private:
+  rclcpp::Node & _node;
+  std::shared_ptr<px4_ros2::LandDetected> _land_detected;
+  bool _continuing_from_landed{false};
+};

examples/cpp/modes/mission/include/actions/trajectory.hpp

@@ -0,0 +1,27 @@
+/****************************************************************************
+* Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/mission/mission_executor.hpp>
+#include <px4_ros2/third_party/nlohmann/json.hpp>
+
+class CustomTrajectoryAction : public px4_ros2::ActionInterface
+{
+public:
+  explicit CustomTrajectoryAction(px4_ros2::ModeBase & mode) {}
+  std::string name() const override {return "customTrajectory";}
+
+  void run(
+    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
+    const px4_ros2::ActionArguments & arguments,
+    const std::function<void()> & on_completed) override
+  {
+    // Run a custom trajectory
+    auto mission = std::make_shared<px4_ros2::Mission>(
+      std::vector<px4_ros2::MissionItem>{px4_ros2::Waypoint({47.39820919, 8.54595214, 500}),
+        px4_ros2::Waypoint({47.39820919, 8.54595214, 510})});
+    handler->runTrajectory(mission, on_completed);
+  }
+};

examples/cpp/modes/mission/include/mission.hpp

@@ -7,54 +7,13 @@
 #include <px4_ros2/mission/mission_executor.hpp>
 #include <px4_ros2/third_party/nlohmann/json.hpp>
 
-static const std::string kName = "Mission Example";
-
-class CustomAction : public px4_ros2::ActionInterface
-{
-public:
-  explicit CustomAction(px4_ros2::ModeBase & mode)
-  : _node(mode.node())
-  {
-  }
-  std::string name() const override {return "customAction";}
-
-  void run(
-    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
-    const px4_ros2::ActionArguments & arguments,
-    const std::function<void()> & on_completed) override
-  {
-    RCLCPP_INFO(_node.get_logger(), "Running custom action");
-    if (arguments.contains("customArgument")) {
-      RCLCPP_INFO(
-        _node.get_logger(), "Custom argument: %s",
-        arguments.at<std::string>("customArgument").c_str());
-    }
-    // Directly continue
-    on_completed();
-  }
+#include "actions/basic.hpp"
+#include "actions/camera_trigger.hpp"
+#include "actions/mode.hpp"
+#include "actions/pickup.hpp"
+#include "actions/trajectory.hpp"
 
-private:
-  rclcpp::Node & _node;
-};
-
-class CustomTrajectoryAction : public px4_ros2::ActionInterface
-{
-public:
-  explicit CustomTrajectoryAction(px4_ros2::ModeBase & mode) {}
-  std::string name() const override {return "customTrajectory";}
-
-  void run(
-    const std::shared_ptr<px4_ros2::ActionHandler> & handler,
-    const px4_ros2::ActionArguments & arguments,
-    const std::function<void()> & on_completed) override
-  {
-    // Run a custom trajectory
-    auto mission = std::make_shared<px4_ros2::Mission>(
-      std::vector<px4_ros2::MissionItem>{px4_ros2::Waypoint({47.39820919, 8.54595214, 500}),
-        px4_ros2::Waypoint({47.39820919, 8.54595214, 510})});
-    handler->runTrajectory(mission, on_completed);
-  }
-};
+static const std::string kName = "Mission Example";
 
 class Mission
 {
@@ -94,7 +53,7 @@ class Mission
                "frame": "global"
            },
            {
-               "type": "customAction",
+               "type": "basicCustomAction",
                "customArgument": "custom value"
            },
            {
@@ -133,7 +92,10 @@ class Mission
 )"));
     _mission_executor = std::make_unique<px4_ros2::MissionExecutor>(
       kName,
-      px4_ros2::MissionExecutor::Configuration().addCustomAction<CustomAction>()
+      px4_ros2::MissionExecutor::Configuration().addCustomAction<BasicCustomAction>()
+      .addCustomAction<CameraTriggerAction>()
+      .addCustomAction<ModeAction>()
+      .addCustomAction<PickupAction>()
       .addCustomAction<CustomTrajectoryAction>(),
       *node);
 

mission/schema.yaml

@@ -116,7 +116,11 @@ properties:
             - if:
                 properties: { type: { const: "takeoff" } }
               then:
-                properties: {}
+                properties:
+                  altitude:
+                    description: |
+                      optional ASML altitude in m. Default: vehicle-specific default altitude
+                    type: number
             - if:
                 properties: { type: { const: "rtl" } }
               then:

px4_ros2_cpp/include/px4_ros2/components/mode_executor.hpp

@@ -103,6 +103,12 @@ class ModeExecutorBase
     ModeBase::ModeID mode_id, const CompletedCallback & on_completed,
     bool forced = false);
 
+  /**
+   * @brief Trigger a takeoff
+   * @param on_completed callback to execute when the takeoff is completed
+   * @param altitude optional altitude AMSL [m]
+   * @param heading optional heading [rad] from North
+   */
   void takeoff(const CompletedCallback & on_completed, float altitude = NAN, float heading = NAN);
   void land(const CompletedCallback & on_completed);
   void rtl(const CompletedCallback & on_completed);