Merge pull request #78 from OUXT-Polaris/fix/crop_box

Fix/crop box
OUXT-Polaris · Apr 28, 2024 · 7f0fc5f · 7f0fc5f
2 parents 66bce3d + 9ed8246
commit 7f0fc5f
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 1 deletion.
diff --git a/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp b/pcl_apps/include/pcl_apps/filter/crop_box_filter/crop_box_filter_component.hpp
@@ -53,10 +53,15 @@ extern "C" {
 }  // extern "C"
 #endif
 
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <memory>
 #include <pcl_apps/adapter.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 namespace pcl_apps
 {
@@ -73,6 +78,7 @@ class CropBoxFilterComponent : public rclcpp::Node
   bool keep_organized_, negative_;
   PointCloudPublisher pub_;
   PointCloudSubscriber sub_;
+  std::shared_ptr<rclcpp::Publisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
 };
 }  // namespace pcl_apps
 

diff --git a/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp b/pcl_apps/src/filter/crop_box_filter/crop_box_filter_component.cpp
@@ -21,6 +21,8 @@
 #include <pcl/filters/crop_box.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <color_names/color_names.hpp>
+
 namespace pcl_apps
 {
 CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options)
@@ -42,21 +44,51 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio
   get_parameter("keep_organized", keep_organized_);
   declare_parameter("negative", false);
   get_parameter("negative", negative_);
+  marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("~/bbox", 1);
   pub_ = create_publisher<PointCloudAdapterType>("~/points_filtered", 1);
   sub_ = create_subscription<PointCloudAdapterType>(
     "~/points", 1, [this](const PCLPointCloudTypePtr & msg) { pointsCallback(msg); });
 }
 
 void CropBoxFilterComponent::pointsCallback(const PCLPointCloudTypePtr & msg)
 {
+  marker_pub_->publish(visualization_msgs::build<visualization_msgs::msg::MarkerArray>().markers(
+    {visualization_msgs::build<visualization_msgs::msg::Marker>()
+       .header(pcl_conversions::fromPCL(msg->header))
+       .ns("bbox")
+       .id(0)
+       .type(visualization_msgs::msg::Marker::CUBE)
+       .action(visualization_msgs::msg::Marker::ADD)
+       .pose(geometry_msgs::build<geometry_msgs::msg::Pose>()
+               .position(geometry_msgs::build<geometry_msgs::msg::Point>()
+                           .x((min_x_ + max_x_) * 0.5)
+                           .y((min_y_ + max_y_) * 0.5)
+                           .z((min_z_ + max_z_) * 0.5))
+               .orientation(geometry_msgs::msg::Quaternion()))
+       .scale(geometry_msgs::build<geometry_msgs::msg::Vector3>()
+                .x(std::abs(max_x_ - min_x_))
+                .y(std::abs(max_y_ - min_y_))
+                .z(std::abs(max_z_ - min_z_)))
+       .color(color_names::makeColorMsg("greenyellow", 0.3))
+       .lifetime(builtin_interfaces::msg::Duration())
+       .frame_locked(true)
+       .points({})
+       .colors({})
+       .texture_resource("")
+       .texture(sensor_msgs::msg::CompressedImage())
+       .uv_coordinates({})
+       .text("")
+       .mesh_resource("")
+       .mesh_file(visualization_msgs::msg::MeshFile())
+       .mesh_use_embedded_materials(false)}));
   pcl::CropBox<PCLPointType> filter;
   Eigen::Vector4f new_min_point, new_max_point;
   new_min_point << min_x_, min_y_, min_z_, 0.0;
   new_max_point << max_x_, max_y_, max_z_, 0.0;
   filter.setMin(new_min_point);
   filter.setMax(new_max_point);
   filter.setNegative(negative_);
-  filter.setKeepOrganized(keep_organized_);
+  // filter.setKeepOrganized(keep_organized_);
   filter.setInputCloud(msg);
   PCLPointCloudTypePtr filtered_cloud(new PCLPointCloudType());
   filter.filter(*filtered_cloud);