control: add global goto setpoint

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::shared_ptr<MapProjection> _map_projection;
+  std::shared_ptr<GotoSetpointType> _goto_setpoint;
+};
+
 /** @}*/
 } /* namespace px4_ros2 */

px4_ros2_cpp/src/control/setpoint_types/goto.cpp

@@ -64,4 +64,34 @@ SetpointBase::Configuration GotoSetpointType::getConfiguration()
   config.climb_rate_enabled = true;
   return config;
 }
+
+GotoGlobalSetpointType::GotoGlobalSetpointType(Context & context)
+: _node(context.node()), _map_projection(std::make_shared<MapProjection>(context)),
+  _goto_setpoint(std::make_shared<GotoSetpointType>(context))
+{
+  RequirementFlags requirements{};
+  requirements.global_position = true;
+  context.setRequirement(requirements);
+}
+
+void GotoGlobalSetpointType::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)
+{
+  if (!_map_projection->isInitialized()) {
+    RCLCPP_ERROR(
+      _node.get_logger(),
+      "Goto global setpoint update failed: map projection is uninitialized. Is /fmu/out/vehicle_local_position published?");
+    return;
+  }
+
+  Eigen::Vector3f local_position = _map_projection->globalToLocal(global_position);
+  _goto_setpoint->update(
+    local_position, heading, max_horizontal_speed, max_vertical_speed,
+    max_heading_rate);
+}
+
 } // namespace px4_ros2