trajectory setpoint: complete update method

examples/cpp/modes/mode_with_executor/include/mode.hpp

@@ -46,8 +46,12 @@ class FlightModeTest : public px4_ros2::ModeBase
     }
 
     const float elapsed_s = (now - _activation_time).seconds();
+    const Eigen::Vector3f position{NAN, NAN, NAN};
     const Eigen::Vector3f velocity{10.f, elapsed_s * 2.f, -2.f};
-    _trajectory_setpoint->update(velocity);
+    const Eigen::Vector3f acceleration{NAN, NAN, NAN};
+    const float yaw = NAN;
+    const float yaw_rate = NAN;
+    _trajectory_setpoint->update(position, velocity, acceleration, yaw, yaw_rate);
   }
 
 private:

examples/cpp/modes/rtl_replacement/include/mode.hpp

@@ -48,9 +48,12 @@ class FlightModeTest : public px4_ros2::ModeBase
       completed(px4_ros2::Result::Success);
       return;
     }
-
+    const Eigen::Vector3f position{NAN, NAN, NAN};
     const Eigen::Vector3f velocity{0.f, 1.f, 5.f};
-    _trajectory_setpoint->update(velocity);
+    const Eigen::Vector3f acceleration{NAN, NAN, NAN};
+    const float yaw = NAN;
+    const float yaw_rate = NAN;
+    _trajectory_setpoint->update(position, velocity, acceleration, yaw, yaw_rate);
   }
 
 private:

px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp

@@ -25,7 +25,11 @@ class TrajectorySetpointType : public SetpointBase
 
   Configuration getConfiguration() override;
 
-  void update(const Eigen::Vector3f & velocity_setpoint_ned_m_s);
+  void update(
+    const Eigen::Vector3f & position_ned_m,
+    const Eigen::Vector3f & velocity_ned_m_s,
+    const Eigen::Vector3f & acceleration_ned_m_s2, float yaw_ned_rad,
+    float yaw_rate_ned_rad_s);
 
 private:
   rclcpp::Node & _node;

px4_ros2_cpp/src/control/setpoint_types/experimental/trajectory.cpp

@@ -16,27 +16,42 @@ TrajectorySetpointType::TrajectorySetpointType(Context & context)
     context.topicNamespacePrefix() + "/fmu/in/trajectory_setpoint", 1);
 }
 
-void TrajectorySetpointType::update(const Eigen::Vector3f & velocity_setpoint_ned_m_s)
+void TrajectorySetpointType::update(
+  const Eigen::Vector3f & position_ned_m,
+  const Eigen::Vector3f & velocity_ned_m_s,
+  const Eigen::Vector3f & acceleration_ned_m_s2, float yaw_ned_rad,
+  float yaw_rate_ned_rad_s)
 {
   onUpdate();
 
   px4_msgs::msg::TrajectorySetpoint sp{};
-  // TODO
-  sp.yaw = NAN;
-  sp.yawspeed = NAN;
-  sp.position[0] = sp.position[1] = sp.position[2] = NAN;
-  sp.acceleration[0] = sp.acceleration[1] = sp.acceleration[2] = NAN;
-  sp.velocity[0] = velocity_setpoint_ned_m_s(0);
-  sp.velocity[1] = velocity_setpoint_ned_m_s(1);
-  sp.velocity[2] = velocity_setpoint_ned_m_s(2);
+  sp.yaw = yaw_ned_rad;
+  sp.yawspeed = yaw_rate_ned_rad_s;
+  sp.position[0] = position_ned_m.x();
+  sp.position[1] = position_ned_m.y();
+  sp.position[2] = position_ned_m.z();
+  sp.velocity[0] = velocity_ned_m_s.x();
+  sp.velocity[1] = velocity_ned_m_s.y();
+  sp.velocity[2] = velocity_ned_m_s.z();
+  sp.acceleration[0] = acceleration_ned_m_s2.x();
+  sp.acceleration[1] = acceleration_ned_m_s2.y();
+  sp.acceleration[2] = acceleration_ned_m_s2.z();
   sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+
   _trajectory_setpoint_pub->publish(sp);
 
 }
 
 SetpointBase::Configuration TrajectorySetpointType::getConfiguration()
 {
   Configuration config{};
+  config.rates_enabled = true;
+  config.attitude_enabled = true;
+  config.acceleration_enabled = true;
+  config.velocity_enabled = true;
+  config.position_enabled = true;
+  config.altitude_enabled = true;
+  config.climb_rate_enabled = true;
   return config;
 }
 } // namespace px4_ros2

px4_ros2_cpp/test/integration/mode_executor.cpp

@@ -62,8 +62,12 @@ class FlightModeTest : public px4_ros2::ModeBase
 
     // Send some random setpoints
     const float elapsed_s = (now - _activation_time).seconds();
+    const Eigen::Vector3f position{NAN, NAN, NAN};
     const Eigen::Vector3f velocity{5.f, elapsed_s, 0.f};
-    _trajectory_setpoint->update(velocity);
+    const Eigen::Vector3f acceleration{NAN, NAN, NAN};
+    const float yaw = NAN;
+    const float yaw_rate = NAN;
+    _trajectory_setpoint->update(position, velocity, acceleration, yaw, yaw_rate);
   }
 
   int num_activations{0};

px4_ros2_cpp/test/integration/overrides.cpp

@@ -61,8 +61,12 @@ class FlightModeTest : public px4_ros2::ModeBase
 
     // Send some random setpoints
     const float elapsed_s = (now - _activation_time).seconds();
+    const Eigen::Vector3f position{NAN, NAN, NAN};
     const Eigen::Vector3f velocity{5.f, elapsed_s, 0.f};
-    _trajectory_setpoint->update(velocity);
+    const Eigen::Vector3f acceleration{NAN, NAN, NAN};
+    const float yaw = NAN;
+    const float yaw_rate = NAN;
+    _trajectory_setpoint->update(position, velocity, acceleration, yaw, yaw_rate);
   }
 
   int num_activations{0};