Replaced sensor_msgs/PointCloud to sensor_msgs/PointCloud2 (#2263)

* converted PointCloud -> PointCloud2 adn checked linting. WIP

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* modified base_obstacke_test

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* Changed to PointCloudModifier WIP

* formating fix

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* formating fix2

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* added tests for clearing_endpoints

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* removing print statements

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* added subscribers for voxel marked and cost_cloud

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* modified test cases to use single wait for all pointcloud2 subscribers

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* removed unnecessary comments and publish_cost_grid_pc from yaml file

Signed-off-by: Sachin <sachin.guruswamy@gmail.com>

* Fix ament_f...

nav2_bringup/bringup/params/nav2_params.yaml

@@ -152,7 +152,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 @@ private:
   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 @@ protected:
   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 @@ public:
    *
    * @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()
   {

nav2_dwb_controller/dwb_core/src/publisher.cpp

@@ -40,6 +40,7 @@
 #include <vector>
 #include <utility>
 
+#include "sensor_msgs/point_cloud2_iterator.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
@@ -105,7 +106,7 @@ DWBPublisher::on_configure()
   transformed_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
   local_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
   marker_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("marker", 1);
-  cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud>("cost_cloud", 1);
+  cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("cost_cloud", 1);
 
   double marker_lifetime = 0.0;
   node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime);
@@ -256,7 +257,7 @@ DWBPublisher::publishCostGrid(
 
   if (!publish_cost_grid_pc_) {return;}
 
-  auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud>();
+  auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud2>();
   cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID();
   cost_grid_pc->header.stamp = clock_->now();
 
@@ -264,38 +265,75 @@ DWBPublisher::publishCostGrid(
   double x_coord, y_coord;
   unsigned int size_x = costmap->getSizeInCellsX();
   unsigned int size_y = costmap->getSizeInCellsY();
-  cost_grid_pc->points.resize(size_x * size_y);
-  unsigned int i = 0;
-  for (unsigned int cy = 0; cy < size_y; cy++) {
-    for (unsigned int cx = 0; cx < size_x; cx++) {
-      costmap->mapToWorld(cx, cy, x_coord, y_coord);
-      cost_grid_pc->points[i].x = x_coord;
-      cost_grid_pc->points[i].y = y_coord;
-      i++;
-    }
-  }
 
-  sensor_msgs::msg::ChannelFloat32 totals;
-  totals.name = "total_cost";
-  totals.values.resize(size_x * size_y, 0.0);
+  std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
+  std::vector<float> total_cost(size_x * size_y, 0.0);
 
   for (TrajectoryCritic::Ptr critic : critics) {
-    unsigned int channel_index = cost_grid_pc->channels.size();
-    critic->addCriticVisualization(*cost_grid_pc);
-    if (channel_index == cost_grid_pc->channels.size()) {
+    unsigned int channel_index = cost_channels.size();
+    critic->addCriticVisualization(cost_channels);
+    if (channel_index == cost_channels.size()) {
       // No channels were added, so skip to next critic
       continue;
     }
     double scale = critic->getScale();
-    for (i = 0; i < size_x * size_y; i++) {
-      totals.values[i] += cost_grid_pc->channels[channel_index].values[i] * scale;
+    for (unsigned int i = 0; i < size_x * size_y; i++) {
+      total_cost[i] += cost_channels[channel_index].second[i] * scale;
+    }
+  }
+
+  cost_channels.push_back(std::make_pair("total_cost", total_cost));
+
+  cost_grid_pc->width = size_x * size_y;
+  cost_grid_pc->height = 1;
+  cost_grid_pc->fields.resize(3 + cost_channels.size());  // x,y,z, + cost channels
+  cost_grid_pc->is_dense = true;
+  cost_grid_pc->is_bigendian = false;
+
+  int offset = 0;
+  for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i, offset += 4) {
+    cost_grid_pc->fields[i].offset = offset;
+    cost_grid_pc->fields[i].count = 1;
+    cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    if (i >= 3) {
+      cost_grid_pc->fields[i].name = cost_channels[i - 3].first;
+    }
+  }
+
+  cost_grid_pc->fields[0].name = "x";
+  cost_grid_pc->fields[1].name = "y";
+  cost_grid_pc->fields[2].name = "z";
+
+  cost_grid_pc->point_step = offset;
+  cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width;
+  cost_grid_pc->data.resize(cost_grid_pc->row_step * cost_grid_pc->height);
+
+  std::vector<sensor_msgs::PointCloud2Iterator<float>> cost_grid_pc_iter;
+
+  for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i) {
+    sensor_msgs::PointCloud2Iterator<float> iter(*cost_grid_pc, cost_grid_pc->fields[i].name);
+    cost_grid_pc_iter.push_back(iter);
+  }
+
+  unsigned int j = 0;
+  for (unsigned int cy = 0; cy < size_y; cy++) {
+    for (unsigned int cx = 0; cx < size_x; cx++) {
+      costmap->mapToWorld(cx, cy, x_coord, y_coord);
+      *cost_grid_pc_iter[0] = x_coord;
+      *cost_grid_pc_iter[1] = y_coord;
+      *cost_grid_pc_iter[2] = 0.0;   // z value
+
+      for (size_t i = 3; i < cost_grid_pc_iter.size(); ++i) {
+        *cost_grid_pc_iter[i] = cost_channels[i - 3].second[j];
+        ++cost_grid_pc_iter[i];
+      }
+      ++cost_grid_pc_iter[0];
+      ++cost_grid_pc_iter[1];
+      ++cost_grid_pc_iter[2];
+      j++;
     }
   }
-  cost_grid_pc->channels.push_back(totals);
 
-  // TODO(crdelsey): convert pc to pc2
-  // sensor_msgs::msg::PointCloud2 cost_grid_pc2;
-  // convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2);
   cost_grid_pc_pub_->publish(std::move(cost_grid_pc));
 }
 

nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp

@@ -35,6 +35,10 @@
 #ifndef DWB_CRITICS__BASE_OBSTACLE_HPP_
 #define DWB_CRITICS__BASE_OBSTACLE_HPP_
 
+#include <string>
+#include <vector>
+#include <utility>
+
 #include "dwb_core/trajectory_critic.hpp"
 
 namespace dwb_critics
@@ -55,7 +59,8 @@ class BaseObstacleCritic : public dwb_core::TrajectoryCritic
 public:
   void onInit() override;
   double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
-  void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override;
+  void addCriticVisualization(
+    std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
 
   /**
    * @brief Return the obstacle score for a particular pose

nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp

@@ -37,6 +37,9 @@
 
 #include <vector>
 #include <memory>
+#include <string>
+#include <utility>
+
 #include "dwb_core/trajectory_critic.hpp"
 #include "costmap_queue/costmap_queue.hpp"
 
@@ -61,7 +64,8 @@ public:
   // Standard TrajectoryCritic Interface
   void onInit() override;
   double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
-  void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override;
+  void addCriticVisualization(
+    std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
   double getScale() const override {return costmap_->getResolution() * 0.5 * scale_;}
 
   // Helper Functions

nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp

@@ -31,6 +31,9 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
+#include <vector>
+#include <string>
+#include <utility>
 
 #include "dwb_critics/base_obstacle.hpp"
 #include "dwb_core/exceptions.hpp"
@@ -92,22 +95,23 @@ bool BaseObstacleCritic::isValidCost(const unsigned char cost)
          cost != nav2_costmap_2d::NO_INFORMATION;
 }
 
-void BaseObstacleCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc)
+void BaseObstacleCritic::addCriticVisualization(
+  std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
 {
-  sensor_msgs::msg::ChannelFloat32 grid_scores;
-  grid_scores.name = name_;
+  std::pair<std::string, std::vector<float>> grid_scores;
+  grid_scores.first = name_;
 
   unsigned int size_x = costmap_->getSizeInCellsX();
   unsigned int size_y = costmap_->getSizeInCellsY();
-  grid_scores.values.resize(size_x * size_y);
+  grid_scores.second.resize(size_x * size_y);
   unsigned int i = 0;
   for (unsigned int cy = 0; cy < size_y; cy++) {
     for (unsigned int cx = 0; cx < size_x; cx++) {
-      grid_scores.values[i] = costmap_->getCost(cx, cy);
+      grid_scores.second[i] = costmap_->getCost(cx, cy);
       i++;
     }
   }
-  pc.channels.push_back(grid_scores);
+  cost_channels.push_back(grid_scores);
 }
 
 }  // namespace dwb_critics

nav2_dwb_controller/dwb_critics/src/map_grid.cpp

@@ -35,6 +35,8 @@
 #include "dwb_critics/map_grid.hpp"
 #include <cmath>
 #include <string>
+#include <vector>
+#include <utility>
 #include <algorithm>
 #include <memory>
 #include "dwb_core/exceptions.hpp"
@@ -164,23 +166,24 @@ double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
   return getScore(cell_x, cell_y);
 }
 
-void MapGridCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc)
+void MapGridCritic::addCriticVisualization(
+  std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
 {
-  sensor_msgs::msg::ChannelFloat32 grid_scores;
-  grid_scores.name = name_;
+  std::pair<std::string, std::vector<float>> grid_scores;
+  grid_scores.first = name_;
 
   nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
   unsigned int size_x = costmap->getSizeInCellsX();
   unsigned int size_y = costmap->getSizeInCellsY();
-  grid_scores.values.resize(size_x * size_y);
+  grid_scores.second.resize(size_x * size_y);
   unsigned int i = 0;
   for (unsigned int cy = 0; cy < size_y; cy++) {
     for (unsigned int cx = 0; cx < size_x; cx++) {
-      grid_scores.values[i] = getScore(cx, cy);
+      grid_scores.second[i] = getScore(cx, cy);
       i++;
     }
   }
-  pc.channels.push_back(grid_scores);
+  cost_channels.push_back(grid_scores);
 }
 
 }  // namespace dwb_critics

nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp

@@ -35,6 +35,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
@@ -141,8 +142,8 @@ TEST(BaseObstacle, CriticVisualization)
   costmap_ros->getCostmap()->setCost(10, 49, 24);
   costmap_ros->getCostmap()->setCost(45, 2, 12);
 
-  sensor_msgs::msg::PointCloud pointcloud;
-  critic->addCriticVisualization(pointcloud);
+  std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
+  critic->addCriticVisualization(cost_channels);
 
   unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX();
   unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY();
@@ -150,7 +151,7 @@ TEST(BaseObstacle, CriticVisualization)
   // The values in the pointcloud should be equal to the values in the costmap
   for (unsigned int y = 0; y < size_y; y++) {
     for (unsigned int x = 0; x < size_x; x++) {
-      float pointValue = pointcloud.channels[0].values[y * size_y + x];
+      float pointValue = cost_channels[0].second[y * size_y + x];
       ASSERT_EQ(static_cast<int>(pointValue), costmap_ros->getCostmap()->getCost(x, y));
     }
   }

nav2_system_tests/src/costmap_filters/keepout_params.yaml

@@ -146,6 +146,8 @@ controller_server:
       RotateToGoal.scale: 32.0
       RotateToGoal.slowing_factor: 5.0
       RotateToGoal.lookahead_time: -1.0
+      publish_cost_grid_pc: True
+
 
 controller_server_rclcpp_node:
   ros__parameters:
@@ -185,6 +187,10 @@ local_costmap:
           clearing: True
           marking: True
           data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
       static_layer:
         map_subscribe_transient_local: True
       keepout_filter:

nav2_system_tests/src/costmap_filters/test_keepout_launch.py

@@ -114,6 +114,13 @@ def generate_launch_description():
                               'params_file': new_yaml,
                               'bt_xml_file': bt_navigator_xml,
                               'autostart': 'True'}.items()),
+
+        Node(
+            package='nav2_costmap_2d',
+            executable='nav2_costmap_2d_cloud',
+            name='costmap_2d_cloud',
+            output='screen',
+            remappings=[('voxel_grid', 'local_costmap/voxel_grid')]),
     ])
 
 

nav2_system_tests/src/costmap_filters/tester_node.py

-#! /usr/bin/env python3
+#!/usr/bin/env python3
 
 # Copyright (c) 2018 Intel Corporation.
 # Copyright (c) 2020 Samsung Research Russia
@@ -34,11 +34,11 @@ from nav_msgs.msg import OccupancyGrid
 from nav_msgs.msg import Path
 
 import rclpy
-
 from rclpy.action import ActionClient
 from rclpy.node import Node
 from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
 from rclpy.qos import QoSProfile
+from sensor_msgs.msg import PointCloud2
 
 
 class TestType(Enum):
@@ -96,26 +96,46 @@ class NavTester(Node):
         self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
 
         transient_local_qos = QoSProfile(
-          durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
-          reliability=QoSReliabilityPolicy.RELIABLE,
-          history=QoSHistoryPolicy.KEEP_LAST,
-          depth=1)
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=1)
 
         volatile_qos = QoSProfile(
-          durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
-          reliability=QoSReliabilityPolicy.RELIABLE,
-          history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
-          depth=1)
+            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+            depth=1)
 
         self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
                                                        'amcl_pose', self.poseCallback,
                                                        transient_local_qos)
-
+        self.clearing_ep_sub = self.create_subscription(PointCloud2,
+                                                        'local_costmap/clearing_endpoints',
+                                                        self.clearingEndpointsCallback,
+                                                        transient_local_qos)
         self.test_type = test_type
         self.filter_test_result = True
+        self.clearing_endpoints_received = False
+        self.voxel_marked_received = False
+        self.voxel_unknown_received = False
+        self.cost_cloud_received = False
+
         if self.test_type == TestType.KEEPOUT:
             self.plan_sub = self.create_subscription(Path, 'plan',
                                                      self.planCallback, volatile_qos)
+            self.voxel_marked_sub = self.create_subscription(PointCloud2,
+                                                             'voxel_marked_cloud',
+                                                             self.voxelMarkedCallback,
+                                                             1)
+            self.voxel_unknown_sub = self.create_subscription(PointCloud2,
+                                                              'voxel_unknown_cloud',
+                                                              self.voxelUnknownCallback,
+                                                              1)
+            self.cost_cloud_sub = self.create_subscription(PointCloud2,
+                                                           'cost_cloud',
+                                                           self.dwbCostCloudCallback,
+                                                           1)
         elif self.test_type == TestType.SPEED:
             self.speed_it = 0
             # Expected chain of speed limits
@@ -132,7 +152,8 @@ class NavTester(Node):
         self.initial_pose_received = False
         self.initial_pose = initial_pose
         self.goal_pose = goal_pose
-        self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+        self.action_client = ActionClient(
+            self, NavigateToPose, 'navigate_to_pose')
 
     def info_msg(self, msg: str):
         self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
@@ -165,7 +186,8 @@ class NavTester(Node):
         # Sends a `NavToPose` action request and waits for completion
         self.info_msg("Waiting for 'NavigateToPose' action server")
         while not self.action_client.wait_for_server(timeout_sec=1.0):
-            self.info_msg("'NavigateToPose' action server not available, waiting...")
+            self.info_msg(
+                "'NavigateToPose' action server not available, waiting...")
 
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         goal_msg = NavigateToPose.Goal()
@@ -208,7 +230,8 @@ class NavTester(Node):
             self.warn_msg('Filter mask was not received')
         elif self.isInKeepout(x, y):
             self.filter_test_result = False
-            self.error_msg('Pose (' + str(x) + ', ' + str(y) + ') belongs to keepout zone')
+            self.error_msg('Pose (' + str(x) + ', ' +
+                           str(y) + ') belongs to keepout zone')
             return False
         return True
 
@@ -245,6 +268,23 @@ class NavTester(Node):
                 self.error_msg('Path plan intersects with keepout zone')
                 return
 
+    def clearingEndpointsCallback(self, msg):
+        if len(msg.data) > 0:
+            self.clearing_endpoints_received = True
+
+    def voxelMarkedCallback(self, msg):
+        if len(msg.data) > 0:
+            self.voxel_marked_received = True
+
+    def voxelUnknownCallback(self, msg):
+        if len(msg.data) > 0:
+            self.voxel_unknown_received = True
+
+    def dwbCostCloudCallback(self, msg):
+        self.info_msg('Received cost_cloud points')
+        if len(msg.data) > 0:
+            self.cost_cloud_received = True
+
     def speedLimitCallback(self, msg):
         self.info_msg('Received speed limit: ' + str(msg.speed_limit))
         self.checkSpeed(self.speed_it, msg.speed_limit)
@@ -266,6 +306,21 @@ class NavTester(Node):
                 return False
         return True
 
+    def wait_for_pointcloud_subscribers(self, timeout):
+        start_time = time.time()
+        while not self.voxel_unknown_received or not self.voxel_marked_received \
+                or not self.clearing_endpoints_received:
+            self.info_msg(
+                'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+                clearing_endpoints msg to be received ...')
+            rclpy.spin_once(self, timeout_sec=1)
+            if (time.time() - start_time) > timeout:
+                self.error_msg(
+                    'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+                    clearing_endpoints msgs')
+                return False
+        return True
+
     def reachesGoal(self, timeout, distance):
         goalReached = False
         start_time = time.time()
@@ -305,7 +360,8 @@ class NavTester(Node):
                 state = future.result().current_state.label
                 self.info_msg('Result of get_state: %s' % state)
             else:
-                self.error_msg('Exception while calling service: %r' % future.exception())
+                self.error_msg('Exception while calling service: %r' %
+                               future.exception())
             time.sleep(5)
 
     def shutdown(self):
@@ -313,9 +369,11 @@ class NavTester(Node):
         self.action_client.destroy()
 
         transition_service = 'lifecycle_manager_navigation/manage_nodes'
-        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        mgr_client = self.create_client(
+            ManageLifecycleNodes, transition_service)
         while not mgr_client.wait_for_service(timeout_sec=1.0):
-            self.info_msg(transition_service + ' service not available, waiting...')
+            self.info_msg(transition_service +
+                          ' service not available, waiting...')
 
         req = ManageLifecycleNodes.Request()
         req.command = ManageLifecycleNodes.Request().SHUTDOWN
@@ -324,13 +382,16 @@ class NavTester(Node):
             self.info_msg('Shutting down navigation lifecycle manager...')
             rclpy.spin_until_future_complete(self, future)
             future.result()
-            self.info_msg('Shutting down navigation lifecycle manager complete.')
+            self.info_msg(
+                'Shutting down navigation lifecycle manager complete.')
         except Exception as e:  # noqa: B902
             self.error_msg('Service call failed %r' % (e,))
         transition_service = 'lifecycle_manager_localization/manage_nodes'
-        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+        mgr_client = self.create_client(
+            ManageLifecycleNodes, transition_service)
         while not mgr_client.wait_for_service(timeout_sec=1.0):
-            self.info_msg(transition_service + ' service not available, waiting...')
+            self.info_msg(transition_service +
+                          ' service not available, waiting...')
 
         req = ManageLifecycleNodes.Request()
         req.command = ManageLifecycleNodes.Request().SHUTDOWN
@@ -339,7 +400,8 @@ class NavTester(Node):
             self.info_msg('Shutting down localization lifecycle manager...')
             rclpy.spin_until_future_complete(self, future)
             future.result()
-            self.info_msg('Shutting down localization lifecycle manager complete')
+            self.info_msg(
+                'Shutting down localization lifecycle manager complete')
         except Exception as e:  # noqa: B902
             self.error_msg('Service call failed %r' % (e,))
 
@@ -382,16 +444,19 @@ def run_all_tests(robot_tester):
         robot_tester.wait_for_initial_pose()
         robot_tester.wait_for_node_active('bt_navigator')
         result = robot_tester.wait_for_filter_mask(10)
-
     if (result):
         result = robot_tester.runNavigateAction()
 
+    if robot_tester.test_type == TestType.KEEPOUT:
+        result = result and robot_tester.wait_for_pointcloud_subscribers(10)
+
     if (result):
         result = test_RobotMovesToGoal(robot_tester)
 
     if (result):
         if robot_tester.test_type == TestType.KEEPOUT:
             result = robot_tester.filter_test_result
+            result = result and robot_tester.cost_cloud_received
         elif robot_tester.test_type == TestType.SPEED:
             result = test_SpeedLimitsAllCorrect(robot_tester)
 
@@ -438,7 +503,8 @@ def get_tester(args):
 
 def main(argv=sys.argv[1:]):
     # The robot(s) positions from the input arguments
-    parser = argparse.ArgumentParser(description='System-level costmap filters tester node')
+    parser = argparse.ArgumentParser(
+        description='System-level costmap filters tester node')
     parser.add_argument('-t', '--type', type=str, action='store', dest='type',
                         help='Type of costmap filter being tested.')
     group = parser.add_mutually_exclusive_group(required=True)