multi-cropbox

README.md

@@ -1,7 +1,7 @@
 # pointcloud_crop_box
 
-A ROS 2 package for filtering 3D point clouds using an axis-aligned cropping box.
-It subscribes to a `sensor_msgs/msg/PointCloud2` or `sensor_msgs/msg/LaserScan`, transforms it into a target frame, filters it using a configurable box, and publishes both the filtered cloud and a 3D bounding box message.
+A ROS 2 package for filtering 3D point clouds using multiple axis-aligned cropping boxes.
+It subscribes to a `sensor_msgs/msg/PointCloud2` or `sensor_msgs/msg/LaserScan`, transforms it into a target frame, filters it using configurable crop boxes, and publishes both the filtered cloud and visualization markers for the crop boxes.
 
 <p float="center">
   <img src=".docs/forward_ground.png" width="30%" />
@@ -14,11 +14,11 @@ It subscribes to a `sensor_msgs/msg/PointCloud2` or `sensor_msgs/msg/LaserScan`,
 
 ## Features
 
-- Subscribes to raw point cloud data (e.g., from a LiDAR)
-- Filters the cloud using a configurable 3D bounding box
+- Subscribes to raw point cloud data (e.g., from a LiDAR) or laser scan data
+- Filters the cloud using multiple configurable 3D crop boxes
 - Supports TF2 transformation to a target frame
-- Option to invert the box (i.e., remove inside instead of outside)
-- Publishes the filtered cloud and a 3D bounding box marker
+- Option to invert the box filtering (i.e., remove inside instead of outside)
+- Publishes the filtered cloud and visualization markers for the crop boxes
 
 ---
 
@@ -36,8 +36,8 @@ It subscribes to a `sensor_msgs/msg/PointCloud2` or `sensor_msgs/msg/LaserScan`,
 - **`<output_topic>`** (*sensor_msgs/msg/PointCloud2*)
   Filtered point cloud after box cropping (default: `/points_filtered`)
 
-- **`~/filter_bounding_box`** (*vision_msgs/msg/BoundingBox3D*)
-  The axis-aligned box used for filtering, for visualization or debugging
+- **`~/filter_crop_boxes`** (*visualization_msgs/msg/MarkerArray*)
+  Visualization markers for all configured crop boxes, for visualization and debugging
 
 ---
 
@@ -46,15 +46,58 @@ It subscribes to a `sensor_msgs/msg/PointCloud2` or `sensor_msgs/msg/LaserScan`,
 - `input_topic` [*string*, default: **"/points_raw"**]: Topic to subscribe for input PointCloud2 data.
 - `output_topic` [*string*, default: **"/points_filtered"**]: Topic to publish filtered PointCloud2 data.
 - `target_frame` [*string*, default: **"base_link"**]: Target TF frame to transform the point cloud into.
