Publish nav_msgs::Path

ethz-asl · Sep 9, 2021 · 101f03c · 101f03c
1 parent 27c8492
commit 101f03c
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 0 deletions.
diff --git a/polygon_coverage_ros/include/polygon_coverage_ros/polygon_planner_base.h b/polygon_coverage_ros/include/polygon_coverage_ros/polygon_planner_base.h
@@ -116,6 +116,7 @@ class PolygonPlannerBase {
   // Publishers and Services
   ros::Publisher marker_pub_;
   ros::Publisher waypoint_list_pub_;
+  ros::Publisher path_pub_;
   ros::Subscriber clicked_point_sub_;
   ros::Subscriber polygon_sub_;
   ros::ServiceServer set_polygon_srv_;

diff --git a/polygon_coverage_ros/include/polygon_coverage_ros/ros_interface.h b/polygon_coverage_ros/include/polygon_coverage_ros/ros_interface.h
@@ -26,6 +26,7 @@
 #include <eigen_conversions/eigen_msg.h>
 #include <geometry_msgs/Polygon.h>
 #include <geometry_msgs/PoseArray.h>
+#include <nav_msgs/Path.h>
 #include <mav_msgs/conversions.h>
 #include <polygon_coverage_msgs/PolygonWithHolesStamped.h>
 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
@@ -73,11 +74,21 @@ void poseArrayMsgFromEigenTrajectoryPointVector(
     const std::string& frame_id,
     geometry_msgs::PoseArray* trajectory_points_pose_array);
 
+void pathMsgFromEigenTrajectoryPointVector(
+    const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
+    const std::string& frame_id,
+    nav_msgs::Path* trajectory_points_path);
+
 void poseArrayMsgFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     const std::string& frame_id,
     geometry_msgs::PoseArray* trajectory_points_pose_array);
 
+void pathMsgFromPath(
+    const std::vector<Point_2>& waypoints, double altitude,
+    const std::string& frame_id,
+    nav_msgs::Path* trajectory_points_path);
+
 void msgMultiDofJointTrajectoryFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     trajectory_msgs::MultiDOFJointTrajectory* msg);

diff --git a/polygon_coverage_ros/src/polygon_planner_base.cc b/polygon_coverage_ros/src/polygon_planner_base.cc
@@ -26,6 +26,7 @@
 #include <polygon_coverage_planners/cost_functions/path_cost_functions.h>
 
 #include <geometry_msgs/PoseArray.h>
+#include <nav_msgs/Path.h>
 #include <visualization_msgs/MarkerArray.h>
 
 namespace polygon_coverage_planning {
@@ -59,6 +60,8 @@ void PolygonPlannerBase::advertiseTopics() {
       "path_markers", 1, true);
   waypoint_list_pub_ = nh_.advertise<geometry_msgs::PoseArray>(
       "waypoint_list", 1, latch_topics_);
+  path_pub_ = nh_.advertise<nav_msgs::Path>(
+      "path", 1, latch_topics_);
   // Services for generating the plan.
   set_polygon_srv_ = nh_private_.advertiseService(
       "set_polygon", &PolygonPlannerBase::setPolygonCallback, this);
@@ -311,15 +314,19 @@ bool PolygonPlannerBase::publishTrajectoryPoints() {
 
   // Convert path to pose array.
   geometry_msgs::PoseArray trajectory_points_pose_array;
+  nav_msgs::Path trajectory_points_path;
   if (!altitude_.has_value()) {
     ROS_WARN_STREAM("Cannot send trajectory because altitude not set.");
     return false;
   }
   poseArrayMsgFromPath(solution_, altitude_.value(), global_frame_id_,
                        &trajectory_points_pose_array);
+  pathMsgFromPath(solution_, altitude_.value(), global_frame_id_,
+                       &trajectory_points_path);
 
   // Publishing
   waypoint_list_pub_.publish(trajectory_points_pose_array);
+  path_pub_.publish(trajectory_points_path);
 
   // Success
   return true;

diff --git a/polygon_coverage_ros/src/ros_interface.cc b/polygon_coverage_ros/src/ros_interface.cc
@@ -66,6 +66,23 @@ void poseArrayMsgFromEigenTrajectoryPointVector(
   }
 }
 
+void pathMsgFromEigenTrajectoryPointVector(
+    const mav_msgs::EigenTrajectoryPointVector& trajectory_points,
+    const std::string& frame_id,
+    nav_msgs::Path* trajectory_points_path) {
+  ROS_ASSERT(trajectory_points_pose_array);
+
+  // Header
+  trajectory_points_path->header.frame_id = frame_id;
+  // Converting and populating the message with all points
+  for (const mav_msgs::EigenTrajectoryPoint& trajectory_point :
+       trajectory_points) {
+    geometry_msgs::PoseStamped msg;
+    mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(trajectory_point, &msg);
+    trajectory_points_path->poses.push_back(msg);
+  }
+}
+
 void poseArrayMsgFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     const std::string& frame_id,
@@ -78,6 +95,18 @@ void poseArrayMsgFromPath(
                                              trajectory_points_pose_array);
 }
 
+void pathMsgFromPath(
+    const std::vector<Point_2>& waypoints, double altitude,
+    const std::string& frame_id,
+    nav_msgs::Path* trajectory_points_path) {
+  ROS_ASSERT(trajectory_points_path);
+
+  mav_msgs::EigenTrajectoryPointVector eigen_traj;
+  eigenTrajectoryPointVectorFromPath(waypoints, altitude, &eigen_traj);
+  pathMsgFromEigenTrajectoryPointVector(eigen_traj, frame_id,
+                                             trajectory_points_path);
+}
+
 void msgMultiDofJointTrajectoryFromPath(
     const std::vector<Point_2>& waypoints, double altitude,
     trajectory_msgs::MultiDOFJointTrajectory* msg) {