ROS2: add modes SDK

src/ros2/cpp/px4_sdk_lib/CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 3.5)
+project(px4_sdk_lib)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+	set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+	add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(px4_msgs REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(include ${Eigen3_INCLUDE_DIRS})
+set(HEADER_FILES
+		include/px4_sdk/components/events.h
+		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/setpoints.h
+		include/px4_sdk/components/wait_for_fmu.h
+	)
+
+add_library(px4_sdk_lib
+		src/components/health_and_arming_checks.cpp
+		src/components/mode.cpp
+		src/components/mode_executor.cpp
+		src/components/registration.cpp
+		src/components/setpoints.cpp
+		src/components/wait_for_fmu.cpp
+		${HEADER_FILES}
+	)
+ament_target_dependencies(px4_sdk_lib Eigen3 rclcpp px4_msgs)
+
+ament_export_targets(px4_sdk_lib HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+    px4_msgs
+    rclcpp
+	eigen3_cmake_module
+	Eigen3
+)
+install(
+	DIRECTORY include/px4_sdk
+	DESTINATION include
+)
+install(
+	TARGETS px4_sdk_lib
+	EXPORT px4_sdk_lib
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin
+	INCLUDES DESTINATION include
+)
+
+if(BUILD_TESTING)
+	find_package(ament_lint_auto REQUIRED)
+	find_package(ament_cmake_gtest REQUIRED)
+	ament_lint_auto_find_test_dependencies()
+
+	add_library(integration_utils
+			test/integration/util.cpp
+			)
+	include_directories(include)
+	ament_target_dependencies(integration_utils rclcpp px4_msgs)
+
+	ament_add_gtest(test_arming_check test/integration/arming_check.cpp)
+	if(TARGET test_arming_check)
+		target_link_libraries(test_arming_check ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_arming_check PRIVATE include)
+	endif()
+
+	ament_add_gtest(test_mode test/integration/mode.cpp)
+	if(TARGET test_mode)
+		target_link_libraries(test_mode ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_mode PRIVATE include)
+	endif()
+
+	ament_add_gtest(test_mode_executor test/integration/mode_executor.cpp)
+	if(TARGET test_mode_executor)
+		target_link_libraries(test_mode_executor ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_mode_executor PRIVATE include)
+	endif()
+
+endif()
+
+ament_package()

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/events.h

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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 <px4_msgs/msg/arming_check_reply.hpp>
+
+namespace events
+{
+
+using EventType = px4_msgs::msg::Event;
+
+enum class LogLevel : uint8_t {
+	Emergency = 0,
+	Alert = 1,
+	Critical = 2,
+	Error = 3,
+	Warning = 4,
+	Notice = 5,
+	Info = 6,
+	Debug = 7,
+	Protocol = 8,
+	Disabled = 9,
+
+	Count
+};
+
+enum class LogLevelInternal : uint8_t {
+	Emergency = 0,
+	Alert = 1,
+	Critical = 2,
+	Error = 3,
+	Warning = 4,
+	Notice = 5,
+	Info = 6,
+	Debug = 7,
+	Protocol = 8,
+	Disabled = 9,
+
+	Count
+};
+
+using Log = LogLevel;
+using LogInternal = LogLevelInternal;
+
+struct LogLevels {
+	LogLevels() {}
+	LogLevels(Log external_level) : external(external_level), internal((LogInternal)external_level) {}
+	LogLevels(Log external_level, LogInternal internal_level)
+		: external(external_level), internal(internal_level) {}
+
+	Log external{Log::Info};
+	LogInternal internal{LogInternal::Info};
+};
+
+namespace util
+{
+
+// source: https://gist.github.com/ruby0x1/81308642d0325fd386237cfa3b44785c
+constexpr uint32_t val_32_const = 0x811c9dc5;
+constexpr uint32_t prime_32_const = 0x1000193;
+constexpr uint64_t val_64_const = 0xcbf29ce484222325;
+constexpr uint64_t prime_64_const = 0x100000001b3;
+inline constexpr uint32_t hash_32_fnv1a_const(const char *const str, const uint32_t value = val_32_const) noexcept
+{
+	return (str[0] == '\0') ? value : hash_32_fnv1a_const(&str[1], (value ^ uint32_t(str[0])) * prime_32_const);
+}
+
+template<typename T>
+inline constexpr void fillEventArguments(uint8_t *buf, T arg)
+{
+	// This assumes we're on little-endian
+	memcpy(buf, &arg, sizeof(T));
+}
+
+template<typename T, typename... Args>
+inline constexpr void fillEventArguments(uint8_t *buf, T arg, Args... args)
+{
+	fillEventArguments(buf, arg);
+	fillEventArguments(buf + sizeof(T), args...);
+}
+
+constexpr unsigned sizeofArguments() { return 0; }
+
+template <typename T, typename... Args>
+constexpr unsigned sizeofArguments(const T &t, const Args &... args)
+{
+	return sizeof(T) + sizeofArguments(args...);
+}
+
+} // namespace util
+
+/**
+ * Generate event ID from an event name
+ */
+template<size_t N>
+constexpr uint32_t ID(const char (&name)[N])
+{
+	// Note: the generated ID must match with the python generator under Tools/px4events
+	uint32_t component_id = 1u << 24; // autopilot component
+	return (0xffffff & util::hash_32_fnv1a_const(name)) | component_id;
+}
+
+
+} // namespace events
+

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/health_and_arming_checks.h

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/arming_check_reply.hpp>
+#include <px4_msgs/msg/arming_check_request.hpp>
+
+#include "events.h"
+
+#include <memory>
+#include <functional>
+
+class Registration;
+
+namespace px4_sdk
+{
+
+struct ModeRequirements {
+
+	static ModeRequirements none()
+	{
+		// None is the default already
+		return ModeRequirements{};
+	}
+	static ModeRequirements autonomous()
+	{
+		ModeRequirements ret{};
+		ret.angular_velocity = true;
+		ret.attitude = true;
+		ret.local_alt = true;
+		ret.local_position = true;
+		ret.global_position = true;
+		ret.home_position = true;
+		return ret;
+	}
+	static ModeRequirements manualControlledPosition()
+	{
+		ModeRequirements ret{};
+		ret.angular_velocity = true;
+		ret.attitude = true;
+		ret.local_alt = true;
+		ret.local_position_relaxed = true;
+		return ret;
+	}
+
+	bool angular_velocity{false};
+	bool attitude{false};
+	bool local_alt{false};
+	bool local_position{false};
+	bool local_position_relaxed{false};
+	bool global_position{false};
+	bool mission{false};
+	bool home_position{false};
+	bool prevent_arming{false};
+};
+
+
+class HealthAndArmingCheckReporter
+{
+public:
+	HealthAndArmingCheckReporter(px4_msgs::msg::ArmingCheckReply &arming_check_reply)
+		: _arming_check_reply(arming_check_reply) {}
+
+	template<typename... Args>
+	void armingCheckFailureExt(uint32_t event_id,
+				   events::Log log_level, const char *message, Args... args)
+	{
+		uint16_t navigation_mode_groups{};
+		uint8_t health_component_index{};
+
+		_arming_check_reply.can_arm_and_run = false;
+
+		if (!addEvent(event_id, log_level, message, navigation_mode_groups,
+			      health_component_index, args...)) {
+			printf("Error: too many events\n");
+		}
+	}
+
+	void setHealth(uint8_t health_component_index, bool is_present, bool warning, bool error);
+
+private:
+	template<typename... Args>
+	bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, Args... args);
+
+	px4_msgs::msg::ArmingCheckReply &_arming_check_reply;
+};
+
+
+template<typename... Args>
+bool HealthAndArmingCheckReporter::addEvent(uint32_t event_id, const events::LogLevels &log_levels,
+		const char *message, Args... args)
+{
+	if (_arming_check_reply.num_events >= _arming_check_reply.events.size()) {
+		return false;
+	}
+
+	events::EventType &e = _arming_check_reply.events[_arming_check_reply.num_events];
+	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
+	e.id = event_id;
+	static_assert(events::util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments");
+	events::util::fillEventArguments((uint8_t *)e.arguments.data(), args...);
+	++_arming_check_reply.num_events;
+	return true;
+}
+
+
+inline void HealthAndArmingCheckReporter::setHealth(uint8_t health_component_index, bool is_present, bool warning,
+		bool error)
+{
+	_arming_check_reply.health_component_index = health_component_index;
+	_arming_check_reply.health_component_is_present = is_present;
+	_arming_check_reply.health_component_warning = warning;
+	_arming_check_reply.health_component_error = error;
+}
+
+
+class HealthAndArmingChecks
+{
+public:
+	using CheckCallback = std::function<void(HealthAndArmingCheckReporter &)>;
+
+	HealthAndArmingChecks(rclcpp::Node &node, const CheckCallback &check_callback,
+			      const std::string &topic_namespace_prefix = "");
+	HealthAndArmingChecks(const HealthAndArmingChecks &) = delete;
+
+	/**
+	 * Register the checks. Call this once on startup. This is a blocking method.
+	 * @param name registration name. Should uniquely identify the component with length < 25 characters
+	 * @return true on success
+	 */
+	bool doRegister(const std::string &name);
+
+	void setModeRequirements(const ModeRequirements &mode_requirements) { _mode_requirements = mode_requirements; }
+
+private:
+	friend class ModeBase;
+	friend class ModeExecutorBase;
+	void overrideRegistration(const std::shared_ptr<Registration> &registration);
+
+	void watchdogTimerUpdate();
+
+	rclcpp::Node &_node;
+	std::shared_ptr<Registration> _registration;
+	CheckCallback _check_callback;
+	bool _check_triggered{true};
+
+	rclcpp::Subscription<px4_msgs::msg::ArmingCheckRequest>::SharedPtr _arming_check_request_sub;
+	rclcpp::Publisher<px4_msgs::msg::ArmingCheckReply>::SharedPtr _arming_check_reply_pub;
+
+	ModeRequirements _mode_requirements{};
+	rclcpp::TimerBase::SharedPtr _watchdog_timer;
+	bool _shutdown_on_timeout{true};
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_msgs/msg/mode_completed.hpp>
+#include "health_and_arming_checks.h"
+#include "setpoints.h"
+
+class Registration;
+struct RegistrationSettings;
+
+namespace px4_sdk
+{
+
+enum class Result {
+	Success = 0,
+	Rejected, ///< The request was rejected
+	Interrupted, ///< Ctrl-C or another error (from ROS)
+	Timeout,
+	Deactivated, ///< Mode or executor got deactivated
+
+	// Mode-specific results
+	ModeFailureOther = 100,
+};
+
+static_assert((int)Result::ModeFailureOther == (int)px4_msgs::msg::ModeCompleted::RESULT_FAILURE_OTHER,
+	      "definition mismatch");
+
+constexpr inline const char *resultToString(Result result) noexcept
+{
+	switch (result) {
+	case Result::Success: return "Success";
+
+	case Result::Rejected: return "Rejected";
+
+	case Result::Interrupted: return "Interrupted";
+
+	case Result::Timeout: return "Timeout";
+
+	case Result::Deactivated: return "Deactivated";
+
+	case Result::ModeFailureOther: return "Mode Failure (generic)";
+	}
+
+	return "Unknown";
+}
+
+class ModeBase
+{
+public:
+	using ID_t = uint8_t; ///< Mode ID, corresponds to nav_state
+	static constexpr ID_t ID_INVALID = 0xff;
+
+	static constexpr ID_t ID_NAVIGATION_STATE_POSCTL = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_POSCTL;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_TAKEOFF = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF;
+	static constexpr ID_t ID_NAVIGATION_STATE_DESCEND = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_LAND = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LAND;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_RTL = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_RTL;
+
+	struct Settings {
+		std::string name; ///< Name of the mode with length < 25 characters
+		bool activate_even_while_disarmed{true}; ///< If true, the mode is also activated while disarmed if selected
+		ID_t replace_internal_mode{ID_INVALID}; ///< Can be used to replace an fmu-internal mode
+	};
+
+	ModeBase(rclcpp::Node &node, const Settings &settings, const ModeRequirements &requirements,
+		 const std::string &topic_namespace_prefix = "");
+	ModeBase(const ModeBase &) = delete;
+	virtual ~ModeBase() = default;
+
+	/**
+	 * Register the mode. Call this once on startup, unless there's an associated executor. This is a blocking method.
+	 * @return true on success
+	 */
+	bool doRegister();
+
+
+	/**
+	 * Report any custom mode requirements. This is called regularly, also while the mode is active.
+	 */
+	virtual void checkArmingAndRunConditions(HealthAndArmingCheckReporter &reporter) {}
+
+	/**
+	 * Called whenever the mode is activated, also if the vehicle is disarmed
+	 */
+	virtual void onActivate() = 0;
+
+	/**
+	 * Called whenever the mode is deactivated, also if the vehicle is disarmed
+	 */
+	virtual void onDeactivate() = 0;
+
+	/**
+	 * Set the update rate when the mode is active. This is disabled by default.
+	 * @param rate_hz set to 0 to disable
+	 */
+	void setSetpointUpdateRate(float rate_hz);
+
+	virtual void updateSetpoint() {}
+
+	/**
+	 * Mode completed signal. Call this when the mode is finished. A mode might never call this, but modes like
+	 * RTL, Land or Takeoff are expected to signal their completion.
+	 * @param result
+	 */
+	void completed(Result result);
+
+
+	// Properties & state
+
+	ID_t id() const;
+
+	bool isArmed() const { return _is_armed; }
+
+	bool isActive() const { return _is_active; }
+
+	rclcpp::Node &node() { return _node; }
+
+	SetpointSender &setpoints() { return _setpoint_sender; }
+
+private:
+	friend class ModeExecutorBase;
+	void overrideRegistration(const std::shared_ptr<Registration> &registration);
+	RegistrationSettings getRegistrationSettings() const;
+
+	void unsubscribeVehicleStatus();
+	void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate = false);
+
+	void callOnActivate();
+	void callOnDeactivate();
+
+	void updateSetpointUpdateTimer();
+
+	rclcpp::Node &_node;
+	std::shared_ptr<Registration> _registration;
+
+	const Settings _settings;
+
+	HealthAndArmingChecks _health_and_arming_checks;
+
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr _vehicle_status_sub;
+	rclcpp::Publisher<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_pub;
+
+	bool _is_active{false}; ///< Mode is currently selected
+	bool _is_armed{false}; ///< Is vehicle armed?
+	bool _completed{false}; ///< Is mode completed?
+
+	float _setpoint_update_rate_hz{0.f};
+	rclcpp::TimerBase::SharedPtr _setpoint_update_timer;
+	SetpointSender _setpoint_sender;
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h

@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   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 "mode.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_command_ack.hpp>
+#include <px4_msgs/msg/mode_completed.hpp>
+
+#include <functional>
+
+class Registration;
+
+namespace px4_sdk
+{
+
+class ModeExecutorBase
+{
+public:
+	using CompletedCallback = std::function<void(Result)>;
+
+	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 DeactivateReason {
+		FailsafeActivated,
+		Other
+	};
+
+	ModeExecutorBase(rclcpp::Node &node, const Settings &settings, ModeBase &owned_mode,
+			 const std::string &topic_namespace_prefix = "");
+	ModeExecutorBase(const ModeExecutorBase &) = delete;
+	virtual ~ModeExecutorBase() = default;
+
+	/**
+	 * Register the mode executor. Call this once on startup. This is a blocking method.
+	 * @return true on success
+	 */
+	bool doRegister();
+
+
+	/**
+	 * Called whenever the mode is activated, also if the vehicle is disarmed
+	 */
+	virtual void onActivate() = 0;
+
+	/**
+	 * Called whenever the mode is deactivated, also if the vehicle is disarmed
+	 */
+	virtual void onDeactivate(DeactivateReason reason) = 0;
+
+
+	/**
+	* Send command and wait for ack/nack
+	*/
+	Result sendCommandSync(uint32_t command, float param1 = NAN, float param2 = NAN, float param3 = NAN, float param4 = NAN,
+			       float param5 = NAN, float param6 = NAN, float param7 = NAN);
+
+	/**
+	 * Switch to a mode with a callback when it is finished.
+	 * The callback is also executed when the mode is deactivated.
+	 * If there's already a mode scheduling active, the previous one is cancelled.
+	 */
+	void scheduleMode(ModeBase::ID_t mode_id, const CompletedCallback &on_completed);
+
+	void takeoff(const CompletedCallback &on_completed, float altitude = NAN, float heading = NAN);
+	void land(const CompletedCallback &on_completed);
+	void rtl(const CompletedCallback &on_completed);
+
+	void arm(const CompletedCallback &on_completed);
+	void waitReadyToArm(const CompletedCallback &on_completed);
+	void waitUntilDisarmed(const CompletedCallback &on_completed);
+
+	bool isInCharge() const { return _is_in_charge; }
+
+	bool isArmed() const { return _is_armed; }
+
+	ModeBase &ownedMode() { return _owned_mode; }
+
+	int id() const;
+
+	rclcpp::Node &node() { return _node; }
+
+private:
+
+	class ScheduledMode
+	{
+	public:
+		ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix);
+
+		bool active() const { return _mode_id != ModeBase::ID_INVALID; }
+		void activate(ModeBase::ID_t mode_id, const CompletedCallback &on_completed);
+		void cancel();
+		ModeBase::ID_t modeId() const { return _mode_id; }
+	private:
+		void reset() { _mode_id = ModeBase::ID_INVALID; }
+
+		ModeBase::ID_t _mode_id{ModeBase::ID_INVALID};
+		CompletedCallback _on_completed_callback;
+		rclcpp::Subscription<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_sub;
+	};
+
+	class WaitForVehicleStatusCondition
+	{
+	public:
+		using RunCheckCallback = std::function<bool(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)>;
+		WaitForVehicleStatusCondition() = default;
+
+		bool active() const { return _on_completed_callback != nullptr; }
+		void update(const px4_msgs::msg::VehicleStatus::UniquePtr &msg);
+
+		void activate(const RunCheckCallback &run_check_callback, const CompletedCallback &on_completed);
+		void cancel();
+	private:
+		CompletedCallback _on_completed_callback;
+		RunCheckCallback _run_check_callback;
+	};
+
+	void callOnActivate();
+	void callOnDeactivate(DeactivateReason reason);
+
+	void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg);
+
+	void scheduleMode(ModeBase::ID_t mode_id, const px4_msgs::msg::VehicleCommand &cmd,
+			  const ModeExecutorBase::CompletedCallback &on_completed);
+
+	rclcpp::Node &_node;
+	const Settings _settings;
+	ModeBase &_owned_mode;
+
+	std::shared_ptr<Registration> _registration;
+
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr _vehicle_status_sub;
+	rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr _vehicle_command_pub;
+	rclcpp::Subscription<px4_msgs::msg::VehicleCommandAck>::SharedPtr _vehicle_command_ack_sub;
+
+	ScheduledMode _current_scheduled_mode;
+	WaitForVehicleStatusCondition _current_wait_vehicle_status;
+
+	bool _is_in_charge{false};
+	bool _is_armed{false};
+	bool _was_never_activated{true};
+	ModeBase::ID_t _prev_nav_state{ModeBase::ID_INVALID};
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/setpoints.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+
+#include <Eigen/Core>
+
+namespace px4_sdk
+{
+
+class ModeBase;
+
+class SetpointSender
+{
+public:
+	enum class SetpointConfigurationResult {
+		Success = 0,
+		Timeout,
+		FailureOther
+	};
+
+	struct SetpointConfiguration {
+		// TODO...
+		bool manual_enabled{false};
+		bool auto_enabled{false};
+		bool rates_enabled{true};
+		bool attitude_enabled{true};
+		bool acceleration_enabled{true};
+		bool velocity_enabled{true};
+		bool position_enabled{true};
+		bool altitude_enabled{true};
+		bool climb_rate_enabled{false};
+	};
+
+	SetpointSender(rclcpp::Node &node, const ModeBase &mode, const std::string &topic_namespace_prefix = "");
+
+	SetpointConfigurationResult configureSetpointsSync(const SetpointConfiguration &config);
+
+	void sendTrajectorySetpoint(const Eigen::Vector3f &velocity);
+
+	// TODO: goto, stop, ...
+
+private:
+	rclcpp::Node &_node;
+	const ModeBase &_mode;
+	SetpointConfiguration _setpoint_configuration{};
+	rclcpp::Publisher<px4_msgs::msg::VehicleControlMode>::SharedPtr _config_control_setpoints_pub;
+	rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr _trajectory_setpoint_pub;
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/wait_for_fmu.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+using namespace std::chrono_literals;
+
+namespace px4_sdk
+{
+
+/**
+ * Wait for a heartbeat/status message from the FMU
+ * @return true on success
+ */
+bool waitForFMU(rclcpp::Node &node, rclcpp::Duration timeout = 30s, const std::string &topic_namespace_prefix = "");
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>px4_sdk_lib</name>
+  <version>0.0.1</version>
+  <description>Library to interface with PX4 from ROS2</description>
+  <maintainer email="beat-kueng@gmx.net">beat</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>px4_msgs</build_depend>
+  <build_export_depend>eigen</build_export_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/cpp/px4_sdk_lib/src/components/health_and_arming_checks.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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 "registration.h"
+#include "px4_sdk/components/health_and_arming_checks.h"
+
+#include <cassert>
+
+using namespace std::chrono_literals;
+using namespace px4_sdk;
+
+HealthAndArmingChecks::HealthAndArmingChecks(rclcpp::Node &node, const CheckCallback &check_callback,
+		const std::string &topic_namespace_prefix)
+	: _node(node), _registration(std::make_shared<Registration>(node, topic_namespace_prefix)),
+	  _check_callback(check_callback)
+{
+	_arming_check_reply_pub = _node.create_publisher<px4_msgs::msg::ArmingCheckReply>(
+					  topic_namespace_prefix + "/fmu/in/arming_check_reply", 1);
+
+	_arming_check_request_sub = _node.create_subscription<px4_msgs::msg::ArmingCheckRequest>(
+					    topic_namespace_prefix + "/fmu/out/arming_check_request",
+					    rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::ArmingCheckRequest::UniquePtr msg) {
+
+		RCLCPP_DEBUG_ONCE(_node.get_logger(), "Arming check request (id=%i, only printed once)", msg->request_id);
+
+		if (_registration->registered()) {
+			px4_msgs::msg::ArmingCheckReply reply{};
+			reply.registration_id = _registration->armingCheckId();
+			reply.request_id = msg->request_id;
+			reply.can_arm_and_run = true;
+
+			HealthAndArmingCheckReporter reporter(reply);
+			_check_callback(reporter);
+
+			reply.mode_req_angular_velocity = _mode_requirements.angular_velocity;
+			reply.mode_req_attitude = _mode_requirements.attitude;
+			reply.mode_req_local_alt = _mode_requirements.local_alt;
+			reply.mode_req_local_position = _mode_requirements.local_position;
+			reply.mode_req_local_position_relaxed = _mode_requirements.local_position_relaxed;
+			reply.mode_req_global_position = _mode_requirements.global_position;
+			reply.mode_req_mission = _mode_requirements.mission;
+			reply.mode_req_home_position = _mode_requirements.home_position;
+			reply.mode_req_prevent_arming = _mode_requirements.prevent_arming;
+
+			reply.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+			_arming_check_reply_pub->publish(reply);
+			_check_triggered = true;
+
+		} else {
+			RCLCPP_DEBUG(_node.get_logger(), "...not registered yet");
+		}
+	});
+
+	_watchdog_timer = _node.create_wall_timer(4s, std::bind(&HealthAndArmingChecks::watchdogTimerUpdate, this));
+}
+
+void HealthAndArmingChecks::overrideRegistration(const std::shared_ptr<Registration> &registration)
+{
+	assert(!_registration->registered());
+	_registration = registration;
+}
+
+bool HealthAndArmingChecks::doRegister(const std::string &name)
+{
+	assert(!_registration->registered());
+	RegistrationSettings settings{};
+	settings.name = name;
+	settings.register_arming_check = true;
+	return _registration->doRegister(settings);
+}
+
+void HealthAndArmingChecks::watchdogTimerUpdate()
+{
+	if (_registration->registered()) {
+		if (!_check_triggered && _shutdown_on_timeout) {
+			RCLCPP_FATAL(_node.get_logger(), "Timeout, no request received from FMU, exiting (this can happen on FMU reboots)");
+			rclcpp::shutdown();
+		}
+
+		_check_triggered = false;
+
+	} else {
+		// avoid false positives while unregistered
+		_check_triggered = true;
+	}
+}
+

src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   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/mode.h"
+
+#include "registration.h"
+
+#include <cassert>
+#include <cfloat>
+
+using namespace px4_sdk;
+
+ModeBase::ModeBase(rclcpp::Node &node, const ModeBase::Settings &settings,
+		   const ModeRequirements &requirements, const std::string &topic_namespace_prefix)
+	: _node(node), _registration(std::make_shared<Registration>(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)
+{
+	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
+				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
+		if (_registration->registered()) {
+			vehicleStatusUpdated(msg);
+		}
+	});
+	_mode_completed_pub = _node.create_publisher<px4_msgs::msg::ModeCompleted>(
+				      topic_namespace_prefix + "/fmu/in/mode_completed", 1);
+
+	_health_and_arming_checks.setModeRequirements(requirements);
+}
+
+ModeBase::ID_t ModeBase::id() const
+{
+	return _registration->modeId();
+}
+
+void ModeBase::overrideRegistration(const std::shared_ptr<Registration> &registration)
+{
+	assert(!_registration->registered());
+	_health_and_arming_checks.overrideRegistration(registration);
+	_registration = registration;
+}
+
+bool ModeBase::doRegister()
+{
+	assert(!_registration->registered());
+	_health_and_arming_checks.overrideRegistration(_registration);
+	RegistrationSettings settings = getRegistrationSettings();
+	return _registration->doRegister(settings);
+}
+
+RegistrationSettings ModeBase::getRegistrationSettings() const
+{
+	RegistrationSettings settings{};
+	settings.name = _settings.name;
+	settings.register_arming_check = true;
+	settings.register_mode = true;
+
+	if (_settings.replace_internal_mode != ID_INVALID) {
+		settings.enable_replace_internal_mode = true;
+		settings.replace_internal_mode = _settings.replace_internal_mode;
+	}
+
+	return settings;
+}
+
+void ModeBase::callOnActivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode '%s' activated", _registration->name().c_str());
+	_is_active = true;
+	_completed = false;
+	onActivate();
+
+	if (_setpoint_update_rate_hz > FLT_EPSILON) {
+		updateSetpoint(); // Immediately update
+	}
+
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::callOnDeactivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode '%s' deactivated", _registration->name().c_str());
+	_is_active = false;
+	onDeactivate();
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::updateSetpointUpdateTimer()
+{
+	bool activate = _is_active && _setpoint_update_rate_hz > FLT_EPSILON;
+
+	if (activate) {
+		if (!_setpoint_update_timer) {
+			_setpoint_update_timer = _node.create_wall_timer(std::chrono::milliseconds((long)(1000.f /
+			_setpoint_update_rate_hz)), [this]() { updateSetpoint(); });
+		}
+
+	} else {
+		if (_setpoint_update_timer) {
+			_setpoint_update_timer.reset();
+		}
+	}
+}
+
+void ModeBase::setSetpointUpdateRate(float rate_hz)
+{
+	_setpoint_update_timer.reset();
+	_setpoint_update_rate_hz = rate_hz;
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::unsubscribeVehicleStatus()
+{
+	_vehicle_status_sub.reset();
+}
+
+void ModeBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate)
+{
+	// Update state
+	_is_armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+	bool is_active = id() == msg->nav_state && (_is_armed || _settings.activate_even_while_disarmed);
+
+	if (_is_active != is_active) {
+		if (is_active) {
+			if (!do_not_activate) {
+				callOnActivate();
+			}
+
+		} else {
+			callOnDeactivate();
+		}
+	}
+}
+
+void ModeBase::completed(Result result)
+{
+	if (_completed) {
+		RCLCPP_DEBUG_ONCE(_node.get_logger(), "Mode '%s': completed was already called", _registration->name().c_str());
+		return;
+	}
+
+	px4_msgs::msg::ModeCompleted mode_completed{};
+	mode_completed.nav_state = (uint8_t)id();
+	mode_completed.result = (uint8_t)result;
+	mode_completed.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_mode_completed_pub->publish(mode_completed);
+	_completed = true;
+}

src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp

@@ -0,0 +1,368 @@
+/****************************************************************************
+ *
+ *   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/mode_executor.h"
+
+#include "registration.h"
+
+#include <cassert>
+#include <future>
+using namespace std::chrono_literals;
+
+using namespace px4_sdk;
+
+ModeExecutorBase::ModeExecutorBase(rclcpp::Node &node, const ModeExecutorBase::Settings &settings,
+				   ModeBase &owned_mode, const std::string &topic_namespace_prefix)
+	: _node(node), _settings(settings), _owned_mode(owned_mode),
+	  _registration(std::make_shared<Registration>(node, topic_namespace_prefix)),
+	  _current_scheduled_mode(node, topic_namespace_prefix)
+{
+	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
+				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
+		if (_registration->registered()) {
+			vehicleStatusUpdated(msg);
+		}
+	});
+
+	_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
+				       topic_namespace_prefix + "/fmu/in/vehicle_command_mode_executor", 1);
+
+	_vehicle_command_ack_sub = _node.create_subscription<px4_msgs::msg::VehicleCommandAck>(
+					   topic_namespace_prefix + "/fmu/out/vehicle_command_ack", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleCommandAck::UniquePtr msg) {});
+}
+
+bool ModeExecutorBase::doRegister()
+{
+	if (_owned_mode._registration->registered()) {
+		RCLCPP_FATAL(_node.get_logger(), "Mode executor %s: mode already registered", _registration->name().c_str());
+	}
+
+	assert(!_registration->registered());
+	_owned_mode.overrideRegistration(_registration);
+	_owned_mode.unsubscribeVehicleStatus();
+	RegistrationSettings settings = _owned_mode.getRegistrationSettings();
+	settings.register_mode_executor = true;
+	settings.activate_mode_immediately = _settings.activate_immediately;
+	return _registration->doRegister(settings);
+}
+
+void ModeExecutorBase::callOnActivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode executor '%s' activated", _registration->name().c_str());
+	_is_in_charge = true;
+	onActivate();
+}
+
+void ModeExecutorBase::callOnDeactivate(DeactivateReason reason)
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode executor '%s' deactivated (%i)", _registration->name().c_str(), (int)reason);
+	_current_scheduled_mode.cancel();
+	_current_wait_vehicle_status.cancel();
+	_is_in_charge = false;
+	_was_never_activated = false; // Set on deactivation, so we stay activated for the first time (while disarmed)
+	onDeactivate(reason);
+}
+
+int ModeExecutorBase::id() const
+{
+	return _registration->modeExecutorId();
+}
+
+Result ModeExecutorBase::sendCommandSync(uint32_t command, float param1, float param2, float param3, float param4,
+		float param5, float param6, float param7)
+{
+	// Send command and wait for ack
+	Result result{Result::Rejected};
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = command;
+	cmd.param1 = param1;
+	cmd.param2 = param2;
+	cmd.param3 = param3;
+	cmd.param4 = param4;
+	cmd.param5 = param5;
+	cmd.param6 = param6;
+	cmd.param7 = param7;
+	cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START + id();
+	cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+
+
+	rclcpp::WaitSet wait_set;
+	wait_set.add_subscription(_vehicle_command_ack_sub);
+
+	bool got_reply = false;
+	auto start_time = std::chrono::steady_clock::now();
+	auto timeout = 200ms;
+	_vehicle_command_pub->publish(cmd);
+
+	while (!got_reply) {
+		auto now = std::chrono::steady_clock::now();
+
+		if (now >= start_time + timeout) {
+			break;
+		}
+
+		auto wait_ret = wait_set.wait(timeout - (now - start_time));
+
+		if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+			px4_msgs::msg::VehicleCommandAck ack;
+			rclcpp::MessageInfo info;
+
+			if (_vehicle_command_ack_sub->take(ack, info)) {
+				if (ack.command == cmd.command && ack.target_component == cmd.source_component) {
+					if (ack.result == px4_msgs::msg::VehicleCommandAck::VEHICLE_CMD_RESULT_ACCEPTED) {
+						result = Result::Success;
+					}
+
+					got_reply = true;
+				}
+
+			} else {
+				RCLCPP_DEBUG(_node.get_logger(), "no message received");
+			}
+
+		} else {
+			RCLCPP_DEBUG(_node.get_logger(), "timeout");
+		}
+	}
+
+	wait_set.remove_subscription(_vehicle_command_ack_sub);
+
+	if (!got_reply) {
+		// We don't expect to run into an ack timeout
+		result = Result::Timeout;
+		RCLCPP_WARN(_node.get_logger(), "Cmd %i: timeout, no ack received", cmd.command);
+	}
+
+	return result;
+}
+
+void ModeExecutorBase::scheduleMode(ModeBase::ID_t mode_id, const CompletedCallback &on_completed)
+{
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE;
+	cmd.param1 = mode_id;
+	scheduleMode(mode_id, cmd, on_completed);
+}
+
+void ModeExecutorBase::scheduleMode(ModeBase::ID_t mode_id, const px4_msgs::msg::VehicleCommand &cmd,
+				    const CompletedCallback &on_completed)
+{
+	if (!_is_armed) {
+		on_completed(Result::Rejected);
+		return;
+	}
+
+	// If there's already an active mode, cancel it (it will call the callback with a failure result)
+	_current_scheduled_mode.cancel();
+
+	Result result = sendCommandSync(cmd.command, cmd.param1, cmd.param2, cmd.param3, cmd.param4, cmd.param5, cmd.param6,
+					cmd.param7);
+
+	if (result != Result::Success) {
+		on_completed(result);
+		return;
+	}
+
+	// Store the callback and ensure it's called eventually. There's a number of outcomes:
+	// - The mode finishes and publishes the completion result.
+	// - Failsafe is entered or the user switches out. In that case the executor gets deactivated.
+	// - The user switches into the owned mode. In that case the fmu does not deactivate the executor.
+	_current_scheduled_mode.activate(mode_id, on_completed);
+}
+
+void ModeExecutorBase::takeoff(const CompletedCallback &on_completed, float altitude, float heading)
+{
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_TAKEOFF;
+	cmd.param1 = NAN;
+	cmd.param2 = NAN;
+	cmd.param3 = NAN;
+	cmd.param4 = heading;
+	cmd.param5 = NAN;
+	cmd.param6 = NAN;
+	cmd.param7 = altitude; // TODO: this is AMSL, local (relative to home) would be better
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_TAKEOFF, cmd, on_completed);
+}
+
+void ModeExecutorBase::land(const CompletedCallback &on_completed)
+{
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_LAND, on_completed);
+}
+
+void ModeExecutorBase::rtl(const CompletedCallback &on_completed)
+{
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_RTL, on_completed);
+}
+
+void ModeExecutorBase::arm(const CompletedCallback &on_completed)
+{
+	if (_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	Result result = sendCommandSync(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f);
+
+	if (result != Result::Success) {
+		on_completed(result);
+		return;
+	}
+
+	// Wait until our internal state changes to armed
+	_current_wait_vehicle_status.activate(
+	[this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return _is_armed; }, on_completed);
+}
+
+void ModeExecutorBase::waitReadyToArm(const CompletedCallback &on_completed)
+{
+	if (_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Waiting until ready to arm...");
+	_current_wait_vehicle_status.activate([](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return msg->pre_flight_checks_pass; },
+	on_completed);
+}
+
+void ModeExecutorBase::waitUntilDisarmed(const CompletedCallback &on_completed)
+{
+	if (!_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Waiting until disarmed...");
+	_current_wait_vehicle_status.activate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return !_is_armed; },
+	on_completed);
+}
+
+void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)
+{
+	// Update state
+	const bool was_armed = _is_armed;
+	const ModeBase::ID_t current_mode = (ModeBase::ID_t)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 is_in_charge = id() == msg->executor_in_charge && (_is_armed || wants_to_activate_immediately);
+	const bool changed_to_armed = !was_armed && _is_armed;
+
+	if (_is_in_charge != is_in_charge) {
+		if (is_in_charge) {
+			callOnActivate();
+
+		} else {
+			callOnDeactivate(msg->failsafe ? DeactivateReason::FailsafeActivated : DeactivateReason::Other);
+		}
+	}
+
+	if (_is_in_charge && _current_scheduled_mode.active() && _current_scheduled_mode.modeId() == _prev_nav_state
+	    && current_mode == _owned_mode.id() && _prev_nav_state != current_mode) {
+		// If the user switched from the currently scheduled mode to the owned mode, the executor stays in charge.
+		// In order for the executor to restore the correct state, we re-activate it (which also cancels the scheduled
+		// mode)
+		callOnDeactivate(DeactivateReason::Other);
+		callOnActivate();
+	}
+
+	// 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())
+				    || (changed_to_armed && _current_wait_vehicle_status.active());
+	// To avoid race conditions and ensure consistent ordering we update vehicle status of the mode directly.
+	_owned_mode.vehicleStatusUpdated(msg, do_not_activate_mode && _is_in_charge);
+
+	_prev_nav_state = current_mode;
+
+	_current_wait_vehicle_status.update(msg);
+}
+
+ModeExecutorBase::ScheduledMode::ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+{
+	_mode_completed_sub = node.create_subscription<px4_msgs::msg::ModeCompleted>(
+				      topic_namespace_prefix + "/fmu/out/mode_completed", rclcpp::QoS(1).best_effort(),
+	[this, &node](px4_msgs::msg::ModeCompleted::UniquePtr msg) {
+		if (active() && msg->nav_state == (uint8_t)_mode_id) {
+			RCLCPP_DEBUG(node.get_logger(), "Got matching ModeCompleted message, result: %i", msg->result);
+			CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+			reset();
+			on_completed_callback((Result)msg->result); // Call after, as it might trigger new requests
+		}
+	});
+}
+
+void ModeExecutorBase::ScheduledMode::activate(ModeBase::ID_t mode_id, const CompletedCallback &on_completed)
+{
+	assert(!active());
+	_mode_id = mode_id;
+	_on_completed_callback = on_completed;
+}
+
+void ModeExecutorBase::ScheduledMode::cancel()
+{
+	if (active()) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		reset();
+		on_completed_callback(Result::Deactivated); // Call after, as it might trigger new requests
+	}
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::update(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)
+{
+	if (_on_completed_callback && _run_check_callback(msg)) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		_on_completed_callback = nullptr;
+		_run_check_callback = nullptr;
+		on_completed_callback(Result::Success); // Call after, as it might trigger new requests
+	}
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::activate(const RunCheckCallback &run_check_callback,
+		const CompletedCallback &on_completed)
+{
+	assert(!_on_completed_callback);
+	_on_completed_callback = on_completed;
+	_run_check_callback = run_check_callback;
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::cancel()
+{
+	if (_on_completed_callback) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		_on_completed_callback = nullptr;
+		_run_check_callback = nullptr;
+		on_completed_callback(Result::Deactivated); // Call after, as it might trigger new requests
+	}
+}

src/ros2/cpp/px4_sdk_lib/src/components/registration.cpp

@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   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 "registration.h"
+
+#include <cassert>
+#include <random>
+
+using namespace std::chrono_literals;
+
+Registration::Registration(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+	: _node(node)
+{
+	_register_ext_component_reply_sub = node.create_subscription<px4_msgs::msg::RegisterExtComponentReply>(
+			topic_namespace_prefix + "/fmu/out/register_ext_component_reply",
+			rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::RegisterExtComponentReply::UniquePtr msg) {
+	});
+
+	_register_ext_component_request_pub = node.create_publisher<px4_msgs::msg::RegisterExtComponentRequest>(
+			topic_namespace_prefix + "/fmu/in/register_ext_component_request", 1);
+
+	_unregister_ext_component_pub = node.create_publisher<px4_msgs::msg::UnregisterExtComponent>(
+						topic_namespace_prefix + "/fmu/in/unregister_ext_component", 1);
+
+	_unregister_ext_component.mode_id = px4_sdk::ModeBase::ID_INVALID;
+}
+
+bool Registration::doRegister(const RegistrationSettings &settings)
+{
+	assert(!_registered);
+	px4_msgs::msg::RegisterExtComponentRequest request{};
+
+	if (settings.name.length() >= request.name.size() || settings.name.length() >= _unregister_ext_component.name.size()) {
+		RCLCPP_ERROR(_node.get_logger(), "Name too long (%i >= %i)", (int)settings.name.length(), (int)request.name.size());
+		return false;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Registering '%s' (arming check: %i, mode: %i, mode executor: %i)",
+		     settings.name.c_str(),
+		     settings.register_arming_check, settings.register_mode, settings.register_mode_executor);
+
+	strcpy((char *)request.name.data(), settings.name.c_str());
+	request.register_arming_check = settings.register_arming_check;
+	request.register_mode = settings.register_mode;
+	request.register_mode_executor = settings.register_mode_executor;
+	request.enable_replace_internal_mode = settings.enable_replace_internal_mode;
+	request.replace_internal_mode = settings.replace_internal_mode;
+	request.activate_mode_immediately = settings.activate_mode_immediately;
+
+	std::random_device rd;
+	std::mt19937 gen(rd());
+	std::uniform_int_distribution<uint64_t> distrib{};
+	request.request_id = distrib(gen);
+
+	// wait for subscription, it might take a while initially...
+	for (int i = 0; i < 100; ++i) {
+		if (_register_ext_component_request_pub->get_subscription_count() > 0) {
+			RCLCPP_DEBUG(_node.get_logger(), "Subscriber found, continuing");
+			break;
+		}
+
+		usleep(100000);
+	}
+
+	// send request and wait for response
+	rclcpp::WaitSet wait_set;
+	wait_set.add_subscription(_register_ext_component_reply_sub);
+
+	bool got_reply = false;
+
+	for (int retries = 0; retries < 5 && !got_reply; ++retries) {
+		request.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_register_ext_component_request_pub->publish(request);
+
+		// wait for publisher, it might take a while initially...
+		for (int i = 0; i < 100 && retries == 0; ++i) {
+			if (_register_ext_component_reply_sub->get_publisher_count() > 0) {
+				RCLCPP_DEBUG(_node.get_logger(), "Publisher found, continuing");
+				break;
+			}
+
+			usleep(100000);
+		}
+
+		auto start_time = std::chrono::steady_clock::now();
+		auto timeout = 300ms;
+
+		while (!got_reply) {
+			auto now = std::chrono::steady_clock::now();
+
+			if (now >= start_time + timeout) {
+				break;
+			}
+
+			auto wait_ret = wait_set.wait(timeout - (now - start_time));
+
+			if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+				px4_msgs::msg::RegisterExtComponentReply reply;
+				rclcpp::MessageInfo info;
+
+				if (_register_ext_component_reply_sub->take(reply, info)) {
+					reply.name.back() = '\0';
+
+					if (strcmp((const char *) reply.name.data(), settings.name.c_str()) == 0 &&
+					    request.request_id == reply.request_id) {
+						RCLCPP_DEBUG(_node.get_logger(), "Got RegisterExtComponentReply");
+
+						if (reply.success) {
+							_unregister_ext_component.arming_check_id = reply.arming_check_id;
+							_unregister_ext_component.mode_id = reply.mode_id;
+							_unregister_ext_component.mode_executor_id = reply.mode_executor_id;
+							strcpy((char *) _unregister_ext_component.name.data(), settings.name.c_str());
+							_registered = true;
+
+						} else {
+							RCLCPP_ERROR(_node.get_logger(), "Registration failed");
+						}
+
+						got_reply = true;
+					}
+
+				} else {
+					RCLCPP_INFO(_node.get_logger(), "no message received");
+				}
+
+			} else {
+				RCLCPP_INFO(_node.get_logger(), "timeout");
+			}
+		}
+	}
+
+	wait_set.remove_subscription(_register_ext_component_reply_sub);
+
+	return _registered;
+}
+
+void Registration::doUnregister()
+{
+	if (_registered) {
+		RCLCPP_DEBUG(_node.get_logger(), "Unregistering");
+		_unregister_ext_component.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_unregister_ext_component_pub->publish(_unregister_ext_component);
+		_registered = false;
+	}
+}

src/ros2/cpp/px4_sdk_lib/src/components/registration.h

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/register_ext_component_request.hpp>
+#include <px4_msgs/msg/register_ext_component_reply.hpp>
+#include <px4_msgs/msg/unregister_ext_component.hpp>
+
+#include <px4_sdk/components/mode.h>
+
+struct RegistrationSettings {
+	std::string name;
+	bool register_arming_check{false};
+	bool register_mode{false};
+	bool register_mode_executor{false};
+
+	bool enable_replace_internal_mode{false};
+	px4_sdk::ModeBase::ID_t replace_internal_mode{};
+	bool activate_mode_immediately{false};
+};
+
+class Registration
+{
+public:
+
+	Registration(rclcpp::Node &node, const std::string &topic_namespace_prefix = "");
+	~Registration()
+	{
+		doUnregister();
+	}
+
+	bool doRegister(const RegistrationSettings &settings);
+	void doUnregister();
+
+	bool registered() const { return _registered; }
+
+	int armingCheckId() const { return _unregister_ext_component.arming_check_id; }
+	px4_sdk::ModeBase::ID_t modeId() const { return _unregister_ext_component.mode_id; }
+	int modeExecutorId() const { return _unregister_ext_component.mode_executor_id; }
+
+	std::string name() const { return (const char *)_unregister_ext_component.name.data(); }
+private:
+	rclcpp::Subscription<px4_msgs::msg::RegisterExtComponentReply>::SharedPtr _register_ext_component_reply_sub;
+	rclcpp::Publisher<px4_msgs::msg::RegisterExtComponentRequest>::SharedPtr _register_ext_component_request_pub;
+	rclcpp::Publisher<px4_msgs::msg::UnregisterExtComponent>::SharedPtr _unregister_ext_component_pub;
+
+	bool _registered{false};
+	px4_msgs::msg::UnregisterExtComponent _unregister_ext_component{};
+	rclcpp::Node &_node;
+};
+

src/ros2/cpp/px4_sdk_lib/src/components/setpoints.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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/setpoints.h"
+#include "px4_sdk/components/mode.h"
+
+#include <cassert>
+
+using namespace px4_sdk;
+
+SetpointSender::SetpointSender(rclcpp::Node &node, const ModeBase &mode, const std::string &topic_namespace_prefix)
+	: _node(node), _mode(mode)
+{
+	_config_control_setpoints_pub = node.create_publisher<px4_msgs::msg::VehicleControlMode>(
+						topic_namespace_prefix + "/fmu/in/config_control_setpoints", 1);
+
+	_trajectory_setpoint_pub = node.create_publisher<px4_msgs::msg::TrajectorySetpoint>(
+					   topic_namespace_prefix + "/fmu/in/trajectory_setpoint", 1);
+}
+
+SetpointSender::SetpointConfigurationResult SetpointSender::configureSetpointsSync(const SetpointConfiguration &config)
+{
+	assert(_mode.id() != ModeBase::ID_INVALID); // Ensure mode is registered
+
+	px4_msgs::msg::VehicleControlMode control_mode{};
+	control_mode.source_id = (uint8_t)_mode.id();
+	control_mode.flag_control_manual_enabled = config.manual_enabled;
+	control_mode.flag_control_auto_enabled = config.auto_enabled;
+	control_mode.flag_control_rates_enabled = config.rates_enabled;
+	control_mode.flag_control_attitude_enabled = config.attitude_enabled;
+	control_mode.flag_control_acceleration_enabled = config.acceleration_enabled;
+	control_mode.flag_control_velocity_enabled = config.velocity_enabled;
+	control_mode.flag_control_position_enabled = config.position_enabled;
+	control_mode.flag_control_altitude_enabled = config.altitude_enabled;
+	control_mode.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_config_control_setpoints_pub->publish(control_mode);
+	// TODO: wait for feedback from FMU
+
+	_setpoint_configuration = config;
+	return SetpointConfigurationResult::Success;
+}
+
+void SetpointSender::sendTrajectorySetpoint(const Eigen::Vector3f &velocity)
+{
+	// TODO: check if configured setpoints match
+
+	// TODO: add mode id to setpoint
+
+	px4_msgs::msg::TrajectorySetpoint sp{};
+	sp.yaw = NAN;
+	sp.yawspeed = NAN;
+	sp.position[0] = sp.position[1] = sp.position[2] = NAN; // TODO ...
+	sp.acceleration[0] = sp.acceleration[1] = sp.acceleration[2] = NAN;
+	sp.velocity[0] = velocity(0);
+	sp.velocity[1] = velocity(1);
+	sp.velocity[2] = velocity(2);
+	sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_trajectory_setpoint_pub->publish(sp);
+}

src/ros2/cpp/px4_sdk_lib/src/components/wait_for_fmu.cpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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/wait_for_fmu.h"
+#include <px4_msgs/msg/vehicle_status.hpp>
+
+bool px4_sdk::waitForFMU(rclcpp::Node &node, rclcpp::Duration timeout, const std::string &topic_namespace_prefix)
+{
+	RCLCPP_INFO(node.get_logger(), "Waiting for FMU...");
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub =
+		node.create_subscription<px4_msgs::msg::VehicleStatus>(
+			topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[](px4_msgs::msg::VehicleStatus::UniquePtr msg) {});
+
+	rclcpp::WaitSet wait_set;
+	wait_set.add_subscription(vehicle_status_sub);
+
+	bool got_message = false;
+	auto start_time = node.now();
+
+	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<std::chrono::microseconds>());
+
+		if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+			px4_msgs::msg::VehicleStatus msg;
+			rclcpp::MessageInfo info;
+
+			if (vehicle_status_sub->take(msg, info)) {
+				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;
+}

src/ros2/cpp/px4_sdk_lib/test/integration/arming_check.cpp

@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+
+#include "util.h"
+
+#include <px4_sdk/components/health_and_arming_checks.h>
+#include <px4_sdk/components/wait_for_fmu.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+using namespace px4_sdk;
+using namespace std;
+using namespace std::chrono_literals;
+
+class Tester : public ::testing::Test
+{
+public:
+	static void SetUpTestCase()
+	{
+		rclcpp::init(0, nullptr);
+	}
+	static void TearDownTestCase()
+	{
+		rclcpp::shutdown();
+	}
+};
+
+class TestExecution
+{
+public:
+	TestExecution(rclcpp::Node &node)
+		: _node(node), _vehicle_state(node) {}
+
+	void run();
+private:
+	enum class State {
+		WaitForFirstCallback,
+		WaitUntilCanArm,
+		WaitUntilCannotArm,
+		Completed,
+	};
+
+	void setState(State state)
+	{
+		RCLCPP_DEBUG(_node.get_logger(), "Executing state %i", (int)state);
+		_state = state;
+	}
+
+	rclcpp::Node &_node;
+	State _state{State::WaitForFirstCallback};
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+	VehicleState _vehicle_state;
+	int _num_failures_reported{0};
+	std::unique_ptr<HealthAndArmingChecks> _health_and_arming_checks;
+};
+
+void TestExecution::run()
+{
+	_test_timeout = _node.create_wall_timer(20s, [this] {
+		EXPECT_EQ(_state, State::Completed);
+		rclcpp::shutdown();
+	});
+
+	// The test does the following:
+	// - wait until arming callback called
+	// - wait until arming is possible
+	// - report a failing check
+	// - expect arming denied
+	_vehicle_state.setOnVehicleStatusUpdate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) {
+		switch (_state) {
+		case State::WaitUntilCanArm:
+			if (msg->pre_flight_checks_pass) {
+				setState(State::WaitUntilCannotArm);
+			}
+
+			break;
+
+		case State::WaitUntilCannotArm:
+
+			// Ensure we got a vehicle status update after we reported a failing check (prevent race conditions)
+			if (_num_failures_reported == 2) {
+				ASSERT_FALSE(msg->pre_flight_checks_pass);
+				setState(State::Completed);
+				rclcpp::shutdown();
+			}
+
+			break;
+
+		default:
+			break;
+		}
+	});
+
+	_health_and_arming_checks = std::make_unique<HealthAndArmingChecks>(_node, [this](HealthAndArmingCheckReporter &
+	reporter) {
+		if (_state == State::WaitForFirstCallback) {
+			setState(State::WaitUntilCanArm);
+
+		} else if (_state == State::WaitUntilCannotArm) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailureExt(events::ID("check_unit_test_failure"),
+						       events::Log::Error, "Test Failure");
+			++_num_failures_reported;
+		}
+	});
+	ASSERT_TRUE(_health_and_arming_checks->doRegister("arming check test"));
+}
+
+
+TEST_F(Tester, deny_arming)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecution test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}

src/ros2/cpp/px4_sdk_lib/test/integration/mode.cpp

@@ -0,0 +1,297 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+
+#include "util.h"
+
+#include <px4_sdk/components/mode.h>
+#include <px4_sdk/components/wait_for_fmu.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+using namespace px4_sdk;
+using namespace std;
+using namespace std::chrono_literals;
+
+static const char *name = "Test Descend";
+
+
+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, true, ModeBase::ID_NAVIGATION_STATE_DESCEND}, ModeRequirements::none())
+	{
+		setSetpointUpdateRate(50.f);
+	}
+
+	virtual ~FlightModeTest() = default;
+
+	void onActivate() override
+	{
+		setpoints().configureSetpointsSync(SetpointSender::SetpointConfiguration{});
+		++num_activations;
+	}
+
+	void checkArmingAndRunConditions(HealthAndArmingCheckReporter &reporter) override
+	{
+		if (check_should_fail) {
+			/* EVENT
+			 */
+			reporter.armingCheckFailureExt(events::ID("check_custom_mode_test_failure"),
+						       events::Log::Error, "Custom check failed");
+		}
+
+		++num_arming_check_updates;
+	}
+
+	void onDeactivate() override
+	{
+		++num_deactivations;
+	}
+
+	void updateSetpoint() override
+	{
+		++num_setpoint_updates;
+
+		// Send some random setpoints, make sure it stays in the air, we don't want it to land
+		Eigen::Vector3f velocity{1.f, 0.f, -0.5f };
+		setpoints().sendTrajectorySetpoint(velocity);
+	}
+
+	int num_activations{0};
+	int num_deactivations{0};
+	int num_setpoint_updates{0};
+	int num_arming_check_updates{0};
+	bool check_should_fail{false};
+private:
+};
+
+
+class TestExecution
+{
+public:
+	TestExecution(rclcpp::Node &node)
+		: _node(node), _vehicle_state(node) {}
+
+	void run();
+private:
+
+	enum class State {
+		ActivatingLand,
+		WaitForExternalMode,
+		WaitForArming,
+		WaitUntilInAir,
+		WaitForCustomMode,
+		WaitForHold,
+		WaitForFailsafe,
+		TerminateMode,
+		WaitForDisarm,
+	};
+
+	State _state{State::ActivatingLand};
+
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+	rclcpp::TimerBase::SharedPtr _testing_timer;
+
+	std::unique_ptr<FlightModeTest> _mode;
+	VehicleState _vehicle_state;
+	bool _was_armed{false};
+	uint8_t _current_nav_state{};
+	int _num_pre_flight_checks_pass{0};
+};
+
+void TestExecution::run()
+{
+	_test_timeout = _node.create_wall_timer(80s, [] {
+		EXPECT_TRUE(false); // Timed out
+		rclcpp::shutdown();
+	});
+
+	_mode = std::make_unique<FlightModeTest>(_node);
+
+	// Testing steps:
+	// - switch to descend
+	// - register mode, replace internal descend
+	// - ensure fmu switched to external mode
+	// - switch to takeoff
+	// - wait for arming
+	// - takeoff + wait
+	// - trigger extra failing mode check
+	// - try to switch into custom mode -> prevented
+	// - clear extra check
+	// - try to switch into custom mode -> works
+	// - switch to hold
+	// - trigger failsafe
+	// - ensure switch to external descend
+	// - terminate mode -> switch to internal
+	// - wait for disarming
+
+
+	// First, switch to Descend and wait for it
+	RCLCPP_INFO(_node.get_logger(), "Activating Land");
+	_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+				   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+	_vehicle_state.callbackOnModeSet([this]() {
+		_state = State::WaitForExternalMode;
+		RCLCPP_INFO(_node.get_logger(), "Registering");
+		ASSERT_TRUE(_mode->doRegister());
+
+		// We expect to switch into our custom mode, as it replaces the currently selected one
+		_vehicle_state.callbackOnModeSet([this]() {
+			_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+						   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF);
+			_state = State::WaitForArming;
+			RCLCPP_INFO(_node.get_logger(), "Wait for arming");
+
+		}, _mode->id());
+
+	}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+
+
+	_vehicle_state.setOnVehicleStatusUpdate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) {
+		bool armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+		_current_nav_state = msg->nav_state;
+
+		if (_state == State::WaitForArming) {
+			if (msg->pre_flight_checks_pass) {
+				++_num_pre_flight_checks_pass;
+
+			} else {
+				_num_pre_flight_checks_pass = 0;
+			}
+
+			if (_num_pre_flight_checks_pass >
+			    3) { // Make sure we check after the mode switch happened (as we don't wait for an ack)
+				RCLCPP_INFO(_node.get_logger(), "Arming possible");
+				_state = State::WaitUntilInAir;
+				_mode->check_should_fail = true;
+				_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f);
+				_testing_timer = _node.create_wall_timer(5s, [this] {
+					EXPECT_TRUE(_was_armed);
+					_testing_timer.reset();
+					RCLCPP_INFO(_node.get_logger(), "In air, checking mode switch prevented");
+					_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+								   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+
+					_testing_timer = _node.create_wall_timer(1s, [this] {
+						_testing_timer.reset();
+						// Mode did not switch to Descend/our mode, as the custom check prevented it
+						EXPECT_NE(_current_nav_state, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+						EXPECT_NE(_current_nav_state, _mode->id());
+						RCLCPP_INFO(_node.get_logger(), "Current mode: %i", _current_nav_state);
+
+						// Now clear the check and try again
+						_mode->check_should_fail = false;
+						_testing_timer = _node.create_wall_timer(1s, [this] {
+							_testing_timer.reset();
+							RCLCPP_INFO(_node.get_logger(), "In air, checking mode switch again");
+							// This must activate our mode
+							_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+										   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+							_state = State::WaitForCustomMode;
+						});
+					});
+				});
+			}
+
+		} else if (_state == State::WaitForCustomMode) {
+			if (msg->nav_state == _mode->id()) {
+				RCLCPP_INFO(_node.get_logger(), "Custom mode active, switching to hold");
+				_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+							   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LOITER);
+				_state = State::WaitForHold;
+			}
+
+		} else if (_state == State::WaitForHold) {
+			if (msg->nav_state == px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LOITER) {
+				RCLCPP_INFO(_node.get_logger(), "Hold mode active, triggering failsafe");
+				_vehicle_state.setGPSFailure(true);
+				_state = State::WaitForFailsafe;
+
+				// Failsafe must switch into our mode
+				_vehicle_state.callbackOnModeSet([this]() {
+					RCLCPP_INFO(_node.get_logger(), "Custom mode got activated, stopping custom mode");
+					_state = State::TerminateMode;
+
+					// Now stop the mode
+					EXPECT_GT(_mode->num_activations, 0);
+					EXPECT_GT(_mode->num_deactivations, 0);
+					EXPECT_GT(_mode->num_setpoint_updates, 1);
+					EXPECT_GT(_mode->num_arming_check_updates, 1);
+					_mode.reset();
+
+					// The FMU must fall back to the internal mode
+					_vehicle_state.callbackOnModeSet([this]() {
+						RCLCPP_INFO(_node.get_logger(), "Descend mode got activated, waiting for landing & disarm");
+						_state = State::WaitForDisarm;
+					}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+
+				}, _mode->id());
+			}
+		}
+
+		if (_was_armed && !armed && _state == State::WaitForDisarm) {
+			// Disarming, complete the test
+			_vehicle_state.setGPSFailure(false);
+			rclcpp::shutdown();
+		}
+
+		_was_armed = armed;
+	});
+}
+
+TEST_F(Tester, run_mode_tests)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecution test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}
+