-- `min_x` [*double*, default: **-1.0**]: Minimum X boundary of the crop box.
-- `max_x` [*double*, default: **1.0**]: Maximum X boundary of the crop box.
-- `min_y` [*double*, default: **-1.0**]: Minimum Y boundary of the crop box.
-- `max_y` [*double*, default: **1.0**]: Maximum Y boundary of the crop box.
-- `min_z` [*double*, default: **-1.0**]: Minimum Z boundary of the crop box.
-- `max_z` [*double*, default: **1.0**]: Maximum Z boundary of the crop box.
-- `message_type` [*string*, default: **pointcloud**]: type of source data to crop, 'pointcloud' or 'lidarscan'.
-- `negative` [*bool*, default: **false**]: If true, keeps points **outside** the crop box instead of inside.
-- `visualize_bounding_box` [*bool*, default: **true**]: Whether to publish a visualization marker for the bounding box.
+- `crop_boxes_names` [*string_array*, default: **["robot_box"]**]: List of crop box names to apply sequentially.
+- `crop_boxes` [*map*]: Configuration for multiple crop boxes, with each crop box having the following parameters:
+  - `min_x` [*double*, default: **-1.0**]: Minimum X boundary of the crop box.
+  - `max_x` [*double*, default: **1.0**]: Maximum X boundary of the crop box.
+  - `min_y` [*double*, default: **-1.0**]: Minimum Y boundary of the crop box.
+  - `max_y` [*double*, default: **1.0**]: Maximum Y boundary of the crop box.
+  - `min_z` [*double*, default: **-1.0**]: Minimum Z boundary of the crop box.
+  - `max_z` [*double*, default: **1.0**]: Maximum Z boundary of the crop box.
+- `message_type` [*string*, default: **"pointcloud"**]: type of source data to crop, 'pointcloud' or 'laserscan'.
+- `negative` [*bool*, default: **false**]: If true, keeps points **outside** the crop boxes instead of inside.
+- `visualize_crop_boxes` [*bool*, default: **true**]: Whether to publish visualization markers for the crop boxes.
+
+---
+
+## Configuration
+
+### Multiple Crop Boxes
+
+This package supports multiple crop boxes that are applied sequentially to filter the point cloud. You can configure multiple crop boxes in the parameter file:
+
+```yaml
+/**:
+  pointcloud_crop_box:
+    ros__parameters:
+      target_frame: base_link
+      crop_boxes_names: ["robot_box", "robot_attachment_1", "robot_attachment_2"]
+      crop_boxes:
+        robot_box:
+          min_x: -0.5
+          max_x: 0.5
+          min_y: -0.5
+          max_y: 0.5
+          min_z: -0.1
+          max_z: 0.3
+        robot_attachment_1:
+          min_x: -2.0
+          max_x: 2.0
+          min_y: -1.0
+          max_y: 1.0
+          min_z: 0.5
+          max_z: 2.0
+        robot_attachment_2:
+          min_x: -5.0
+          max_x: 5.0
+          min_y: -5.0
+          max_y: 5.0
+          min_z: 2.5
+          max_z: 5.0
+      negative: false
+      message_type: pointcloud
+      visualize_crop_boxes: true
+```
 
 ---
 

pointcloud_crop_box/CMakeLists.txt

@@ -18,7 +18,7 @@ set(PACKAGE_DEPENDENCIES
     tf2_geometry_msgs
     tf2_ros
     tf2_sensor_msgs
-    vision_msgs)
+    visualization_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)

pointcloud_crop_box/config/pointcloud_crop_box_params.yaml

@@ -2,12 +2,15 @@
   pointcloud_crop_box:
     ros__parameters:
       target_frame: panther/base_footprint
-      min_x: -0.405
-      max_x: 0.405
-      min_y: -0.424
-      max_y: 0.424
-      min_z: -0.05
-      max_z: 0.5
+      crop_boxes_names: ["robot_box"]
+      crop_boxes:
+        robot_box:
+          min_x: -0.405
+          max_x: 0.405
+          min_y: -0.424
+          max_y: 0.424
+          min_z: -0.05
+          max_z: 0.5
       negative: true
       message_type: laserscan #or pointcloud
       input_topic: /panther/rplidar/scan

pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp

@@ -31,7 +31,8 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
-#include <vision_msgs/msg/bounding_box3_d.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include "pointcloud_crop_box/pointcloud_crop_box_params.hpp"
 
@@ -58,15 +59,15 @@ class PointCloudCropBoxNode : public rclcpp::Node
   geometry_msgs::msg::TransformStamped InverseTransform(
     const geometry_msgs::msg::TransformStamped & transform_stamped);
 
-  vision_msgs::msg::BoundingBox3D CreateBoundingBox();
+  visualization_msgs::msg::MarkerArray CreateVisualizationMarkers();
 
   std::shared_ptr<pointcloud_crop_box_params::ParamListener> param_listener_;
   pointcloud_crop_box_params::Params params_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pc2_sub_;
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr ls_sub_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
-  rclcpp::Publisher<vision_msgs::msg::BoundingBox3D>::SharedPtr bbox_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;

