examples/cpp/modes: add goto example

examples/cpp/modes/goto/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(example_mode_goto_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_goto
+        src/main.cpp)
+ament_target_dependencies(example_mode_goto Eigen3 px4_ros2_cpp rclcpp)
+
+install(TARGETS
+        example_mode_goto
+        DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

examples/cpp/modes/goto/include/mode.hpp

@@ -0,0 +1,222 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include "util.hpp"
+
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/control/setpoint_types/goto.hpp>
+#include <px4_ros2/odometry/local_position.hpp>
+#include <px4_msgs/msg/vehicle_attitude.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include <algorithm>
+
+static const std::string kName = "Go-to Example";
+static const std::string kNodeName = "example_mode_goto";
+
+class FlightModeTest : public px4_ros2::ModeBase
+{
+public:
+  explicit FlightModeTest(rclcpp::Node & node)
+  : ModeBase(node, kName)
+  {
+    _goto_setpoint = std::make_shared<px4_ros2::GotoSetpointType>(*this);
+
+    _vehicle_local_position = std::make_shared<px4_ros2::OdometryLocalPosition>(*this);
+
+    _vehicle_attitude_sub = node.create_subscription<px4_msgs::msg::VehicleAttitude>(
+      "/fmu/out/vehicle_attitude", rclcpp::QoS(1).best_effort(),
+      [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) {
+        updateVehicleHeading(msg);
+      }
+    );
+  }
+
+  void onActivate() override
+  {
+    _state = State::kSettlingAtStart;
+    _start_position_set = false;
+    _start_heading_set = false;
+  }
+
+  void onDeactivate() override {}
+
+  void updateSetpoint(float dt_s) override
+  {
+    if (!_start_position_set) {
+      _start_position_m = _vehicle_local_position->position();
+      _start_position_set = true;
+    }
+
+    switch (_state) {
+      case State::SettlingAtStart: {
+          // just settling at the starting vehicle position
+          _goto_setpoint->update(_start_position_m);
+          if (positionReached(_start_position_m)) {
+            _state = State::GoingNorth;
+          }
+        }
+        break;
+
+      case State::GoingNorth: {
+          // go north to the northwest corner facing in direction of travel
+          const Eigen::Vector3f target_position_m = _start_position_m +
+            Eigen::Vector3f{kTriangleHeight, 0.F, 0.F};
+
+          const Eigen::Vector2f vehicle_to_target_xy = target_position_m.head(2) -
+            _vehicle_local_position->position().head(2);
+          const float heading_target_rad = atan2f(vehicle_to_target_xy(1), vehicle_to_target_xy(0));
+
+          if (vehicle_to_target_xy.norm() < 0.1F) {
+            // stop caring about heading (the arctangent becomes undefined)
+            _goto_setpoint->update(target_position_m);
+          } else {
+            _goto_setpoint->update(target_position_m, heading_target_rad);
+          }
+
+          if (positionReached(target_position_m)) {
+            _state = State::GoingEast;
+          }
+        }
+        break;
+
+      case State::GoingEast: {
+          // go to the northeast corner while spinning
+          const Eigen::Vector3f target_position_m = _start_position_m +
+            Eigen::Vector3f{kTriangleHeight, kTriangleWidth, 0.F};
+
+          // scale the speed limits by distance to the target
+          const Eigen::Vector2f vehicle_to_target_xy = target_position_m.head(2) -
+            _vehicle_local_position->position().head(2);
+          const float speed_scale = std::min(vehicle_to_target_xy.norm() / kTriangleWidth, 1.F);
+
+          const float max_horizontal_velocity_m_s = 5.F * speed_scale + (1.F - speed_scale) * 1.F;
+          const float max_vertical_velocity_m_s = 3.F * speed_scale + (1.F - speed_scale) * 0.5F;
+          const float max_heading_rate_rad_s = (45.F * speed_scale + (1.F - speed_scale) * 25.F) *
+            (float)M_PI / 180.F;
+          const float heading_setpoint_rate_of_change =
+            (40.F * speed_scale + (1.F - speed_scale) * 20.F) * (float)M_PI / 180.F;
+
+          if (!_start_heading_set) {
+            _spinning_heading_rad = _vehicle_heading_rad;
+            _start_heading_set = true;
+          }
+
+          if (!positionReached(target_position_m)) {
+            _spinning_heading_rad += heading_setpoint_rate_of_change * dt_s;
+          }
+
+          _goto_setpoint->update(
+            target_position_m,
+            _spinning_heading_rad,
+            max_horizontal_velocity_m_s,
+            max_vertical_velocity_m_s,
+            max_heading_rate_rad_s);
+
+          if (positionReached(target_position_m)) {
+            _state = State::GoingSouthwest;
+          }
+        }
+        break;
+
+      case State::GoingSouthwest: {
+          // go to southwest corner while facing the northeastern corner
+          const Eigen::Vector2f position_of_interest_m = _start_position_m.head(2) +
+            Eigen::Vector2f{kTriangleHeight, 0.F};
+          const Eigen::Vector2f vehicle_to_poi_xy = position_of_interest_m -
+            _vehicle_local_position->position().head(2);
+          const float heading_target_rad = atan2f(vehicle_to_poi_xy(1), vehicle_to_poi_xy(0));
+
+          _goto_setpoint->update(_start_position_m, heading_target_rad);
+          if (positionReached(_start_position_m)) {
+            completed(px4_ros2::Result::Success);
+            return;
+          }
+        }
+        break;
+    }
+  }
+
+private:
+  static constexpr float kTriangleHeight = 20.F; // [m]
+  static constexpr float kTriangleWidth = 30.F; // [m]
+
+  enum class State
+  {
+    SettlingAtStart = 0,
+    GoingNorth,
+    GoingEast,
+    GoingSouthwest
+  } _state;
+
+  // NED earth-fixed frame. box pattern starting corner (first position the mode sees on activation)
+  Eigen::Vector3f _start_position_m;
+  bool _start_position_set{false};
+
+  // [-pi, pi] current vehicle heading from VehicleAttitude subscription
+  float _vehicle_heading_rad{0.F};
+
+  // [-pi, pi] current heading setpoint during spinning phase
+  float _spinning_heading_rad{0.F};
+
+  // used for heading initialization when dynamically updating heading setpoints
+  bool _start_heading_set{false};
+
+  std::shared_ptr<px4_ros2::GotoSetpointType> _goto_setpoint;
+  std::shared_ptr<px4_ros2::OdometryLocalPosition> _vehicle_local_position;
+  rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr _vehicle_attitude_sub;
+
+  void updateVehicleHeading(const px4_msgs::msg::VehicleAttitude::UniquePtr & msg)
+  {
+    const Eigen::Quaternionf q = Eigen::Quaternionf(msg->q[0], msg->q[1], msg->q[2], msg->q[3]);
+    const Eigen::Vector3f rpy = quaternionToEuler(q);
+    _vehicle_heading_rad = rpy(2);
+  }
+
+  bool positionReached(const Eigen::Vector3f & target_position_m) const
+  {
+    static constexpr float kPositionErrorThreshold = 0.5F; // [m]
+    static constexpr float kVelocityErrorThreshold = 0.3F; // [m/s]
+    const Eigen::Vector3f position_error_m = target_position_m -
+      _vehicle_local_position->position();
+    return (position_error_m.norm() < kPositionErrorThreshold) &&
+           (_vehicle_local_position->velocity().norm() < kVelocityErrorThreshold);
+  }
+
+  bool headingReached(float target_heading_rad) const
+  {
+    static constexpr float kHeadingErrorThreshold = 7.F * (float)M_PI / 180.F; // [rad]
+    const float heading_error_wrapped = wrapPi(target_heading_rad - _vehicle_heading_rad);
+    return fabsf(heading_error_wrapped) < kHeadingErrorThreshold;
+  }
+};
+
+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();
+    }
+
+    _mode = std::make_unique<FlightModeTest>(*this);
+
+    if (!_mode->doRegister()) {
+      throw std::runtime_error("Registration failed");
+    }
+  }
+
+private:
+  std::unique_ptr<FlightModeTest> _mode;
+};