src/ros2/cpp/px4_sdk_lib/test/integration/mode_executor.cpp

@@ -0,0 +1,503 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+
+#include "util.h"
+
+#include <px4_sdk/components/mode_executor.h>
+#include <px4_sdk/components/wait_for_fmu.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <Eigen/Core>
+
+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,
+		MyMode = 4,
+		RTL = 5,
+		WaitUntilDisarmed = 6,
+		Completed = 7,
+	};
+
+	void onActivate() override
+	{
+		++num_activations;
+		runState(State::WaitForArming, Result::Success);
+	}
+
+	void onDeactivate(DeactivateReason reason) override
+	{
+		++num_deactivations;
+	}
+
+	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);
+
+		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::MyMode, result); });
+			break;
+
+		case State::MyMode:
+			scheduleMode(ownedMode().id(), [this](Result result) { runState(State::RTL, result); });
+			break;
+
+		case State::RTL:
+			rtl([this](Result result) { runState(State::WaitUntilDisarmed, result); });
+			break;
+
+		case State::WaitUntilDisarmed:
+			waitUntilDisarmed([this](Result result) { runState(State::Completed, result); });
+			break;
+
+		case State::Completed:
+			on_completed();
+			rclcpp::shutdown();
+			break;
+		}
+	}
+
+	int num_activations{0};
+	int num_deactivations{0};
+
+	std::function<void()> on_completed;
+	std::function<void(State, Result)> on_state_completed;
+
+private:
+	rclcpp::Node &_node;
+};
+
+
+class TestExecutionAutonomous
+{
+public:
+	TestExecutionAutonomous(rclcpp::Node &node)
+		: _node(node) {}
+
+	void run();
+private:
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+
+	std::unique_ptr<FlightModeTest> _mode;
+	std::unique_ptr<ModeExecutorTest> _mode_executor;
+};
+
+void TestExecutionAutonomous::run()
+{
+	_test_timeout = _node.create_wall_timer(80s, [] {
+		EXPECT_TRUE(false); // Timed out
+		rclcpp::shutdown();
+	});
+
+	_mode = std::make_unique<FlightModeTest>(_node);
+	_mode_executor = std::make_unique<ModeExecutorTest>(_node, *_mode.get(), true);
+
+
+	// The executor is expected to be activated and then run through its states (successfully)
+
+	_mode_executor->on_completed = [this]() {
+		EXPECT_EQ(_mode_executor->num_activations, 1);
+		EXPECT_EQ(_mode_executor->num_deactivations, 0);
+		EXPECT_EQ(_mode->num_activations, 1);
+		EXPECT_EQ(_mode->num_deactivations, 1);
+		EXPECT_GT(_mode->num_setpoint_updates, 1);
+		EXPECT_GT(_mode->num_arming_check_updates, 1);
+	};
+	_mode_executor->on_state_completed = [](ModeExecutorTest::State, Result result) {
+		EXPECT_EQ(result, Result::Success);
+	};
+
+	ASSERT_TRUE(_mode_executor->doRegister());
+}
+
+TEST_F(Tester, run_executor_autonomous)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecutionAutonomous test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}
+
+
+class TestExecutionInCharge
+{
+public:
+	TestExecutionInCharge(rclcpp::Node &node)
+		: _node(node), _vehicle_state(node) {}
+
+	void run();
+private:
+
+	enum class State {
+		WaitForArming,
+		TryToArm,
+		SwitchMode,
+		Arming,
+		WaitForTakeoff,
+		WaitForRTL,
+		WaitForDisarm,
+	};
+
+	State _state{State::WaitForArming};
+
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+
+	std::unique_ptr<FlightModeTest> _mode;
+	std::unique_ptr<ModeExecutorTest> _mode_executor;
+	VehicleState _vehicle_state;
+	bool _was_armed{false};
+	bool _land_activated{false};
+};
+
+void TestExecutionInCharge::run()
+{
+	_test_timeout = _node.create_wall_timer(80s, [] {
+		EXPECT_TRUE(false); // Timed out
+		rclcpp::shutdown();
+	});
+
+	_mode = std::make_unique<FlightModeTest>(_node);
+	_mode_executor = std::make_unique<ModeExecutorTest>(_node, *_mode.get(), false);
+
+
+	// Testing steps:
+	// - Wait until we can arm
+	// - try to arm via executor -> must fail because it's not in charge
+	// - switch into owned mode + arm
+	// - when in takeoff, switch into owned mode: must trigger executor re-activation
+	// - wait until in owned mode
+	// - switch to descend mode -> executor gets deactivated
+	// - wait until disarmed
+
+	_vehicle_state.setOnVehicleStatusUpdate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) {
+		bool armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+
+		if (_state == State::WaitForArming) {
+			if (msg->pre_flight_checks_pass) {
+				RCLCPP_INFO(_node.get_logger(), "Arming possible");
+				_state = State::TryToArm;
+				_mode_executor->arm([this](Result result) {
+					ASSERT_EQ(result, Result::Rejected);
+					RCLCPP_INFO(_node.get_logger(), "Arming rejected, switching mode");
+					_state = State::SwitchMode;
+					_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE, _mode->id());
+
+					_vehicle_state.callbackOnModeSet([this]() {
+						RCLCPP_INFO(_node.get_logger(), "Mode Activated, arming");
+						_state = State::Arming;
+						_mode_executor->arm([this](Result result) {
+							ASSERT_EQ(result, Result::Success);
+							RCLCPP_INFO(_node.get_logger(), "Armed");
+
+							// Now the executor must take over and trigger a takeoff
+							_state = State::WaitForTakeoff;
+							_vehicle_state.callbackOnModeSet([this]() {
+								RCLCPP_INFO(_node.get_logger(), "Takeoff Activated, switching to owned mode");
+								_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE, _mode->id());
+								_state = State::WaitForRTL;
+
+							}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF);
+
+						});
+					}, _mode->id());
+				});
+			}
+		}
+
+		if (_was_armed && !armed && _state == State::WaitForDisarm) {
+
+			// Disarming, complete the test
+			EXPECT_TRUE(_land_activated);
+
+			EXPECT_EQ(_mode_executor->num_activations, 2);
+			EXPECT_EQ(_mode_executor->num_deactivations, 2);
+			EXPECT_EQ(_mode->num_activations, 1);
+			EXPECT_EQ(_mode->num_deactivations, 1);
+			EXPECT_GT(_mode->num_setpoint_updates, 1);
+			EXPECT_GT(_mode->num_arming_check_updates, 1);
+
+			rclcpp::shutdown();
+		}
+
+		_was_armed = armed;
+	});
+
+
+	_mode_executor->on_completed = []() {
+		EXPECT_FALSE(true); // never completed
+	};
+	_mode_executor->on_state_completed = [this](ModeExecutorTest::State next_state, Result result) {
+
+		if (_state == State::WaitForRTL && next_state == ModeExecutorTest::State::RTL) {
+			EXPECT_EQ(result, Result::Success);
+			EXPECT_EQ(_mode_executor->num_activations, 2);
+			EXPECT_EQ(_mode_executor->num_deactivations, 1);
+
+			// Wait until RTL actually activated (otherwise we can have a race condition with the previous command from the executor)
+			_vehicle_state.callbackOnModeSet([this]() {
+				RCLCPP_INFO(_node.get_logger(), "Activating Land");
+				_vehicle_state.sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE,
+							   px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+				_vehicle_state.callbackOnModeSet([this]() {
+					RCLCPP_INFO(_node.get_logger(), "Landing activated");
+					_land_activated = true;
+				}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+
+				_state = State::WaitForDisarm;
+			}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_RTL);
+
+		} else if (_state == State::WaitForRTL && next_state == ModeExecutorTest::State::MyMode) {
+			// We can get here twice, when switching into the owned mode and after re-activation
+
+		} else if (_state == State::WaitForDisarm) {
+			EXPECT_TRUE(result == Result::Deactivated || result == Result::Rejected); // Switching to land deactivated the executor
+
+		} else {
+			EXPECT_EQ(result, Result::Success);
+		}
+	};
+
+	ASSERT_TRUE(_mode_executor->doRegister());
+}
+
+TEST_F(Tester, run_executor_in_charge)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecutionInCharge test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}
+
+class TestExecutionFailsafe
+{
+public:
+	TestExecutionFailsafe(rclcpp::Node &node)
+		: _node(node), _vehicle_state(node) {}
+
+	void run();
+private:
+	rclcpp::Node &_node;
+	rclcpp::TimerBase::SharedPtr _test_timeout;
+
+	std::unique_ptr<FlightModeTest> _mode;
+	std::unique_ptr<ModeExecutorTest> _mode_executor;
+	VehicleState _vehicle_state;
+	bool _failsafe_triggered{false};
+	bool _land_activated{false};
+	bool _was_armed{false};
+};
+
+void TestExecutionFailsafe::run()
+{
+	_test_timeout = _node.create_wall_timer(80s, [] {
+		EXPECT_TRUE(false); // Timed out
+		rclcpp::shutdown();
+	});
+
+	_mode = std::make_unique<FlightModeTest>(_node);
+	_mode_executor = std::make_unique<ModeExecutorTest>(_node, *_mode.get(), true);
+
+
+	// Run the executor and trigger a failsafe (descend) while it's in the custom mode
+
+	_mode_executor->on_completed = []() {
+		EXPECT_FALSE(true); // never completed
+	};
+	_mode_executor->on_state_completed = [this](ModeExecutorTest::State next_state, Result result) {
+
+		if (next_state == ModeExecutorTest::State::TakingOff) {
+
+			_vehicle_state.callbackOnModeSet([this]() {
+				RCLCPP_INFO(_node.get_logger(), "Owned mode started, triggering failsafe");
+				_vehicle_state.setGPSFailure(true);
+				_failsafe_triggered = true;
+			}, _mode->id());
+
+			EXPECT_EQ(result, Result::Success);
+
+		} else if (next_state == ModeExecutorTest::State::RTL) {
+			EXPECT_EQ(result, Result::Deactivated); // Expect the failsafe to have triggered in MyMode
+
+			_vehicle_state.callbackOnModeSet([this]() {
+				RCLCPP_INFO(_node.get_logger(), "Landing activated");
+				_land_activated = true;
+			}, px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND);
+
+
+		} 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) {
+
+			_vehicle_state.setGPSFailure(false);
+
+			// Disarming, complete the test
+			EXPECT_TRUE(_land_activated);
+			EXPECT_TRUE(_failsafe_triggered);
+
+			EXPECT_EQ(_mode_executor->num_activations, 1);
+			EXPECT_EQ(_mode_executor->num_deactivations, 1);
+			EXPECT_EQ(_mode->num_activations, 1);
+			EXPECT_EQ(_mode->num_deactivations, 1);
+			EXPECT_GT(_mode->num_setpoint_updates, 1);
+			EXPECT_GT(_mode->num_arming_check_updates, 1);
+
+			rclcpp::shutdown();
+		}
+
+		_was_armed = armed;
+	});
+
+	ASSERT_TRUE(_mode_executor->doRegister());
+}
+
+TEST_F(Tester, run_executor_failsafe)
+{
+	auto test_node = initNode();
+	ASSERT_TRUE(waitForFMU(*test_node, 10s));
+	TestExecutionFailsafe test_execution{*test_node};
+	test_execution.run();
+	rclcpp::spin(test_node);
+}