pointcloud_crop_box/package.xml

@@ -27,9 +27,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_sensor_msgs</depend>
-  <depend>vision_msgs</depend>
-
-  <exec_depend>vision_msgs_rviz_plugins</exec_depend>
+  <depend>visualization_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

pointcloud_crop_box/src/pointcloud_crop_box_node.cpp

@@ -48,12 +48,12 @@ PointCloudCropBoxNode::PointCloudCropBoxNode(const rclcpp::NodeOptions & options
       std::bind(&PointCloudCropBoxNode::LaserScanCallback, this, std::placeholders::_1));
   }
 
-  if (params_.visualize_bounding_box) {
-    RCLCPP_INFO(this->get_logger(), "Bounding box visualization enabled.");
-    bbox_pub_ = this->create_publisher<vision_msgs::msg::BoundingBox3D>(
-      "~/filter_bounding_box", 10);
+  if (params_.visualize_crop_boxes) {
+    RCLCPP_INFO(this->get_logger(), "Crop boxes visualization enabled.");
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "~/filter_crop_boxes", 10);
   } else {
-    RCLCPP_INFO(this->get_logger(), "Bounding box visualization disabled.");
+    RCLCPP_INFO(this->get_logger(), "Crop boxes visualization disabled.");
   }
 
   RCLCPP_INFO(this->get_logger(), "Initialized successfully.");
@@ -95,9 +95,9 @@ void PointCloudCropBoxNode::PointCloudCallback(const sensor_msgs::msg::PointClou
 
   pub_->publish(output_msg);
 
-  if (params_.visualize_bounding_box) {
-    vision_msgs::msg::BoundingBox3D bbox_msg = CreateBoundingBox();
-    bbox_pub_->publish(bbox_msg);
+  if (params_.visualize_crop_boxes) {
+    auto marker_array = CreateVisualizationMarkers();
+    marker_pub_->publish(marker_array);
   }
 }
 
@@ -146,13 +146,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudCropBoxNode::Crop(
   const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
 {
   pcl::CropBox<pcl::PointXYZ> crop_box_filter;
-  crop_box_filter.setMin(Eigen::Vector4f(params_.min_x, params_.min_y, params_.min_z, 1.0));
-  crop_box_filter.setMax(Eigen::Vector4f(params_.max_x, params_.max_y, params_.max_z, 1.0));
-  crop_box_filter.setInputCloud(cloud);
-
   pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
-  crop_box_filter.setNegative(params_.negative);
-  crop_box_filter.filter(*filtered_cloud);
+  crop_box_filter.setInputCloud(cloud);
+  for (auto const & [key, val] : params_.crop_boxes.crop_boxes_names_map) {
+    crop_box_filter.setMin(Eigen::Vector4f(val.min_x, val.min_y, val.min_z, 1.0));
+    crop_box_filter.setMax(Eigen::Vector4f(val.max_x, val.max_y, val.max_z, 1.0));
+    crop_box_filter.setNegative(params_.negative);
+    crop_box_filter.filter(*filtered_cloud);
+    crop_box_filter.setInputCloud(filtered_cloud);
+  }
   return filtered_cloud;
 }
 
@@ -185,19 +187,59 @@ geometry_msgs::msg::TransformStamped PointCloudCropBoxNode::InverseTransform(
   return inverse_transform_stamped;
 }
 
-vision_msgs::msg::BoundingBox3D PointCloudCropBoxNode::CreateBoundingBox()
+visualization_msgs::msg::MarkerArray PointCloudCropBoxNode::CreateVisualizationMarkers()
 {
-  vision_msgs::msg::BoundingBox3D bbox;
-
-  bbox.center.position.x = (params_.min_x + params_.max_x) / 2.0;
-  bbox.center.position.y = (params_.min_y + params_.max_y) / 2.0;
-  bbox.center.position.z = (params_.min_z + params_.max_z) / 2.0;
-
-  bbox.size.x = params_.max_x - params_.min_x;
-  bbox.size.y = params_.max_y - params_.min_y;
-  bbox.size.z = params_.max_z - params_.min_z;
+  visualization_msgs::msg::MarkerArray marker_array;
+  int marker_id = 0;
+  for (const auto & [key, val] : params_.crop_boxes.crop_boxes_names_map) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = params_.target_frame;
+    marker.header.stamp = this->now();
+    marker.ns = key;
+    marker.id = marker_id++;
+    marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.scale.x = 0.01;  // Line width
+    marker.color.a = 1.0;   // Fully opaque
+    marker.color.r = 1.0;   // Red color
+
+    // Define the 8 points of the bounding box
+    std::array<Eigen::Vector3f, 8> points = {
+      Eigen::Vector3f(val.min_x, val.min_y, val.min_z),
+      Eigen::Vector3f(val.max_x, val.min_y, val.min_z),
+      Eigen::Vector3f(val.max_x, val.max_y, val.min_z),
+      Eigen::Vector3f(val.min_x, val.max_y, val.min_z),
+      Eigen::Vector3f(val.min_x, val.min_y, val.max_z),
+      Eigen::Vector3f(val.max_x, val.min_y, val.max_z),
+      Eigen::Vector3f(val.max_x, val.max_y, val.max_z),
+      Eigen::Vector3f(val.min_x, val.max_y, val.max_z),
+    };
+
+    // Define the lines that make up the bounding box
+    std::vector<std::pair<int, int>> line_indices = {
+      {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6},
+      {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7},
+    };
+
+    for (const auto & [start, end] : line_indices) {
+      geometry_msgs::msg::Point p_start;
+      p_start.x = points[start].x();
+      p_start.y = points[start].y();
+      p_start.z = points[start].z();
+
+      geometry_msgs::msg::Point p_end;
+      p_end.x = points[end].x();
+      p_end.y = points[end].y();
+      p_end.z = points[end].z();
+
+      marker.points.push_back(p_start);
+      marker.points.push_back(p_end);
+    }
+
+    marker_array.markers.push_back(marker);
+  }
 
-  return bbox;
+  return marker_array;
 }
 
 }  // namespace pointcloud_crop_box

