Replaced sensor_msgs/PointCloud to sensor_msgs/PointCloud2

nav2_bringup/bringup/params/nav2_params.yaml

@@ -150,7 +150,7 @@ controller_server:
       RotateToGoal.scale: 32.0
       RotateToGoal.slowing_factor: 5.0
       RotateToGoal.lookahead_time: -1.0
-
+      
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True

nav2_bringup/bringup/rviz/nav2_default_view.rviz

@@ -293,7 +293,7 @@ Visualization Manager:
             Value: true
           Axis: Z
           Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud
+          Class: rviz_default_plugins/PointCloud2
           Color: 125; 125; 125
           Color Transformer: FlatColor
           Decay Time: 0
@@ -407,7 +407,7 @@ Visualization Manager:
             Value: true
           Axis: Z
           Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud
+          Class: rviz_default_plugins/PointCloud2
           Color: 255; 255; 255
           Color Transformer: RGB8
           Decay Time: 0

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -99,7 +99,8 @@ class VoxelLayer : public ObstacleLayer
   nav2_voxel_grid::VoxelGrid voxel_grid_;
   double z_resolution_, origin_z_;
   int unknown_threshold_, mark_threshold_, size_z_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr clearing_endpoints_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr
+    clearing_endpoints_pub_;
 
   inline bool worldToMap3DFloat(
     double wx, double wy, double wz, double & mx, double & my,

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -96,8 +96,9 @@ void VoxelLayer::onInitialize()
     voxel_pub_->on_activate();
   }
 
-  clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud>(
+  clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
     "clearing_endpoints", custom_qos);
+  clearing_endpoints_pub_->on_activate();
 
   unknown_threshold_ += (VOXEL_BITS - size_z_);
   matchSize();
@@ -302,11 +303,9 @@ void VoxelLayer::raytraceFreespace(
   double * max_x,
   double * max_y)
 {
-  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud>();
+  auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  size_t clearing_observation_cloud_size = clearing_observation.cloud_->height *
-    clearing_observation.cloud_->width;
-  if (clearing_observation_cloud_size == 0) {
+  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
     return;
   }
 
@@ -334,10 +333,23 @@ void VoxelLayer::raytraceFreespace(
   }
 
   if (publish_clearing_points) {
-    clearing_endpoints_->points.clear();
-    clearing_endpoints_->points.reserve(clearing_observation_cloud_size);
+    clearing_endpoints_->data.clear();
+    clearing_endpoints_->width = clearing_observation.cloud_->width;
+    clearing_endpoints_->height = clearing_observation.cloud_->height;
+    clearing_endpoints_->is_dense = true;
+    clearing_endpoints_->is_bigendian = false;
   }
 
+  sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
+  modifier.setPointCloud2Fields(
+    3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(*clearing_endpoints_, "x");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(*clearing_endpoints_, "y");
+  sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(*clearing_endpoints_, "z");
+
   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
   double map_end_x = origin_x_ + getSizeInMetersX();
   double map_end_y = origin_y_ + getSizeInMetersY();
@@ -414,11 +426,13 @@ void VoxelLayer::raytraceFreespace(
         max_y);
 
       if (publish_clearing_points) {
-        geometry_msgs::msg::Point32 point;
-        point.x = wpx;
-        point.y = wpy;
-        point.z = wpz;
-        clearing_endpoints_->points.push_back(point);
+        *clearing_endpoints_iter_x = wpx;
+        *clearing_endpoints_iter_y = wpy;
+        *clearing_endpoints_iter_z = wpz;
+
+        ++clearing_endpoints_iter_x;
+        ++clearing_endpoints_iter_y;
+        ++clearing_endpoints_iter_z;
       }
     }
   }

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -31,8 +31,8 @@
 #include <utility>
 
 #include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/point_cloud.hpp"
-#include "sensor_msgs/msg/channel_float32.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
 #include "nav2_voxel_grid/voxel_grid.hpp"
 #include "nav2_msgs/msg/voxel_grid.hpp"
 #include "nav2_util/execution_timer.hpp"
@@ -70,8 +70,63 @@ V_Cell g_unknown;
 
 rclcpp::Node::SharedPtr g_node;
 
-rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_marked;
-rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_unknown;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
+
+/**
+ * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
+ * @param cloud PointCloud2 Ptr which needs to be filled
+ * @param num_channels Represents the total number of points that are going to be filled
+ * @param header Carries the header information that needs to be assigned to PointCloud2 header
+ * @param g_cells contains the x, y, z values that needs to be added to the PointCloud2
+ */
+void pointCloud2Helper(
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,
+  uint32_t num_channels,
+  std_msgs::msg::Header header,
+  V_Cell & g_cells)
+{
+  cloud->header = header;
+  cloud->width = num_channels;
+  cloud->height = 1;
+  cloud->is_dense = true;
+  cloud->is_bigendian = false;
+  sensor_msgs::PointCloud2Modifier modifier(*cloud);
+
+  modifier.setPointCloud2Fields(
+    6, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "r", 1, sensor_msgs::msg::PointField::UINT8,
+    "g", 1, sensor_msgs::msg::PointField::UINT8,
+    "b", 1, sensor_msgs::msg::PointField::UINT8);
+
+  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud, "z");
+
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud, "r");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud, "g");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud, "b");
+
+  for (uint32_t i = 0; i < num_channels; ++i) {
+    Cell & c = g_cells[i];
+    // assigning value to the point cloud2's iterator
+    *iter_x = c.x;
+    *iter_y = c.y;
+    *iter_z = c.z;
+    *iter_r = g_colors_r[c.status] * 255.0;
+    *iter_g = g_colors_g[c.status] * 255.0;
+    *iter_b = g_colors_b[c.status] * 255.0;
+
+    ++iter_x;
+    ++iter_y;
+    ++iter_z;
+    ++iter_r;
+    ++iter_g;
+    ++iter_b;
+  }
+}
 
 void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
 {
@@ -133,65 +188,19 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
     }
   }
 
-  {
-    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
-    cloud->points.resize(num_marked);
-    cloud->channels.resize(1);
-    cloud->channels[0].values.resize(num_marked);
-    cloud->channels[0].name = "rgb";
-    cloud->header.frame_id = frame_id;
-    cloud->header.stamp = stamp;
-
-    sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
-    for (uint32_t i = 0; i < num_marked; ++i) {
-      geometry_msgs::msg::Point32 & p = cloud->points[i];
-      float & cval = chan.values[i];
-      Cell & c = g_marked[i];
-
-      p.x = c.x;
-      p.y = c.y;
-      p.z = c.z;
-
-      uint32_t r = g_colors_r[c.status] * 255.0;
-      uint32_t g = g_colors_g[c.status] * 255.0;
-      uint32_t b = g_colors_b[c.status] * 255.0;
-      // uint32_t a = g_colors_a[c.status] * 255.0;
-
-      uint32_t col = (r << 16) | (g << 8) | b;
-      memcpy(&cval, &col, sizeof col);
-    }
+  std_msgs::msg::Header pcl_header;
+  pcl_header.frame_id = frame_id;
+  pcl_header.stamp = stamp;
 
+  {
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pointCloud2Helper(cloud, num_marked, pcl_header, g_marked);
     pub_marked->publish(std::move(cloud));
   }
 
   {
-    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
-    cloud->points.resize(num_unknown);
-    cloud->channels.resize(1);
-    cloud->channels[0].values.resize(num_unknown);
-    cloud->channels[0].name = "rgb";
-    cloud->header.frame_id = frame_id;
-    cloud->header.stamp = stamp;
-
-    sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
-    for (uint32_t i = 0; i < num_unknown; ++i) {
-      geometry_msgs::msg::Point32 & p = cloud->points[i];
-      float & cval = chan.values[i];
-      Cell & c = g_unknown[i];
-
-      p.x = c.x;
-      p.y = c.y;
-      p.z = c.z;
-
-      uint32_t r = g_colors_r[c.status] * 255.0;
-      uint32_t g = g_colors_g[c.status] * 255.0;
-      uint32_t b = g_colors_b[c.status] * 255.0;
-      // uint32_t a = g_colors_a[c.status] * 255.0;
-
-      uint32_t col = (r << 16) | (g << 8) | b;
-      memcpy(&cval, &col, sizeof col);
-    }
-
+    auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown);
     pub_unknown->publish(std::move(cloud));
   }
 
@@ -208,9 +217,9 @@ int main(int argc, char ** argv)
 
   RCLCPP_DEBUG(g_node->get_logger(), "Starting up costmap_2d_cloud");
 
-  pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud>(
+  pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
     "voxel_marked_cloud", 1);
-  pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud>(
+  pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
     "voxel_unknown_cloud", 1);
   auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
     "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);

nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp

@@ -124,7 +124,7 @@ class DWBPublisher
   std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_;
   std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_;
   std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
-  std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud>> cost_grid_pc_pub_;
+  std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_;
 
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   rclcpp::Clock::SharedPtr clock_;

nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp

@@ -38,6 +38,7 @@
 #include <string>
 #include <vector>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
@@ -166,7 +167,7 @@ class TrajectoryCritic
    *
    * @param pc PointCloud to add channels to
    */
-  virtual void addCriticVisualization(sensor_msgs::msg::PointCloud &) {}
+  virtual void addCriticVisualization(std::vector<std::pair<std::string, std::vector<float>>> &) {}
 
   std::string getName()
   {