TrajectorySetpoint positionUpdate() method

Auterion · Sep 12, 2024 · 0456711 · 0456711
1 parent c9d43fd
commit 0456711
Show file tree

Hide file tree

Showing 2 changed files with 28 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,16 @@ class TrajectorySetpointType : public SetpointBase
     std::optional<float> yaw_ned_rad = {},
     std::optional<float> yaw_rate_ned_rad_s = {});
 
+  /**
+   * @brief Position setpoint update.
+   *
+   * The GotoSetpointType should be preferred.
+   *
+   * @param position_ned_m [m] NED earth-fixed frame
+   */
+  void updatePosition(
+    const Eigen::Vector3f & position_ned_m);
+
 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
@@ -39,7 +39,25 @@ void TrajectorySetpointType::update(
   sp.yawspeed = yaw_rate_ned_rad_s.value_or(NAN);
 
   _trajectory_setpoint_pub->publish(sp);
+}
 
+void TrajectorySetpointType::updatePosition(
+  const Eigen::Vector3f & position_ned_m)
+{
+  onUpdate();
+
+  px4_msgs::msg::TrajectorySetpoint sp{};
+  sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+
+  sp.position[0] = position_ned_m.x();
+  sp.position[1] = position_ned_m.y();
+  sp.position[2] = position_ned_m.z();
+  sp.velocity[0] = sp.velocity[1] = sp.velocity[2] = NAN;
+  sp.acceleration[0] = sp.acceleration[1] = sp.acceleration[2] = NAN;
+  sp.yaw = NAN;
+  sp.yawspeed = NAN;
+
+  _trajectory_setpoint_pub->publish(sp);
 }
 
 SetpointBase::Configuration TrajectorySetpointType::getConfiguration()