From fb1b56d623d66c84796a000ff901a9066ebb9f07 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 15 Nov 2023 14:54:02 +0100 Subject: [PATCH] setpoint_types: add goto setpoint type --- px4_ros2_cpp/CMakeLists.txt | 2 + .../message_compatibility_check.hpp | 1 + .../px4_ros2/control/setpoint_types/goto.hpp | 57 ++++++++++++++++ .../src/control/setpoint_types/goto.cpp | 67 +++++++++++++++++++ 4 files changed, 127 insertions(+) create mode 100644 px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp create mode 100644 px4_ros2_cpp/src/control/setpoint_types/goto.cpp diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index 1943410..eec6d29 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/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 diff --git a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp b/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp index 6610eec..d19ffbd 100644 --- a/px4_ros2_cpp/include/px4_ros2/components/message_compatibility_check.hpp +++ b/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"}, \ diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/goto.hpp new file mode 100644 index 0000000..e55a2ae --- /dev/null +++ b/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 +#include +#include + +#include + +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 & heading = {}, + const std::optional & max_horizontal_speed = {}, + const std::optional & max_vertical_speed = {}, + const std::optional & max_heading_rate = {} + ); + + float desiredUpdateRateHz() override {return 30.F;} + +private: + rclcpp::Node & _node; + rclcpp::Publisher::SharedPtr + _goto_setpoint_pub; +}; + +/** @}*/ +} /* namespace px4_ros2 */ diff --git a/px4_ros2_cpp/src/control/setpoint_types/goto.cpp b/px4_ros2_cpp/src/control/setpoint_types/goto.cpp new file mode 100644 index 0000000..d6c2ebb --- /dev/null +++ b/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 + + +namespace px4_ros2 +{ + +GotoSetpointType::GotoSetpointType(Context & context) +: SetpointBase(context), _node(context.node()) +{ + _goto_setpoint_pub = + context.node().create_publisher( + context.topicNamespacePrefix() + "/fmu/in/goto_setpoint", 1); +} + +void GotoSetpointType::update( + const Eigen::Vector3f & position, + const std::optional & heading, + const std::optional & max_horizontal_speed, + const std::optional & max_vertical_speed, + const std::optional & 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