distanceGround() and direct TrajectorySetpoint

Auterion · Jul 23, 2024 · 2d460ad · 2d460ad
1 parent bfebd82
commit 2d460ad
Show file tree

Hide file tree

Showing 3 changed files with 14 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/include/px4_ros2/odometry/local_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp
@@ -71,6 +71,12 @@ class OdometryLocalPosition : public Subscription<px4_msgs::msg::VehicleLocalPos
     const px4_msgs::msg::VehicleLocalPosition & pos = last();
     return pos.heading;
   }
+
+  float distanceGround() const
+  {
+    const px4_msgs::msg::VehicleLocalPosition& pos = last();
+    return pos.dist_bottom;
+  }
 };
 
 /** @}*/

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{};