setpoint_types: add goto setpoint type

px4_ros2_cpp/CMakeLists.txt

@@ -34,6 +34,7 @@ set(HEADER_FILES
         include/px4_ros2/components/wait_for_fmu.hpp
         include/px4_ros2/control/peripheral_actuators.hpp
         include/px4_ros2/control/setpoint_types/direct_actuators.hpp
+        include/px4_ros2/control/setpoint_types/goto.hpp
         include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
         include/px4_ros2/control/setpoint_types/experimental/rates.hpp
         include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp
@@ -56,6 +57,7 @@ add_library(px4_ros2_cpp
         src/components/wait_for_fmu.cpp
         src/control/peripheral_actuators.cpp
         src/control/setpoint_types/direct_actuators.cpp
+        src/control/setpoint_types/goto.cpp
         src/control/setpoint_types/experimental/attitude.cpp
         src/control/setpoint_types/experimental/rates.cpp
         src/control/setpoint_types/experimental/trajectory.cpp

px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp

@@ -16,6 +16,7 @@ using namespace std::chrono_literals; // NOLINT
   {"/fmu/in/aux_global_position", "VehicleGlobalPosition"}, \
   {"/fmu/in/config_control_setpoints", "VehicleControlMode"}, \
   {"/fmu/in/config_overrides_request", "ConfigOverrides"}, \
+  {"/fmu/in/goto_setpoint"}, \
   {"/fmu/in/mode_completed"}, \
   {"/fmu/in/register_ext_component_request"}, \
   {"/fmu/in/trajectory_setpoint"}, \

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

@@ -0,0 +1,57 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_msgs/msg/goto_setpoint.hpp>
+#include <Eigen/Eigen>
+#include <optional>
+
+#include <px4_ros2/common/setpoint_base.hpp>
+
+namespace px4_ros2
+{
+/** \ingroup setpoint_types
+ *  @{
+ */
+
+class GotoSetpointType : public SetpointBase
+{
+public:
+  explicit GotoSetpointType(Context & context);
+
+  ~GotoSetpointType() override = default;
+
+  Configuration getConfiguration() override;
+
+  /**
+   * @brief Go-to setpoint update
+   *
+   * Unset optional values are not controlled
+   *
+   * @param position [m] NED earth-fixed frame
+   * @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::Vector3f & 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 = {}
+  );
+
+  float desiredUpdateRateHz() override {return 30.F;}
+
+private:
+  rclcpp::Node & _node;
+  rclcpp::Publisher<px4_msgs::msg::GotoSetpoint>::SharedPtr
+    _goto_setpoint_pub;
+};
+
+/** @}*/
+} /* namespace px4_ros2 */

px4_ros2_cpp/src/control/setpoint_types/goto.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ * Copyright (c) 2023 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+
+#include <px4_ros2/control/setpoint_types/goto.hpp>
+
+
+namespace px4_ros2
+{
+
+GotoSetpointType::GotoSetpointType(Context & context)
+: SetpointBase(context), _node(context.node())
+{
+  _goto_setpoint_pub =
+    context.node().create_publisher<px4_msgs::msg::GotoSetpoint>(
+    context.topicNamespacePrefix() + "/fmu/in/goto_setpoint", 1);
+}
+
+void GotoSetpointType::update(
+  const Eigen::Vector3f & 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)
+{
+  onUpdate();
+
+  px4_msgs::msg::GotoSetpoint sp{};
+
+  // setpoints
+  sp.position[0] = position(0);
+  sp.position[1] = position(1);
+  sp.position[2] = position(2);
+  sp.heading = heading.value_or(0.F);
+
+  // setpoint flags
+  sp.flag_control_heading = heading.has_value();
+
+  // constraints
+  sp.max_horizontal_speed = max_horizontal_speed.value_or(0.F);
+  sp.max_vertical_speed = max_vertical_speed.value_or(0.F);
+  sp.max_heading_rate = max_heading_rate.value_or(0.F);
+
+  // constraint flags
+  sp.flag_set_max_horizontal_speed = max_horizontal_speed.has_value();
+  sp.flag_set_max_vertical_speed = max_vertical_speed.has_value();
+  sp.flag_set_max_heading_rate = max_heading_rate.has_value();
+
+  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+  _goto_setpoint_pub->publish(sp);
+}
+
+SetpointBase::Configuration GotoSetpointType::getConfiguration()
+{
+  Configuration config{};
+  config.control_allocation_enabled = true;
+  config.rates_enabled = true;
+  config.attitude_enabled = true;
+  config.altitude_enabled = true;
+  config.acceleration_enabled = true;
+  config.velocity_enabled = true;
+  config.position_enabled = true;
+  config.climb_rate_enabled = true;
+  return config;
+}
+} // namespace px4_ros2