pointcloud_crop_box/src/pointcloud_crop_box_params.yaml

@@ -11,24 +11,30 @@ pointcloud_crop_box_params:
     type: string
     default_value: "base_link"
     description: "Target TF frame to transform pointcloud into"
-  min_x:
-    type: double
-    default_value: -1.0
-  max_x:
-    type: double
-    default_value: 1.0
-  min_y:
-    type: double
-    default_value: -1.0
-  max_y:
-    type: double
-    default_value: 1.0
-  min_z:
-    type: double
-    default_value: -1.0
-  max_z:
-    type: double
-    default_value: 1.0
+  crop_boxes_names:
+    type: string_array
+    default_value: ["robot_box"]
+    description: "List of crop boxes to apply"
+  crop_boxes:
+    __map_crop_boxes_names:
+      min_x:
+        type: double
+        default_value: -1.0
+      max_x:
+        type: double
+        default_value: 1.0
+      min_y:
+        type: double
+        default_value: -1.0
+      max_y:
+        type: double
+        default_value: 1.0
+      min_z:
+        type: double
+        default_value: -1.0
+      max_z:
+        type: double
+        default_value: 1.0
   message_type:
     type: string
     default_value: "pointcloud"
@@ -39,7 +45,7 @@ pointcloud_crop_box_params:
     type: bool
     default_value: false
     description: "Whether to keep points outside the box"
-  visualize_bounding_box:
+  visualize_crop_boxes:
     type: bool
     default_value: true
-    description: "Whether to publish a visualization marker for the bounding box"
+    description: "Whether to publish a visualization marker for the crop boxes"