examples/cpp/modes/goto/include/util.hpp

@@ -0,0 +1,45 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <Eigen/Eigen>
+#include <cmath>
+
+// !!! move these to some math/conversions dir !!!
+
+static inline Eigen::Vector3f quaternionToEuler(const Eigen::Quaternionf & q)
+{
+  auto dcm = q.toRotationMatrix();
+  using Type = float;
+  Eigen::Vector3f rpy;
+  rpy(1) = std::asin(-dcm(2, 0));
+
+  if ((std::fabs(rpy(1) - Type(M_PI / 2))) < Type(1.0e-3)) {
+    rpy(0) = 0;
+    rpy(2) = std::atan2(dcm(1, 2), dcm(0, 2));
+
+  } else if ((std::fabs(rpy(1) + Type(M_PI / 2))) < Type(1.0e-3)) {
+    rpy(0) = 0;
+    rpy(2) = std::atan2(-dcm(1, 2), -dcm(0, 2));
+
+  } else {
+    rpy(0) = std::atan2(dcm(2, 1), dcm(2, 2));
+    rpy(2) = std::atan2(dcm(1, 0), dcm(0, 0));
+  }
+  return rpy;
+}
+
+static inline float wrapPi(const float angle)
+{
+  const float m_pi_f = (float)M_PI;
+  if (-m_pi_f <= angle && angle < m_pi_f) {
+    return angle;
+  }
+
+  const float range = 2 * m_pi_f;
+  const float inv_range = 1.F / range;
+  const float num_wraps = std::floor((angle + m_pi_f) * inv_range);
+  return angle - range * num_wraps;
+}

examples/cpp/modes/goto/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_goto_cpp</name>
+  <version>0.0.1</version>
+  <description>Example mode: Go-to</description>
+  <maintainer email="[email protected]">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>

examples/cpp/modes/goto/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;
+}