From afb87f2f34779fa19f7eb40522ebe36c62f21f26 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Fri, 19 Jan 2024 18:23:27 +0100 Subject: [PATCH] `planner` ROS - PCL with explored trajectories extended with the trajectory ID field --- src/humap_planner_ros.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/humap_planner_ros.cpp b/src/humap_planner_ros.cpp index a2a29d15..4de669f1 100644 --- a/src/humap_planner_ros.cpp +++ b/src/humap_planner_ros.cpp @@ -743,12 +743,13 @@ sensor_msgs::PointCloud2 HumapPlannerROS::createExploredTrajectoriesPcl() const sensor_msgs::PointCloud2Modifier cloud_mod(traj_cloud); cloud_mod.setPointCloud2Fields( - 5, + 6, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, "theta", 1, sensor_msgs::PointField::FLOAT32, - "cost", 1, sensor_msgs::PointField::FLOAT32 + "cost", 1, sensor_msgs::PointField::FLOAT32, + "id", 1, sensor_msgs::PointField::FLOAT32 ); // check how many points there will be in a point cloud @@ -786,6 +787,8 @@ sensor_msgs::PointCloud2 HumapPlannerROS::createExploredTrajectoriesPcl() const iter_x[2] = 0.0; iter_x[3] = p_th; iter_x[4] = t->cost_; + // ID of the trajectory + iter_x[5] = static_cast(std::distance(explored_trajs.cbegin(), t)); ++iter_x; } }