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
index b5b38a4..8f0b50e 100644
--- 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
index 86c3fbb..f2f0a9c 100644
--- 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{};