diff --git a/README.md b/README.md index 842b0fd..a54d8ae 100644 --- a/README.md +++ b/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.

@@ -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`, - **``** (*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 +``` --- diff --git a/pointcloud_crop_box/CMakeLists.txt b/pointcloud_crop_box/CMakeLists.txt index e263f0a..6e1f528 100644 --- a/pointcloud_crop_box/CMakeLists.txt +++ b/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) diff --git a/pointcloud_crop_box/config/pointcloud_crop_box_params.yaml b/pointcloud_crop_box/config/pointcloud_crop_box_params.yaml index 910df93..375756f 100644 --- a/pointcloud_crop_box/config/pointcloud_crop_box_params.yaml +++ b/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 diff --git a/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp b/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp index 897ac32..84d3514 100644 --- a/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp +++ b/pointcloud_crop_box/include/pointcloud_crop_box/pointcloud_crop_box_node.hpp @@ -31,7 +31,8 @@ #include #include #include -#include +#include +#include #include "pointcloud_crop_box/pointcloud_crop_box_params.hpp" @@ -58,7 +59,7 @@ 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 param_listener_; pointcloud_crop_box_params::Params params_; @@ -66,7 +67,7 @@ class PointCloudCropBoxNode : public rclcpp::Node rclcpp::Subscription::SharedPtr pc2_sub_; rclcpp::Subscription::SharedPtr ls_sub_; rclcpp::Publisher::SharedPtr pub_; - rclcpp::Publisher::SharedPtr bbox_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; diff --git a/pointcloud_crop_box/package.xml b/pointcloud_crop_box/package.xml index c829150..df6f2b3 100644 --- a/pointcloud_crop_box/package.xml +++ b/pointcloud_crop_box/package.xml @@ -27,9 +27,7 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - vision_msgs - - vision_msgs_rviz_plugins + visualization_msgs ament_cmake diff --git a/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp b/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp index 39070fb..eaa4340 100644 --- a/pointcloud_crop_box/src/pointcloud_crop_box_node.cpp +++ b/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( - "~/filter_bounding_box", 10); + if (params_.visualize_crop_boxes) { + RCLCPP_INFO(this->get_logger(), "Crop boxes visualization enabled."); + marker_pub_ = this->create_publisher( + "~/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::Ptr PointCloudCropBoxNode::Crop( const pcl::PointCloud::Ptr & cloud) { pcl::CropBox 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::Ptr filtered_cloud(new pcl::PointCloud()); - 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 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> 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 diff --git a/pointcloud_crop_box/src/pointcloud_crop_box_params.yaml b/pointcloud_crop_box/src/pointcloud_crop_box_params.yaml index 8a6d872..ca49fd6 100644 --- a/pointcloud_crop_box/src/pointcloud_crop_box_params.yaml +++ b/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"