Add local global conversion

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

@@ -125,7 +125,7 @@ class FlightModeTest : public px4_ros2::ModeBase
         break;
 
       case State::GoingSouthwest: {
-          // go to southwest corner while facing the northeastern corner
+          // go to southwest corner while facing the northwestern 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 -

examples/cpp/modes/goto_global/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(example_mode_goto_global_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_global
+        src/main.cpp)
+ament_target_dependencies(example_mode_goto_global Eigen3 px4_ros2_cpp rclcpp)
+
+install(TARGETS
+        example_mode_goto_global
+        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_global/include/mode.hpp

@@ -0,0 +1,224 @@
+/****************************************************************************
+ * Copyright (c) 2024 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/control/setpoint_types/goto.hpp>
+#include <px4_ros2/odometry/global_position.hpp>
+#include <px4_ros2/utils/geometry.hpp>
+#include <px4_ros2/utils/geodesic.hpp>
+#include <px4_msgs/msg/vehicle_attitude.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include <algorithm>
+
+static const std::string kName = "Go-to Global Example";
+static const std::string kNodeName = "example_mode_goto_global";
+
+using namespace px4_ros2::literals; // NOLINT
+
+class FlightModeTest : public px4_ros2::ModeBase
+{
+public:
+  explicit FlightModeTest(rclcpp::Node & node)
+  : ModeBase(node, kName)
+  {
+    _goto_setpoint = std::make_shared<px4_ros2::GotoGlobalSetpointType>(*this);
+
+    _vehicle_global_position = std::make_shared<px4_ros2::OdometryGlobalPosition>(*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::SettlingAtStart;
+    _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_global_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::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition(
+            _start_position_m, Eigen::Vector3f{kTriangleHeight, 0.f, 0.f});
+
+          const float heading_target_rad = px4_ros2::headingToGlobalPosition(
+            _vehicle_global_position->position(), target_position_m);
+
+          if (px4_ros2::horizontalDistanceToGlobalPosition(
+              _vehicle_global_position->position(),
+              target_position_m) < 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::Vector3d target_position_m = px4_ros2::addVectorToGlobalPosition(
+            _start_position_m, Eigen::Vector3f{kTriangleHeight, kTriangleWidth, 0.f});
+
+          // scale the speed limits by distance to the target
+          const float distance_to_target = px4_ros2::horizontalDistanceToGlobalPosition(
+            _vehicle_global_position->position(), target_position_m);
+          const float speed_scale = std::min(distance_to_target / 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 =
+            px4_ros2::degToRad(45.f * speed_scale + (1.f - speed_scale) * 25.f);
+          const float heading_setpoint_rate_of_change =
+            px4_ros2::degToRad(40.f * speed_scale + (1.f - speed_scale) * 20.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 northwestern corner
+          const Eigen::Vector2d position_of_interest_m = px4_ros2::addVectorToGlobalPosition(
+            _start_position_m.head(
+              2), Eigen::Vector2f{kTriangleHeight, 0.f});
+          const float heading_target_rad = px4_ros2::headingToGlobalPosition(
+            _vehicle_global_position->position().head(2), position_of_interest_m);
+
+          _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::Vector3d _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::GotoGlobalSetpointType> _goto_setpoint;
+  std::shared_ptr<px4_ros2::OdometryGlobalPosition> _vehicle_global_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 = px4_ros2::quaternionToEulerRpy(q);
+    _vehicle_heading_rad = rpy(2);
+  }
+
+  bool positionReached(const Eigen::Vector3d & target_position_m) const
+  {
+    static constexpr float kPositionErrorThreshold = 0.5f; // [m]
+    const float position_error = px4_ros2::distanceToGlobalPosition(
+      _vehicle_global_position->position(), target_position_m);
+    return position_error < kPositionErrorThreshold;
+  }
+
+  bool headingReached(float target_heading_rad) const
+  {
+    static constexpr float kHeadingErrorThreshold = 7.0_deg;
+    const float heading_error_wrapped = px4_ros2::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_global/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_global_cpp</name>
+  <version>0.0.1</version>
+  <description>Example mode: Go-to global</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_global/src/main.cpp

@@ -0,0 +1,17 @@
+/****************************************************************************
+ * Copyright (c) 2024 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;
+}

px4_ros2_cpp/CMakeLists.txt

@@ -44,6 +44,7 @@ set(HEADER_FILES
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
         include/px4_ros2/utils/frame_conversion.hpp
+        include/px4_ros2/utils/geodesic.hpp
         include/px4_ros2/utils/geometry.hpp
 )
 
@@ -67,6 +68,8 @@ add_library(px4_ros2_cpp
         src/navigation/experimental/local_position_measurement_interface.cpp
         src/odometry/global_position.cpp
         src/odometry/local_position.cpp
+        src/utils/geodesic.cpp
+        src/utils/map_projection_impl.cpp
 )
 ament_target_dependencies(px4_ros2_cpp ament_index_cpp Eigen3 rclcpp px4_msgs)
 
@@ -129,7 +132,9 @@ if(BUILD_TESTING)
             test/unit/main.cpp
             test/unit/modes.cpp
             test/unit/utils/frame_conversion.cpp
+            test/unit/utils/geodesic.cpp
             test/unit/utils/geometry.cpp
+            test/unit/utils/map_projection_impl.cpp
     )
     target_include_directories(${PROJECT_NAME}_unit_tests PRIVATE ${CMAKE_CURRENT_LIST_DIR})
     target_link_libraries(${PROJECT_NAME}_unit_tests ${PROJECT_NAME} unit_utils)

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp

@@ -10,6 +10,7 @@
 #include <optional>
 
 #include <px4_ros2/common/setpoint_base.hpp>
+#include <px4_ros2/utils/geodesic.hpp>
 
 namespace px4_ros2
 {
@@ -53,5 +54,37 @@ class GotoSetpointType : public SetpointBase
     _goto_setpoint_pub;
 };
 
+class GotoGlobalSetpointType
+{
+public:
+  explicit GotoGlobalSetpointType(Context & context);
+
+  ~GotoGlobalSetpointType() = default;
+
+  /**
+   * @brief Go-to global setpoint update
+   *
+   * Unset optional values are not controlled
+   *
+   * @param global_position latitude [deg], longitude [deg], altitude AMSL [m]
+   * @param heading [rad] from North
+   * @param max_horizontal_speed [m/s] in NE-datum of NED earth-fixed frame
+   * @param max_vertical_speed [m/s] in D-axis of NED earth-fixed frame
+   * @param max_heading_rate [rad/s] about D-axis of NED earth-fixed frame
+   */
+  void update(
+    const Eigen::Vector3d & global_position,
+    const std::optional<float> & heading = {},
+    const std::optional<float> & max_horizontal_speed = {},
+    const std::optional<float> & max_vertical_speed = {},
+    const std::optional<float> & max_heading_rate = {}
+  );
+
+private:
+  rclcpp::Node & _node;
+  std::unique_ptr<MapProjection> _map_projection;
+  std::shared_ptr<GotoSetpointType> _goto_setpoint;
+};
+
 /** @}*/
 } /* namespace px4_ros2 */