From f82ce63f78c9448c467105aa777033633c74c9a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 18 Nov 2022 14:56:00 +0100 Subject: [PATCH] ROS2: add modes SDK --- src/ros2/cpp/px4_sdk_lib/CMakeLists.txt | 91 ++++ .../include/px4_sdk/components/events.h | 136 +++++ .../components/health_and_arming_checks.h | 187 +++++++ .../include/px4_sdk/components/mode.h | 188 +++++++ .../px4_sdk/components/mode_executor.h | 181 +++++++ .../include/px4_sdk/components/setpoints.h | 87 +++ .../include/px4_sdk/components/wait_for_fmu.h | 48 ++ src/ros2/cpp/px4_sdk_lib/package.xml | 25 + .../components/health_and_arming_checks.cpp | 118 ++++ .../cpp/px4_sdk_lib/src/components/mode.cpp | 180 +++++++ .../src/components/mode_executor.cpp | 368 +++++++++++++ .../src/components/registration.cpp | 176 ++++++ .../px4_sdk_lib/src/components/registration.h | 86 +++ .../px4_sdk_lib/src/components/setpoints.cpp | 89 ++++ .../src/components/wait_for_fmu.cpp | 78 +++ .../test/integration/arming_check.cpp | 150 ++++++ .../cpp/px4_sdk_lib/test/integration/mode.cpp | 297 +++++++++++ .../test/integration/mode_executor.cpp | 503 ++++++++++++++++++ .../cpp/px4_sdk_lib/test/integration/util.cpp | 112 ++++ .../cpp/px4_sdk_lib/test/integration/util.h | 73 +++ 20 files changed, 3173 insertions(+) create mode 100644 src/ros2/cpp/px4_sdk_lib/CMakeLists.txt create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/events.h create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/health_and_arming_checks.h create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/setpoints.h create mode 100644 src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/wait_for_fmu.h create mode 100644 src/ros2/cpp/px4_sdk_lib/package.xml create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/health_and_arming_checks.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/registration.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/registration.h create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/setpoints.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/src/components/wait_for_fmu.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/test/integration/arming_check.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/test/integration/mode.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/test/integration/mode_executor.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/test/integration/util.cpp create mode 100644 src/ros2/cpp/px4_sdk_lib/test/integration/util.h diff --git a/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt b/src/ros2/cpp/px4_sdk_lib/CMakeLists.txt new file mode 100644 index 0000000000..69568d4e06 --- /dev/null +++ b/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() diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/events.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/events.h new file mode 100644 index 0000000000..7d625a13a3 --- /dev/null +++ b/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 + +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 +inline constexpr void fillEventArguments(uint8_t *buf, T arg) +{ + // This assumes we're on little-endian + memcpy(buf, &arg, sizeof(T)); +} + +template +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 +constexpr unsigned sizeofArguments(const T &t, const Args &... args) +{ + return sizeof(T) + sizeofArguments(args...); +} + +} // namespace util + +/** + * Generate event ID from an event name + */ +template +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 + diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/health_and_arming_checks.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/health_and_arming_checks.h new file mode 100644 index 0000000000..9d4787ca81 --- /dev/null +++ b/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 +#include +#include + +#include "events.h" + +#include +#include + +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 + 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 + bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, Args... args); + + px4_msgs::msg::ArmingCheckReply &_arming_check_reply; +}; + + +template +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; + + 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 ®istration); + + void watchdogTimerUpdate(); + + rclcpp::Node &_node; + std::shared_ptr _registration; + CheckCallback _check_callback; + bool _check_triggered{true}; + + rclcpp::Subscription::SharedPtr _arming_check_request_sub; + rclcpp::Publisher::SharedPtr _arming_check_reply_pub; + + ModeRequirements _mode_requirements{}; + rclcpp::TimerBase::SharedPtr _watchdog_timer; + bool _shutdown_on_timeout{true}; +}; + +} /* namespace px4_sdk */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h new file mode 100644 index 0000000000..1654e5758d --- /dev/null +++ b/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 + +#include +#include +#include +#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 ®istration); + 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; + + const Settings _settings; + + HealthAndArmingChecks _health_and_arming_checks; + + rclcpp::Subscription::SharedPtr _vehicle_status_sub; + rclcpp::Publisher::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 */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h new file mode 100644 index 0000000000..36ae6af039 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include + +class Registration; + +namespace px4_sdk +{ + +class ModeExecutorBase +{ +public: + using CompletedCallback = std::function; + + 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::SharedPtr _mode_completed_sub; + }; + + class WaitForVehicleStatusCondition + { + public: + using RunCheckCallback = std::function; + 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; + + rclcpp::Subscription::SharedPtr _vehicle_status_sub; + rclcpp::Publisher::SharedPtr _vehicle_command_pub; + rclcpp::Subscription::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 */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/setpoints.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/setpoints.h new file mode 100644 index 0000000000..5b9ba6d30a --- /dev/null +++ b/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 + +#include +#include +#include + +#include + +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::SharedPtr _config_control_setpoints_pub; + rclcpp::Publisher::SharedPtr _trajectory_setpoint_pub; +}; + +} /* namespace px4_sdk */ diff --git a/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/wait_for_fmu.h b/src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/wait_for_fmu.h new file mode 100644 index 0000000000..9db790ac3e --- /dev/null +++ b/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 +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 */ diff --git a/src/ros2/cpp/px4_sdk_lib/package.xml b/src/ros2/cpp/px4_sdk_lib/package.xml new file mode 100644 index 0000000000..6c117c4eb5 --- /dev/null +++ b/src/ros2/cpp/px4_sdk_lib/package.xml @@ -0,0 +1,25 @@ + + + + px4_sdk_lib + 0.0.1 + Library to interface with PX4 from ROS2 + beat + BSD-3-Clause + + eigen3_cmake_module + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + px4_msgs + eigen + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/health_and_arming_checks.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/health_and_arming_checks.cpp new file mode 100644 index 0000000000..5ab2b70248 --- /dev/null +++ b/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 + +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(node, topic_namespace_prefix)), + _check_callback(check_callback) +{ + _arming_check_reply_pub = _node.create_publisher( + topic_namespace_prefix + "/fmu/in/arming_check_reply", 1); + + _arming_check_request_sub = _node.create_subscription( + 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 ®istration) +{ + 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; + } +} + diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp new file mode 100644 index 0000000000..0347945960 --- /dev/null +++ b/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 +#include + +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(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( + 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( + 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 ®istration) +{ + 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; +} diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp new file mode 100644 index 0000000000..a36f6b6c61 --- /dev/null +++ b/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 +#include +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(node, topic_namespace_prefix)), + _current_scheduled_mode(node, topic_namespace_prefix) +{ + _vehicle_status_sub = _node.create_subscription( + 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( + topic_namespace_prefix + "/fmu/in/vehicle_command_mode_executor", 1); + + _vehicle_command_ack_sub = _node.create_subscription( + 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( + 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 + } +} diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/registration.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/registration.cpp new file mode 100644 index 0000000000..abb4953d2b --- /dev/null +++ b/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 +#include + +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( + 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( + topic_namespace_prefix + "/fmu/in/register_ext_component_request", 1); + + _unregister_ext_component_pub = node.create_publisher( + 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 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; + } +} diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/registration.h b/src/ros2/cpp/px4_sdk_lib/src/components/registration.h new file mode 100644 index 0000000000..b3d82537b3 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +#include + +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::SharedPtr _register_ext_component_reply_sub; + rclcpp::Publisher::SharedPtr _register_ext_component_request_pub; + rclcpp::Publisher::SharedPtr _unregister_ext_component_pub; + + bool _registered{false}; + px4_msgs::msg::UnregisterExtComponent _unregister_ext_component{}; + rclcpp::Node &_node; +}; + diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/setpoints.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/setpoints.cpp new file mode 100644 index 0000000000..8b4cec7b3b --- /dev/null +++ b/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 + +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( + topic_namespace_prefix + "/fmu/in/config_control_setpoints", 1); + + _trajectory_setpoint_pub = node.create_publisher( + 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); +} diff --git a/src/ros2/cpp/px4_sdk_lib/src/components/wait_for_fmu.cpp b/src/ros2/cpp/px4_sdk_lib/src/components/wait_for_fmu.cpp new file mode 100644 index 0000000000..dc769819df --- /dev/null +++ b/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 + +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::SharedPtr vehicle_status_sub = + node.create_subscription( + 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()); + + 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; +} diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/arming_check.cpp b/src/ros2/cpp/px4_sdk_lib/test/integration/arming_check.cpp new file mode 100644 index 0000000000..f8737a2f9c --- /dev/null +++ b/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 + +#include "util.h" + +#include +#include + +#include + +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 _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(_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); +} diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/mode.cpp b/src/ros2/cpp/px4_sdk_lib/test/integration/mode.cpp new file mode 100644 index 0000000000..6b6c758f9b --- /dev/null +++ b/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 + +#include "util.h" + +#include +#include + +#include + +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 _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(_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); +} + diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/mode_executor.cpp b/src/ros2/cpp/px4_sdk_lib/test/integration/mode_executor.cpp new file mode 100644 index 0000000000..215e8c6ef5 --- /dev/null +++ b/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 + +#include "util.h" + +#include +#include + +#include + +#include + +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 on_completed; + std::function 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 _mode; + std::unique_ptr _mode_executor; +}; + +void TestExecutionAutonomous::run() +{ + _test_timeout = _node.create_wall_timer(80s, [] { + EXPECT_TRUE(false); // Timed out + rclcpp::shutdown(); + }); + + _mode = std::make_unique(_node); + _mode_executor = std::make_unique(_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 _mode; + std::unique_ptr _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(_node); + _mode_executor = std::make_unique(_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 _mode; + std::unique_ptr _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(_node); + _mode_executor = std::make_unique(_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); +} diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/util.cpp b/src/ros2/cpp/px4_sdk_lib/test/integration/util.cpp new file mode 100644 index 0000000000..7ae4dd8892 --- /dev/null +++ b/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 + +std::shared_ptr initNode() +{ + auto test_node = std::make_shared("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( + 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( + 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); +} diff --git a/src/ros2/cpp/px4_sdk_lib/test/integration/util.h b/src/ros2/cpp/px4_sdk_lib/test/integration/util.h new file mode 100644 index 0000000000..d74032ab86 --- /dev/null +++ b/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 +#include +#include + +std::shared_ptr initNode(); + +class VehicleState +{ +public: + VehicleState(rclcpp::Node &node, const std::string &topic_namespace_prefix = ""); + + using VehicleStatusUpdateCallback = std::function; + + using ModeSetCallback = std::function; + + 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::SharedPtr _vehicle_status_sub; + rclcpp::Publisher::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}; +};