14
14
15
15
#include " pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp"
16
16
17
+ #include " autoware_auto_geometry/common_3d.hpp"
18
+
17
19
#include < sensor_msgs/point_cloud2_iterator.hpp>
18
20
21
+ #include < pcl/search/pcl_search.h>
22
+
19
23
#include < algorithm>
20
24
#include < vector>
21
25
namespace pointcloud_preprocessor
@@ -29,6 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
29
33
using tier4_autoware_utils::StopWatch;
30
34
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
31
35
debug_publisher_ = std::make_unique<DebugPublisher>(this , " ring_outlier_filter" );
36
+ excluded_points_publisher_ =
37
+ this ->create_publisher <sensor_msgs::msg::PointCloud2>(" debug/ring_outlier_filter" , 1 );
32
38
stop_watch_ptr_->tic (" cyclic_time" );
33
39
stop_watch_ptr_->tic (" processing_time" );
34
40
}
@@ -42,6 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
42
48
max_rings_num_ = static_cast <uint16_t >(declare_parameter (" max_rings_num" , 128 ));
43
49
max_points_num_per_ring_ =
44
50
static_cast <size_t >(declare_parameter (" max_points_num_per_ring" , 4000 ));
51
+ publish_excluded_points_ =
52
+ static_cast <bool >(declare_parameter (" publish_excluded_points" , false ));
45
53
}
46
54
47
55
using std::placeholders::_1;
@@ -196,6 +204,17 @@ void RingOutlierFilterComponent::faster_filter(
196
204
sensor_msgs::msg::PointField::FLOAT32, " z" , 1 , sensor_msgs::msg::PointField::FLOAT32,
197
205
" intensity" , 1 , sensor_msgs::msg::PointField::FLOAT32);
198
206
207
+ if (publish_excluded_points_) {
208
+ auto excluded_points = extractExcludedPoints (*input, output, 0.01 );
209
+ // set fields
210
+ sensor_msgs::PointCloud2Modifier excluded_pcd_modifier (excluded_points);
211
+ excluded_pcd_modifier.setPointCloud2Fields (
212
+ num_fields, " x" , 1 , sensor_msgs::msg::PointField::FLOAT32, " y" , 1 ,
213
+ sensor_msgs::msg::PointField::FLOAT32, " z" , 1 , sensor_msgs::msg::PointField::FLOAT32,
214
+ " intensity" , 1 , sensor_msgs::msg::PointField::FLOAT32);
215
+ excluded_points_publisher_->publish (excluded_points);
216
+ }
217
+
199
218
// add processing time for debug
200
219
if (debug_publisher_) {
201
220
const double cyclic_time_ms = stop_watch_ptr_->toc (" cyclic_time" , true );
@@ -241,13 +260,65 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
241
260
if (get_param (p, " num_points_threshold" , num_points_threshold_)) {
242
261
RCLCPP_DEBUG (get_logger (), " Setting new num_points_threshold to: %d." , num_points_threshold_);
243
262
}
263
+ if (get_param (p, " publish_excluded_points" , publish_excluded_points_)) {
264
+ RCLCPP_DEBUG (
265
+ get_logger (), " Setting new publish_excluded_points to: %d." , publish_excluded_points_);
266
+ }
244
267
245
268
rcl_interfaces::msg::SetParametersResult result;
246
269
result.successful = true ;
247
270
result.reason = " success" ;
248
271
249
272
return result;
250
273
}
274
+
275
+ sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints (
276
+ const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output,
277
+ float epsilon)
278
+ {
279
+ // Convert ROS PointCloud2 message to PCL point cloud for easier manipulation
280
+ pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
281
+ pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
282
+ pcl::fromROSMsg (input, *input_cloud);
283
+ pcl::fromROSMsg (output, *output_cloud);
284
+
285
+ pcl::PointCloud<pcl::PointXYZ>::Ptr excluded_points (new pcl::PointCloud<pcl::PointXYZ>);
286
+
287
+ pcl::search::Search<pcl::PointXYZ>::Ptr tree;
288
+ if (output_cloud->isOrganized ()) {
289
+ tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ>());
290
+ } else {
291
+ tree.reset (new pcl::search::KdTree<pcl::PointXYZ>(false ));
292
+ }
293
+ tree->setInputCloud (output_cloud);
294
+ std::vector<int > nn_indices (1 );
295
+ std::vector<float > nn_distances (1 );
296
+ for (const auto & point : input_cloud->points ) {
297
+ if (!tree->nearestKSearch (point, 1 , nn_indices, nn_distances)) {
298
+ continue ;
299
+ }
300
+ if (nn_distances[0 ] > epsilon) {
301
+ excluded_points->points .push_back (point);
302
+ }
303
+ }
304
+
305
+ sensor_msgs::msg::PointCloud2 excluded_points_msg;
306
+ pcl::toROSMsg (*excluded_points, excluded_points_msg);
307
+
308
+ // Set the metadata for the excluded points message based on the input cloud
309
+ excluded_points_msg.height = 1 ;
310
+ excluded_points_msg.width =
311
+ static_cast <uint32_t >(output.data .size () / output.height / output.point_step );
312
+ excluded_points_msg.row_step = static_cast <uint32_t >(output.data .size () / output.height );
313
+ excluded_points_msg.is_bigendian = input.is_bigendian ;
314
+ excluded_points_msg.is_dense = input.is_dense ;
315
+ excluded_points_msg.header = input.header ;
316
+ excluded_points_msg.header .frame_id =
317
+ !tf_input_frame_.empty () ? tf_input_frame_ : tf_input_orig_frame_;
318
+
319
+ return excluded_points_msg;
320
+ }
321
+
251
322
} // namespace pointcloud_preprocessor
252
323
253
324
#include < rclcpp_components/register_node_macro.hpp>
0 commit comments