Skip to content

Commit ac68a06

Browse files
bkuengbkueng
committed
mission: add more example actions
1 parent 9dd8d56 commit ac68a06

File tree

6 files changed

+288
-49
lines changed

6 files changed

+288
-49
lines changed

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

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
/****************************************************************************
2+
* Copyright (c) 2025 PX4 Development Team.
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
****************************************************************************/
5+
#pragma once
6+
7+
#include <px4_ros2/mission/mission_executor.hpp>
8+
#include <px4_ros2/third_party/nlohmann/json.hpp>
9+
10+
class BasicCustomAction : public px4_ros2::ActionInterface
11+
{
12+
public:
13+
explicit BasicCustomAction(px4_ros2::ModeBase & mode)
14+
: _node(mode.node())
15+
{
16+
}
17+
std::string name() const override {return "basicCustomAction";}
18+
19+
void run(
20+
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
21+
const px4_ros2::ActionArguments & arguments,
22+
const std::function<void()> & on_completed) override
23+
{
24+
RCLCPP_INFO(_node.get_logger(), "Running custom action");
25+
if (arguments.contains("customArgument")) {
26+
RCLCPP_INFO(
27+
_node.get_logger(), "Custom argument: %s",
28+
arguments.at<std::string>("customArgument").c_str());
29+
}
30+
// Directly continue
31+
on_completed();
32+
}
33+
34+
private:
35+
rclcpp::Node & _node;
36+
};

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

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
/****************************************************************************
2+
* Copyright (c) 2025 PX4 Development Team.
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
****************************************************************************/
5+
#pragma once
6+
7+
#include <px4_ros2/mission/mission_executor.hpp>
8+
#include <px4_ros2/third_party/nlohmann/json.hpp>
9+
10+
/**
11+
* @brief Example camera trigger action
12+
*
13+
* This is an example for a continuous action, i.e. an action that is ongoing in the background while the mission
14+
* continues.
15+
* By storing the state of the action while it is running, the state is restored when the mission is interrupted
16+
* and resumed later on.
17+
*/
18+
class CameraTriggerAction : public px4_ros2::ActionInterface
19+
{
20+
public:
21+
explicit CameraTriggerAction(px4_ros2::ModeBase & mode)
22+
: _node(mode.node()) {}
23+
std::string name() const override {return "cameraTrigger";}
24+
25+
bool canRun(
26+
const px4_ros2::ActionArguments & arguments,
27+
std::vector<std::string> & errors) override
28+
{
29+
// Require an 'action' argument
30+
if (!arguments.contains("action")) {
31+
errors.push_back("cameraTrigger action must have 'action' argument");
32+
return false;
33+
}
34+
return true;
35+
}
36+
37+
void run(
38+
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
39+
const px4_ros2::ActionArguments & arguments,
40+
const std::function<void()> & on_completed) override
41+
{
42+
const auto action = arguments.at<std::string>("action");
43+
if (action == "start") {
44+
_state = handler->storeState(name(), arguments);
45+
startCameraTrigger();
46+
} else {
47+
_state.reset();
48+
stopCameraTrigger();
49+
}
50+
51+
on_completed();
52+
}
53+
54+
void deactivate() override
55+
{
56+
if (_state != nullptr) {
57+
_state.reset();
58+
stopCameraTrigger();
59+
}
60+
}
61+
62+
private:
63+
void startCameraTrigger()
64+
{
65+
RCLCPP_INFO(_node.get_logger(), "Starting camera trigger");
66+
}
67+
void stopCameraTrigger()
68+
{
69+
RCLCPP_INFO(_node.get_logger(), "Stopping camera trigger");
70+
}
71+
std::unique_ptr<px4_ros2::ActionStateKeeper> _state;
72+
rclcpp::Node & _node;
73+
};

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

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
/****************************************************************************
2+
* Copyright (c) 2025 PX4 Development Team.
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
****************************************************************************/
5+
#pragma once
6+
7+
#include <px4_ros2/components/mode.hpp>
8+
#include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
9+
#include <px4_ros2/mission/mission_executor.hpp>
10+
#include <px4_ros2/third_party/nlohmann/json.hpp>
11+
12+
class FlightModeTest : public px4_ros2::ModeBase
13+
{
14+
public:
15+
explicit FlightModeTest(rclcpp::Node & node)
16+
: ModeBase(node, Settings{"My Mode"})
17+
{
18+
_trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
19+
}
20+
21+
void onActivate() override
22+
{
23+
_duration = 0.0f;
24+
}
25+
26+
void onDeactivate() override {}
27+
28+
void updateSetpoint(float dt_s) override
29+
{
30+
_duration += dt_s;
31+
if (_duration > 5.0f) {
32+
completed(px4_ros2::Result::Success);
33+
}
34+
// Send a fixed velocity setpoint
35+
_trajectory_setpoint->update(Eigen::Vector3f{2.0f, 3.0f, 0.0f});
36+
}
37+
38+
private:
39+
std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
40+
float _duration{0.0f};
41+
};
42+
43+
class ModeAction : public px4_ros2::ActionInterface
44+
{
45+
public:
46+
explicit ModeAction(px4_ros2::ModeBase & mode)
47+
: _node(mode.node())
48+
{
49+
_mode = std::make_shared<FlightModeTest>(_node);
50+
51+
if (!_mode->doRegister()) {
52+
throw std::runtime_error("Registration failed");
53+
}
54+
}
55+
std::string name() const override {return "myMode";}
56+
57+
void run(
58+
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
59+
const px4_ros2::ActionArguments & arguments,
60+
const std::function<void()> & on_completed) override
61+
{
62+
RCLCPP_INFO(_node.get_logger(), "Running custom mode");
63+
handler->runMode(_mode->id(), on_completed);
64+
}
65+
66+
private:
67+
rclcpp::Node & _node;
68+
std::shared_ptr<px4_ros2::ModeBase> _mode;
69+
};

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

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
/****************************************************************************
2+
* Copyright (c) 2025 PX4 Development Team.
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
****************************************************************************/
5+
#pragma once
6+
7+
#include <px4_ros2/mission/mission_executor.hpp>
8+
#include <px4_ros2/vehicle_state/land_detected.hpp>
9+
#include <px4_ros2/third_party/nlohmann/json.hpp>
10+
11+
class PickupAction : public px4_ros2::ActionInterface
12+
{
13+
public:
14+
explicit PickupAction(px4_ros2::ModeBase & mode)
15+
: _node(mode.node())
16+
{
17+
_land_detected = std::make_shared<px4_ros2::LandDetected>(mode);
18+
}
19+
std::string name() const override {return "pickup";}
20+
21+
bool supportsResumeFromLanded() override {return true;}
22+
23+
void run(
24+
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
25+
const px4_ros2::ActionArguments & arguments,
26+
const std::function<void()> & on_completed) override
27+
{
28+
// If currently landed, we proceed with the takeoff.
29+
// If that is interrupted, we don't want to land again, so we keep track of this state.
30+
// If landing gets interrupted, we still want to land again when resuming.
31+
const bool landed = _land_detected->lastValid(3s) && _land_detected->landed();
32+
const bool resuming_from_landed = arguments.resuming() && _continuing_from_landed;
33+
34+
if (landed || resuming_from_landed) {
35+
_continuing_from_landed = true;
36+
37+
const auto continue_after_takeoff = [handler, on_completed] {
38+
handler->controlAutoSetHome(true);
39+
handler->runAction("hold", px4_ros2::ActionArguments{{{"delay", 5}}}, on_completed);
40+
};
41+
42+
if (resuming_from_landed) {
43+
RCLCPP_INFO(_node.get_logger(), "Resuming pickup action");
44+
continue_after_takeoff();
45+
} else {
46+
RCLCPP_INFO(_node.get_logger(), "Running pickup action from landed");
47+
const float altitude =
48+
arguments.contains("altitude") ? arguments.at<float>("altitude") : NAN;
49+
handler->runModeTakeoff(altitude, NAN, continue_after_takeoff);
50+
}
51+
52+
} else {
53+
_continuing_from_landed = false;
54+
RCLCPP_INFO(_node.get_logger(), "Running pickup action (in-air)");
55+
56+
// Go to Hold for a few seconds, then land
57+
handler->runAction(
58+
"hold", px4_ros2::ActionArguments{{{"delay", 5}}}, [handler]
59+
{
60+
// Prevent PX4 from updating the home position on the next takeoff
61+
handler->controlAutoSetHome(false);
62+
// The vehicle disarms after landing, and we don't want to continue with the mission immediately
63+
handler->runMode(px4_ros2::ModeBase::kModeIDLand, [] {});
64+
});
65+
}
66+
}
67+
68+
private:
69+
rclcpp::Node & _node;
70+
std::shared_ptr<px4_ros2::LandDetected> _land_detected;
71+
bool _continuing_from_landed{false};
72+
};

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

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
/****************************************************************************
2+
* Copyright (c) 2025 PX4 Development Team.
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
****************************************************************************/
5+
#pragma once
6+
7+
#include <px4_ros2/mission/mission_executor.hpp>
8+
#include <px4_ros2/third_party/nlohmann/json.hpp>
9+
10+
class CustomTrajectoryAction : public px4_ros2::ActionInterface
11+
{
12+
public:
13+
explicit CustomTrajectoryAction(px4_ros2::ModeBase & mode) {}
14+
std::string name() const override {return "customTrajectory";}
15+
16+
void run(
17+
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
18+
const px4_ros2::ActionArguments & arguments,
19+
const std::function<void()> & on_completed) override
20+
{
21+
// Run a custom trajectory
22+
auto mission = std::make_shared<px4_ros2::Mission>(
23+
std::vector<px4_ros2::MissionItem>{px4_ros2::Waypoint({47.39820919, 8.54595214, 500}),
24+
px4_ros2::Waypoint({47.39820919, 8.54595214, 510})});
25+
handler->runTrajectory(mission, on_completed);
26+
}
27+
};

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