src/ros2/cpp/px4_sdk_lib/test/integration/util.cpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   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 "util.h"
+#include <gtest/gtest.h>
+
+std::shared_ptr<rclcpp::Node> initNode()
+{
+	auto test_node = std::make_shared<rclcpp::Node>("testnode");
+	auto ret = rcutils_logging_set_logger_level(test_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+
+	if (ret != RCUTILS_RET_OK) {
+		RCLCPP_ERROR(test_node->get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
+		rcutils_reset_error();
+	}
+
+	return test_node;
+}
+
+VehicleState::VehicleState(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+	: _node(node)
+{
+	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
+				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
+		if (_on_vehicle_status_update) {
+			_on_vehicle_status_update(msg);
+		}
+
+		if (_on_mode_set_callback && _waiting_for_nav_state == msg->nav_state) {
+			if (++_matching_nav_state_set > 2) { // wait a bit longer for the mode to be set, and mode activation triggered
+				// clear before executing the callback as it might trigger another one
+				ModeSetCallback cb(std::move(_on_mode_set_callback));
+				_on_mode_set_callback = nullptr;
+				cb();
+			}
+		}
+	});
+
+	_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
+				       topic_namespace_prefix + "/fmu/in/vehicle_command", 1);
+
+}
+
+void VehicleState::callbackOnModeSet(const VehicleState::ModeSetCallback &callback, uint8_t nav_state)
+{
+	assert(_on_mode_set_callback == nullptr);
+	_on_mode_set_callback = callback;
+	_waiting_for_nav_state = nav_state;
+	_matching_nav_state_set = 0;
+}
+
+void VehicleState::sendCommand(uint32_t command, float param1, float param2, float param3, float param4,
+			       float param5, float param6, float param7)
+{
+	// Send command, don't wait for ack
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = command;
+	cmd.param1 = param1;
+	cmd.param2 = param2;
+	cmd.param3 = param3;
+	cmd.param4 = param4;
+	cmd.param5 = param5;
+	cmd.param6 = param6;
+	cmd.param7 = param7;
+	cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_vehicle_command_pub->publish(cmd);
+}
+
+void VehicleState::setGPSFailure(bool failure)
+{
+	sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_INJECT_FAILURE,
+		    px4_msgs::msg::VehicleCommand::FAILURE_UNIT_SENSOR_GPS,
+		    failure ? px4_msgs::msg::VehicleCommand::FAILURE_TYPE_OFF : px4_msgs::msg::VehicleCommand::FAILURE_TYPE_OK, 0);
+}
+
+void VehicleState::setForceLowBattery(bool enabled)
+{
+	sendCommand(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_INJECT_FAILURE,
+		    px4_msgs::msg::VehicleCommand::FAILURE_UNIT_SYSTEM_BATTERY,
+		    enabled ? px4_msgs::msg::VehicleCommand::FAILURE_TYPE_OFF : px4_msgs::msg::VehicleCommand::FAILURE_TYPE_OK, 0);
+}

