Add path segment messages (#53)

planner_msgs/CMakeLists.txt

@@ -11,6 +11,8 @@ add_message_files(
   NavigationStatus.msg
   TerrainInfo.msg
   MctsInfo.msg
+  Path.msg
+  PathSegment.msg
 )
 
 add_service_files(

planner_msgs/msg/Path.msg

@@ -0,0 +1,6 @@
+# Path message
+#
+
+std_msgs/Header header
+
+PathSegment[] segments

planner_msgs/msg/PathSegment.msg

@@ -0,0 +1,12 @@
+# Path Segment message
+#
+
+std_msgs/Header header
+
+bool reached
+bool periodic
+std_msgs/Float64 segment_length
+geometry_msgs/Vector3 segment_start
+geometry_msgs/Vector3 segment_tangent
+std_msgs/Float64 curvature
+

terrain_navigation/include/terrain_navigation/path_segment.h

@@ -164,10 +164,10 @@ class PathSegment {
     return theta;
   }
 
-  double getLength(double epsilon = 0.001) {
+  double getLength(double epsilon = 0.001) const {
     double length{0.0};
-    Eigen::Vector3d segment_start = states.front().position;
-    Eigen::Vector3d segment_end = states.back().position;
+    const Eigen::Vector3d segment_start = states.front().position;
+    const Eigen::Vector3d segment_end = states.back().position;
     if (states.size() == 1) {
       return 0.0;
     } else if (std::abs(curvature) < 0.0001) {

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");
@@ -175,6 +176,7 @@ class TerrainPlanner {
   ros::Publisher tree_pub_;
   ros::Publisher vehicle_velocity_pub_;
   ros::Publisher path_segment_pub_;
+  ros::Publisher reference_path_pub_;
   ros::Subscriber mavlocalpose_sub_;
   ros::Subscriber mavglobalpose_sub_;
   ros::Subscriber mavtwist_sub_;

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>
@@ -81,6 +82,7 @@ TerrainPlanner::TerrainPlanner(const ros::NodeHandle &nh, const ros::NodeHandle
   planner_status_pub_ = nh_.advertise<planner_msgs::NavigationStatus>("planner_status", 1);
   path_segment_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("path_segments", 1);
   tree_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("tree", 1);
+  reference_path_pub_ = nh_.advertise<planner_msgs::Path>("reference_path", 1);
 
   mavlocalpose_sub_ = nh_.subscribe("mavros/local_position/pose", 1, &TerrainPlanner::mavLocalPoseCallback, this,
                                     ros::TransportHints().tcpNoDelay());
@@ -461,6 +463,7 @@ void TerrainPlanner::plannerloopCallback(const ros::TimerEvent &event) {
 
   // double planner_time = planner_profiler_->toc();
   publishTrajectory(reference_primitive_.position());
+  publishPath(reference_path_pub_, reference_primitive_);
   // publishGoal(goal_pub_, goal_pos_, 66.67, Eigen::Vector3d(0.0, 1.0, 0.0));
 }
 
@@ -1160,3 +1163,21 @@ PathSegment TerrainPlanner::generateArcTrajectory(Eigen::Vector3d rate, const do
   }
   return trajectory;
 }
+
+void TerrainPlanner::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.periodic = path_segment.is_periodic;
+    segment_msg.curvature.data = double(path_segment.curvature);
+    double segment_length = path_segment.getLength();
+    segment_msg.segment_length.data = double(segment_length);
+    path_msg.segments.push_back(segment_msg);
+  }
+  pub.publish(path_msg);
+  return;
+}