Labeling point cloud

CMakeLists.txt

@@ -79,7 +79,7 @@ install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_
 install(FILES rviz/debug.rviz
   DESTINATION share/rviz)
 
-if (BUILD_TESTING) 
+if (BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
   ament_add_gtest(test_set_utils test/test_set_utils.cpp)

include/robot_body_filter/RobotBodyFilter.h

@@ -256,6 +256,7 @@ class RobotBodyFilter : public filters::FilterBase<T> {
   //! Publisher of scan_point_cloud with robot bounding sphere cut out.
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanPointCloudNoBoundingSpherePublisher;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr debugPointCloudInsidePublisher;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr labeledPointCloudPublisher;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr debugPointCloudClipPublisher;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr debugPointCloudShadowPublisher;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debugContainsMarkerPublisher;
@@ -309,6 +310,7 @@ class RobotBodyFilter : public filters::FilterBase<T> {
   bool publishDebugShadowMarker;
   bool publishDebugBsphereMarker;
   bool publishDebugBboxMarker;
+  bool publishLabeledPointCloud;
 
   //! Timeout for reachable transforms.
   rclcpp::Duration reachableTransformTimeout;
@@ -426,6 +428,9 @@ class RobotBodyFilter : public filters::FilterBase<T> {
   void publishDebugPointClouds(
       const sensor_msgs::msg::PointCloud2& projectedPointCloud,
       const std::vector<RayCastingShapeMask::MaskValue>& pointMask) const;
+  void publishLabeledPointClouds(
+      const sensor_msgs::msg::PointCloud2& projectedPointCloud,
+      const std::vector<RayCastingShapeMask::MaskValue>& pointMask) const;
   /**
    * \brief Computation of the bounding sphere, debug spheres, and publishing of
    * pointcloud without bounding sphere.

include/robot_body_filter/utils/cloud.h

@@ -96,6 +96,53 @@ size_t num_points(const Cloud &cloud);
   OUT.row_step = OUT.width * OUT.point_step;\
 }
 
+// LABEL INSTEAD OF REMOVE
+#define CREATE_LABELED_CLOUD(IN_LABEL, OUT_LABEL, KEEP_ORGANIZED_LABEL) {\
+  const auto inputIsOrganized = IN_LABEL.height > 1; \
+  const auto outIsOrganized = KEEP_ORGANIZED_LABEL && inputIsOrganized; \
+  \
+  OUT_LABEL.header = IN_LABEL.header; \
+  OUT_LABEL.fields = IN_LABEL.fields; \
+  auto newField = sensor_msgs::msg::PointField(); \
+  newField.name = "label"; \
+  newField.offset = IN_LABEL.fields.back().offset + IN_LABEL.fields.back().count * sizeOfPointField(IN_LABEL.fields.back()); \
+  newField.datatype = sensor_msgs::msg::PointField::UINT8; \
+  newField.count = 1; \
+  OUT_LABEL.fields.push_back(newField); \
+  OUT_LABEL.point_step = IN_LABEL.point_step + 1; \
+  OUT_LABEL.height = outIsOrganized ? IN_LABEL.height : 1; \
+  OUT_LABEL.width = outIsOrganized ? IN_LABEL.width : 0; \
+  \
+  OUT_LABEL.data.resize(0); \
+  OUT_LABEL.data.reserve(IN_LABEL.data.size()); \
+  \
+  ::robot_body_filter::CloudConstIter x_it(IN_LABEL, "x"); \
+  ::robot_body_filter::CloudConstIter y_it(IN_LABEL, "y"); \
+  ::robot_body_filter::CloudConstIter z_it(IN_LABEL, "z"); \
+  \
+  const auto numPoints = ::robot_body_filter::num_points(IN_LABEL); \
+  \
+  if (!outIsOrganized) { \
+    for (size_t i = 0; i < numPoints; ++i, ++x_it, ++y_it, ++z_it) { \
+        size_t from = (i / IN_LABEL.width) * IN_LABEL.row_step + (i % IN_LABEL.width) * IN_LABEL.point_step; \
+        size_t to = from + IN_LABEL.point_step; \
+        OUT_LABEL.data.insert(OUT_LABEL.data.end(), IN_LABEL.data.begin() + from, \
+                        IN_LABEL.data.begin() + to); \
+        if ((pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) { \
+            OUT_LABEL.data.push_back(0); \
+        } else { \
+            OUT_LABEL.data.push_back(1); \
+        } \
+        OUT_LABEL.width++; \
+    } \
+    OUT_LABEL.is_dense = true; \
+  } else { \
+    RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "organized cloud labeling not supported"); \
+  }\
+  \
+  OUT_LABEL.row_step = OUT_LABEL.width * OUT_LABEL.point_step;\
+}
+
 /**
  * Return true if the cloud contains a field with the given name.
  * @param cloud The cloud to search.

src/RobotBodyFilter.cpp

@@ -81,6 +81,7 @@ void RobotBodyFilter<T>::DeclareParameters(){
   this->nodeHandle->declare_parameter("transforms/timeout/unreachable", 0.2, param_desc);
   this->nodeHandle->declare_parameter("transforms/require_all_reachable", false);
   this->nodeHandle->declare_parameter("bounding_sphere/publish_cut_out_pointcloud", false);
+  this->nodeHandle->declare_parameter("labeling/publish_labeled_point_cloud", true);
   this->nodeHandle->declare_parameter("bounding_box/publish_cut_out_pointcloud", false);
   this->nodeHandle->declare_parameter("oriented_bounding_box/publish_cut_out_pointcloud", false);
   this->nodeHandle->declare_parameter("local_bounding_box/publish_cut_out_pointcloud", false);
@@ -233,6 +234,7 @@ bool RobotBodyFilter<T>::configure() {
   this->nodeHandle->get_parameter_or("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale, inflationScale);
   this->nodeHandle->get_parameter_or("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding, inflationPadding);
   this->nodeHandle->get_parameter_or("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale, inflationScale);
+  this->nodeHandle->get_parameter("labeling/publish_labeled_point_cloud", this->publishLabeledPointCloud);
 
   // read per-link padding
   std::map<std::string, double> perLinkInflationPadding;
@@ -484,6 +486,12 @@ bool RobotBodyFilter<T>::configure() {
     100);
   }
 
+  if (this->publishLabeledPointCloud) {
+    this->labeledPointCloudPublisher = this->nodeHandle->template
+    create_publisher<sensor_msgs::msg::PointCloud2>("labeled_point_cloud",
+    100);
+  }
+
   // initialize the 3D body masking tool
   auto getShapeTransformCallback =
   std::bind(&RobotBodyFilter::getShapeTransform, this,
@@ -725,6 +733,7 @@ bool RobotBodyFilter<T>::computeMask(
                double(clock() - stopwatchOverall) / CLOCKS_PER_SEC);
 
   this->publishDebugPointClouds(projectedPointCloud, pointMask);
+  this->publishLabeledPointClouds(projectedPointCloud, pointMask);
   this->publishDebugMarkers(scanTime);
   this->computeAndPublishBoundingSphere(projectedPointCloud);
   this->computeAndPublishBoundingBox(projectedPointCloud);
@@ -1445,6 +1454,16 @@ void RobotBodyFilter<T>::publishDebugPointClouds(const sensor_msgs::msg::PointCl
   }
 }
 
+template <typename T>
+void RobotBodyFilter<T>::publishLabeledPointClouds(const sensor_msgs::msg::PointCloud2& projectedPointCloud,
+                                                 const std::vector<RayCastingShapeMask::MaskValue>& pointMask) const {
+  if (this->publishLabeledPointCloud) {
+    sensor_msgs::msg::PointCloud2 insideCloud;
+    CREATE_LABELED_CLOUD(projectedPointCloud, insideCloud, this->keepCloudsOrganized);
+    this->labeledPointCloudPublisher->publish(insideCloud);
+  }
+}
+
 template <typename T>
 void RobotBodyFilter<T>::computeAndPublishBoundingSphere(
     const sensor_msgs::msg::PointCloud2& projectedPointCloud) const {