src/ros2/cpp/px4_sdk_lib/test/integration/util.h

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+
+std::shared_ptr<rclcpp::Node> initNode();
+
+class VehicleState
+{
+public:
+	VehicleState(rclcpp::Node &node, const std::string &topic_namespace_prefix = "");
+
+	using VehicleStatusUpdateCallback = std::function<void(const px4_msgs::msg::VehicleStatus::UniquePtr &)>;
+
+	using ModeSetCallback = std::function<void()>;
+
+	const VehicleStatusUpdateCallback &getOnVehicleStatusUpdate() const
+	{ return _on_vehicle_status_update; }
+
+	void setOnVehicleStatusUpdate(const VehicleStatusUpdateCallback &on_vehicle_status_update)
+	{ _on_vehicle_status_update = on_vehicle_status_update; }
+
+	void callbackOnModeSet(const ModeSetCallback &callback, uint8_t nav_state);
+
+	void sendCommand(uint32_t command, float param1 = NAN, float param2 = NAN, float param3 = NAN, float param4 = NAN,
+			 float param5 = NAN, float param6 = NAN, float param7 = NAN);
+
+	// Methods to trigger failsafes
+	void setGPSFailure(bool failure);
+	void setForceLowBattery(bool enabled);
+private:
+	rclcpp::Node &_node;
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr _vehicle_status_sub;
+	rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr _vehicle_command_pub;
+	VehicleStatusUpdateCallback _on_vehicle_status_update;
+	ModeSetCallback _on_mode_set_callback;
+	uint8_t _waiting_for_nav_state{};
+	int _matching_nav_state_set{0};
+};