From aeb1adcd78ec4ed07dcdea4dfb5c447a0e9314dc Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Sun, 25 Feb 2024 19:33:47 +0100
Subject: [PATCH] Add path segment messages

---
 planner_msgs/CMakeLists.txt                      |  2 ++
 planner_msgs/msg/Path.msg                        |  6 ++++++
 planner_msgs/msg/PathSegment.msg                 | 10 ++++++++++
 .../terrain_navigation_ros/terrain_planner.h     |  1 +
 terrain_navigation_ros/src/terrain_planner.cpp   | 16 ++++++++++++++++
 5 files changed, 35 insertions(+)
 create mode 100644 planner_msgs/msg/Path.msg
 create mode 100644 planner_msgs/msg/PathSegment.msg

diff --git a/planner_msgs/CMakeLists.txt b/planner_msgs/CMakeLists.txt
index 2be0de94..b81803aa 100644
--- 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
new file mode 100644
index 00000000..f77d73e7
--- /dev/null
+++ 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
new file mode 100644
index 00000000..ae392d42
--- /dev/null
+++ 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
index edf50e13..03f5efbf 100644
--- 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
index 18c37a50..d4dd2c3b 100644
--- 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;
+}