Add path segment messages

ethz-asl · Feb 25, 2024 · aeb1adc · aeb1adc
1 parent 6f96ed1
commit aeb1adc
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 0 deletions.
diff --git a/planner_msgs/CMakeLists.txt b/planner_msgs/CMakeLists.txt
@@ -10,6 +10,8 @@ add_message_files(
   FILES
   NavigationStatus.msg
   TerrainInfo.msg
+  Path.msg
+  PathSegment.msg
 )
 
 add_service_files(

diff --git a/planner_msgs/msg/Path.msg b/planner_msgs/msg/Path.msg
@@ -0,0 +1,6 @@
+# Terrain Planner status
+#
+
+std_msgs/Header header
+
+PathSegment[] segments
diff --git a/planner_msgs/msg/PathSegment.msg b/planner_msgs/msg/PathSegment.msg
@@ -0,0 +1,10 @@
+# Terrain Planner status
+#
+
+std_msgs/Header header
+
+bool reached
+geometry_msgs/Vector3 segment_start
+geometry_msgs/Vector3 segment_tangent
+std_msgs/Float64 curvature
+
diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h
@@ -124,6 +124,7 @@ class TerrainPlanner {
   void publishPathSegments(ros::Publisher &pub, Path &trajectory);
   void publishGoal(const ros::Publisher &pub, const Eigen::Vector3d &position, const double radius,
                    Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "goal");
+  void publishPath(const ros::Publisher &pub, Path &path);
   void publishRallyPoints(const ros::Publisher &pub, const std::vector<Eigen::Vector3d> &positions, const double radius,
                           Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0),
                           std::string name_space = "rallypoints");

diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp
@@ -50,6 +50,7 @@
 #include <mavros_msgs/PositionTarget.h>
 #include <mavros_msgs/Trajectory.h>
 #include <planner_msgs/NavigationStatus.h>
+#include <planner_msgs/Path.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -1160,3 +1161,18 @@ PathSegment TerrainPlanner::generateArcTrajectory(Eigen::Vector3d rate, const do
   }
   return trajectory;
 }
+
+void publishPath(const ros::Publisher &pub, Path &path) {
+  planner_msgs::Path path_msg;
+  for (const auto &path_segment : path.segments) {
+    planner_msgs::PathSegment segment_msg;
+    segment_msg.reached = path_segment.reached;
+    segment_msg.segment_start = toVector3(path_segment.states.front().position);
+    Eigen::Vector3d start_velocity = path_segment.states.front().velocity;
+    segment_msg.segment_tangent = toVector3(start_velocity.normalized());
+    segment_msg.curvature = double(path_segment.curvature);
+    path_msg.segments.push_back(segment_msg);
+  }
+  pub.publish(path_msg);
+  return;
+}