Lines changed: 11 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -7,54 +7,13 @@
77
#include <px4_ros2/mission/mission_executor.hpp>
88
#include <px4_ros2/third_party/nlohmann/json.hpp>
99

10-
static const std::string kName = "Mission Example";
11-
12-
class CustomAction : public px4_ros2::ActionInterface
13-
{
14-
public:
15-
explicit CustomAction(px4_ros2::ModeBase & mode)
16-
: _node(mode.node())
17-
{
18-
}
19-
std::string name() const override {return "customAction";}
20-
21-
void run(
22-
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
23-
const px4_ros2::ActionArguments & arguments,
24-
const std::function<void()> & on_completed) override
25-
{
26-
RCLCPP_INFO(_node.get_logger(), "Running custom action");
27-
if (arguments.contains("customArgument")) {
28-
RCLCPP_INFO(
29-
_node.get_logger(), "Custom argument: %s",
30-
arguments.at<std::string>("customArgument").c_str());
31-
}
32-
// Directly continue
33-
on_completed();
34-
}
10+
#include "actions/basic.hpp"
11+
#include "actions/camera_trigger.hpp"
12+
#include "actions/mode.hpp"
13+
#include "actions/pickup.hpp"
14+
#include "actions/trajectory.hpp"
3515

36-
private:
37-
rclcpp::Node & _node;
38-
};
39-
40-
class CustomTrajectoryAction : public px4_ros2::ActionInterface
41-
{
42-
public:
43-
explicit CustomTrajectoryAction(px4_ros2::ModeBase & mode) {}
44-
std::string name() const override {return "customTrajectory";}
45-
46-
void run(
47-
const std::shared_ptr<px4_ros2::ActionHandler> & handler,
48-
const px4_ros2::ActionArguments & arguments,
49-
const std::function<void()> & on_completed) override
50-
{
51-
// Run a custom trajectory
52-
auto mission = std::make_shared<px4_ros2::Mission>(
53-
std::vector<px4_ros2::MissionItem>{px4_ros2::Waypoint({47.39820919, 8.54595214, 500}),
54-
px4_ros2::Waypoint({47.39820919, 8.54595214, 510})});
55-
handler->runTrajectory(mission, on_completed);
56-
}
57-
};
16+
static const std::string kName = "Mission Example";
5817

5918
class Mission
6019
{
@@ -94,7 +53,7 @@ class Mission
9453
"frame": "global"
9554
},
9655
{
97-
"type": "customAction",
56+
"type": "basicCustomAction",
9857
"customArgument": "custom value"
9958
},
10059
{
@@ -133,7 +92,10 @@ class Mission
13392
)"));
13493
_mission_executor = std::make_unique<px4_ros2::MissionExecutor>(
13594
kName,
136-
px4_ros2::MissionExecutor::Configuration().addCustomAction<CustomAction>()
95+
px4_ros2::MissionExecutor::Configuration().addCustomAction<BasicCustomAction>()
96+
.addCustomAction<CameraTriggerAction>()
97+
.addCustomAction<ModeAction>()
98+
.addCustomAction<PickupAction>()
13799
.addCustomAction<CustomTrajectoryAction>(),
138100
*node);
139101

0 commit comments

Comments
 (0)