added TracjectorySetpoint update() overload

Auterion · Jul 24, 2024 · b2c0440 · b2c0440
1 parent 4c7b8da
commit b2c0440
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp
@@ -36,6 +36,8 @@ class TrajectorySetpointType : public SetpointBase
     std::optional<float> yaw_ned_rad = {},
     std::optional<float> yaw_rate_ned_rad_s = {});
 
+  void update(const px4_msgs::msg::TrajectorySetpoint& setpoint);
+
 private:
   rclcpp::Node & _node;
   rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr _trajectory_setpoint_pub;

diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp
@@ -42,6 +42,12 @@ void TrajectorySetpointType::update(
 
 }
 
+void TrajectorySetpointType::update(const px4_msgs::msg::TrajectorySetpoint& setpoint)
+{
+    onUpdate();
+    _trajectory_setpoint_pub->publish(setpoint);
+}
+
 SetpointBase::Configuration TrajectorySetpointType::getConfiguration()
 {
   Configuration config{};