From ad7dc7944ef8a61f64fff749efb31ad5eb73e7c6 Mon Sep 17 00:00:00 2001
From: Bastian <bastian.h.jaeger@gmail.com>
Date: Wed, 14 Feb 2024 15:49:36 +0100
Subject: [PATCH] WIP

---
 .../CMakeLists.txt                            |  34 ++++
 .../include/mode.hpp                          | 171 ++++++++++++++++++
 .../package.xml                               |  26 +++
 .../src/main.cpp                              |  17 ++
 4 files changed, 248 insertions(+)
 create mode 100644 examples/cpp/modes/mode_with_interrupting_executor/CMakeLists.txt
 create mode 100644 examples/cpp/modes/mode_with_interrupting_executor/include/mode.hpp
 create mode 100644 examples/cpp/modes/mode_with_interrupting_executor/package.xml
 create mode 100644 examples/cpp/modes/mode_with_interrupting_executor/src/main.cpp

diff --git a/examples/cpp/modes/mode_with_interrupting_executor/CMakeLists.txt b/examples/cpp/modes/mode_with_interrupting_executor/CMakeLists.txt
new file mode 100644
index 00000000..5d09a9b8
--- /dev/null
+++ b/examples/cpp/modes/mode_with_interrupting_executor/CMakeLists.txt
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(example_mode_with_interrupting_executor_cpp)
+
+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()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(px4_ros2_cpp REQUIRED)
+
+include_directories(include ${Eigen3_INCLUDE_DIRS})
+add_executable(example_mode_with_interrupting_executor_cpp
+        src/main.cpp)
+ament_target_dependencies(example_mode_with_interrupting_executor_cpp Eigen3 px4_ros2_cpp rclcpp)
+
+install(TARGETS
+        example_mode_with_interrupting_executor_cpp
+        DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/examples/cpp/modes/mode_with_interrupting_executor/include/mode.hpp b/examples/cpp/modes/mode_with_interrupting_executor/include/mode.hpp
new file mode 100644
index 00000000..d8b9cc90
--- /dev/null
+++ b/examples/cpp/modes/mode_with_interrupting_executor/include/mode.hpp
@@ -0,0 +1,171 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/components/mode_executor.hpp>
+#include <px4_ros2/components/wait_for_fmu.hpp>
+#include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <Eigen/Core>
+
+using namespace std::chrono_literals; // NOLINT
+
+static const std::string kName = "OA Mission";
+static const std::string kNodeName = "example_mode_with_interrupting_executor";
+
+class FlightModeTest : public px4_ros2::ModeBase
+{
+public:
+  explicit FlightModeTest(rclcpp::Node & node)
+  : ModeBase(node, Settings{kName, false})
+  {
+    _trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
+  }
+
+  ~FlightModeTest() override = default;
+
+  void onActivate() override
+  {
+    _activation_time = node().get_clock()->now();
+  }
+
+  void onDeactivate() override {}
+
+  void updateSetpoint(float dt_s) override
+  {
+    const Eigen::Vector3f velocity{0.f, 0.f, 0.f};
+    _trajectory_setpoint->update(velocity);
+  }
+
+private:
+  rclcpp::Time _activation_time{};
+  std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+};
+
+class ModeExecutorTest : public px4_ros2::ModeExecutorBase
+{
+public:
+  ModeExecutorTest(rclcpp::Node & node, px4_ros2::ModeBase & owned_mode)
+  : ModeExecutorBase(node, px4_ros2::ModeExecutorBase::Settings{}, owned_mode),
+    _node(node)
+  {
+  }
+
+  enum class State
+  {
+    Reset,
+    TakingOff,
+    Mission,
+    OaHold,
+    RTL,
+    WaitUntilDisarmed,
+  };
+
+  void onActivate() override
+  {
+    runState(State::TakingOff, px4_ros2::Result::Success);
+  }
+
+  void onDeactivate(DeactivateReason reason) override
+  {
+  }
+
+  void abortMission()
+  {
+    std::this_thread::sleep_for(std::chrono::seconds(10));
+    RCLCPP_WARN(_node.get_logger(), "Aborting mission");
+    runState(State::OaHold, px4_ros2::Result::Deactivated);
+  }
+
+  void runState(State state, px4_ros2::Result previous_result)
+  {
+    if ( (previous_result != px4_ros2::Result::Success) &&
+      (previous_result != px4_ros2::Result::Deactivated))
+    {
+      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::TakingOff:
+        takeoff([this](px4_ros2::Result result) {runState(State::Mission, result);});
+        break;
+
+      case State::Mission:
+        _my_future = std::async(std::launch::async, &ModeExecutorTest::abortMission, this);
+        scheduleMode(
+          px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_MISSION,
+          [this](px4_ros2::Result result) {
+            runState(State::RTL, result);
+          });
+        break;
+
+      case State::OaHold:
+        scheduleMode(
+          ownedMode().id(), [this](px4_ros2::Result result) {
+            runState(State::RTL, result);
+          });
+        break;
+
+      case State::RTL:
+        rtl([this](px4_ros2::Result result) {runState(State::WaitUntilDisarmed, result);});
+        break;
+
+      case State::WaitUntilDisarmed:
+        waitUntilDisarmed(
+          [this](px4_ros2::Result result) {
+            RCLCPP_INFO(_node.get_logger(), "All states complete (%s)", resultToString(result));
+          });
+        break;
+    }
+  }
+
+private:
+  rclcpp::Node & _node;
+
+  std::future<void> _my_future;
+};
+
+class TestNode : public rclcpp::Node
+{
+public:
+  TestNode()
+  : Node(kNodeName)
+  {
+    // Enable debug output
+    auto ret =
+      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+
+    if (ret != RCUTILS_RET_OK) {
+      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
+      rcutils_reset_error();
+    }
+
+    if (!px4_ros2::waitForFMU(*this)) {
+      throw std::runtime_error("No message from FMU");
+    }
+
+    _mode = std::make_unique<FlightModeTest>(*this);
+    _mode_executor = std::make_unique<ModeExecutorTest>(*this, *_mode);
+
+    if (!_mode_executor->doRegister()) {
+      throw std::runtime_error("Registration failed");
+    }
+  }
+
+private:
+  std::unique_ptr<FlightModeTest> _mode;
+  std::unique_ptr<ModeExecutorTest> _mode_executor;
+};
diff --git a/examples/cpp/modes/mode_with_interrupting_executor/package.xml b/examples/cpp/modes/mode_with_interrupting_executor/package.xml
new file mode 100644
index 00000000..0d957314
--- /dev/null
+++ b/examples/cpp/modes/mode_with_interrupting_executor/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>example_mode_with_interrupting_executor_cpp</name>
+  <version>0.0.1</version>
+  <description>Example mode: Mode with Interrupting Executor</description>
+  <maintainer email="beat-kueng@gmx.net">Beat Kueng</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_export_depend>eigen</build_export_depend>
+
+  <depend>px4_ros2_cpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/examples/cpp/modes/mode_with_interrupting_executor/src/main.cpp b/examples/cpp/modes/mode_with_interrupting_executor/src/main.cpp
new file mode 100644
index 00000000..2ce76220
--- /dev/null
+++ b/examples/cpp/modes/mode_with_interrupting_executor/src/main.cpp
@@ -0,0 +1,17 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <mode.hpp>
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::shutdown();
+  return 0;
+}