@@ -33,7 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
33
33
using tier4_autoware_utils::StopWatch;
34
34
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
35
35
debug_publisher_ = std::make_unique<DebugPublisher>(this , " ring_outlier_filter" );
36
- noise_points_publisher_ = this ->create_publisher <PointCloud2>(" noise/ring_outlier_filter" , 1 );
36
+ outlier_pointcloud_publisher_ =
37
+ this ->create_publisher <PointCloud2>(" noise/ring_outlier_filter" , 1 );
37
38
stop_watch_ptr_->tic (" cyclic_time" );
38
39
stop_watch_ptr_->tic (" processing_time" );
39
40
}
@@ -47,7 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions
47
48
max_rings_num_ = static_cast <uint16_t >(declare_parameter (" max_rings_num" , 128 ));
48
49
max_points_num_per_ring_ =
49
50
static_cast <size_t >(declare_parameter (" max_points_num_per_ring" , 4000 ));
50
- publish_noise_points_ = static_cast <bool >(declare_parameter (" publish_noise_points" , false ));
51
+ publish_outlier_pointcloud_ =
52
+ static_cast <bool >(declare_parameter (" publish_outlier_pointcloud" , false ));
51
53
}
52
54
53
55
using std::placeholders::_1;
@@ -72,11 +74,11 @@ void RingOutlierFilterComponent::faster_filter(
72
74
size_t output_size = 0 ;
73
75
74
76
// Set up the noise points cloud, if noise points are to be published.
75
- PointCloud2 noise_points ;
76
- size_t noise_points_size = 0 ;
77
- if (publish_noise_points_ ) {
78
- noise_points .point_step = sizeof (PointXYZI);
79
- noise_points .data .resize (noise_points .point_step * input->width );
77
+ PointCloud2 outlier_points ;
78
+ size_t outlier_points_size = 0 ;
79
+ if (publish_outlier_pointcloud_ ) {
80
+ outlier_points .point_step = sizeof (PointXYZI);
81
+ outlier_points .data .resize (outlier_points .point_step * input->width );
80
82
}
81
83
82
84
const auto ring_offset =
@@ -159,25 +161,26 @@ void RingOutlierFilterComponent::faster_filter(
159
161
160
162
output_size += output.point_step ;
161
163
}
162
- } else if (publish_noise_points_ ) {
164
+ } else if (publish_outlier_pointcloud_ ) {
163
165
for (int i = walk_first_idx; i <= walk_last_idx; i++) {
164
- auto noise_ptr = reinterpret_cast <PointXYZI *>(&noise_points.data [noise_points_size]);
166
+ auto outlier_ptr =
167
+ reinterpret_cast <PointXYZI *>(&outlier_points.data [outlier_points_size]);
165
168
auto input_ptr =
166
169
reinterpret_cast <const PointXYZI *>(&input->data [indices[walk_first_idx]]);
167
170
if (transform_info.need_transform ) {
168
171
Eigen::Vector4f p (input_ptr->x , input_ptr->y , input_ptr->z , 1 );
169
172
p = transform_info.eigen_transform * p;
170
- noise_ptr ->x = p[0 ];
171
- noise_ptr ->y = p[1 ];
172
- noise_ptr ->z = p[2 ];
173
+ outlier_ptr ->x = p[0 ];
174
+ outlier_ptr ->y = p[1 ];
175
+ outlier_ptr ->z = p[2 ];
173
176
} else {
174
- *noise_ptr = *input_ptr;
177
+ *outlier_ptr = *input_ptr;
175
178
}
176
179
const float & intensity = *reinterpret_cast <const float *>(
177
180
&input->data [indices[walk_first_idx] + intensity_offset]);
178
- noise_ptr ->intensity = intensity;
181
+ outlier_ptr ->intensity = intensity;
179
182
180
- noise_points_size += noise_points .point_step ;
183
+ outlier_points_size += outlier_points .point_step ;
181
184
}
182
185
}
183
186
@@ -208,32 +211,32 @@ void RingOutlierFilterComponent::faster_filter(
208
211
209
212
output_size += output.point_step ;
210
213
}
211
- } else if (publish_noise_points_ ) {
214
+ } else if (publish_outlier_pointcloud_ ) {
212
215
for (int i = walk_first_idx; i < walk_last_idx; i++) {
213
- auto noise_ptr = reinterpret_cast <PointXYZI *>(&noise_points .data [noise_points_size ]);
216
+ auto outlier_ptr = reinterpret_cast <PointXYZI *>(&outlier_points .data [outlier_points_size ]);
214
217
auto input_ptr = reinterpret_cast <const PointXYZI *>(&input->data [indices[i]]);
215
218
if (transform_info.need_transform ) {
216
219
Eigen::Vector4f p (input_ptr->x , input_ptr->y , input_ptr->z , 1 );
217
220
p = transform_info.eigen_transform * p;
218
- noise_ptr ->x = p[0 ];
219
- noise_ptr ->y = p[1 ];
220
- noise_ptr ->z = p[2 ];
221
+ outlier_ptr ->x = p[0 ];
222
+ outlier_ptr ->y = p[1 ];
223
+ outlier_ptr ->z = p[2 ];
221
224
} else {
222
- *noise_ptr = *input_ptr;
225
+ *outlier_ptr = *input_ptr;
223
226
}
224
227
const float & intensity =
225
228
*reinterpret_cast <const float *>(&input->data [indices[i] + intensity_offset]);
226
- noise_ptr ->intensity = intensity;
227
- noise_points_size += noise_points .point_step ;
229
+ outlier_ptr ->intensity = intensity;
230
+ outlier_points_size += outlier_points .point_step ;
228
231
}
229
232
}
230
233
}
231
234
232
235
setUpPointCloudFormat (input, output, output_size, /* num_fields=*/ 4 );
233
236
234
- if (publish_noise_points_ ) {
235
- setUpPointCloudFormat (input, noise_points, noise_points_size , /* num_fields=*/ 4 );
236
- noise_points_publisher_ ->publish (noise_points );
237
+ if (publish_outlier_pointcloud_ ) {
238
+ setUpPointCloudFormat (input, outlier_points, outlier_points_size , /* num_fields=*/ 4 );
239
+ outlier_pointcloud_publisher_ ->publish (outlier_points );
237
240
}
238
241
239
242
// add processing time for debug
@@ -281,8 +284,9 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
281
284
if (get_param (p, " num_points_threshold" , num_points_threshold_)) {
282
285
RCLCPP_DEBUG (get_logger (), " Setting new num_points_threshold to: %d." , num_points_threshold_);
283
286
}
284
- if (get_param (p, " publish_noise_points" , publish_noise_points_)) {
285
- RCLCPP_DEBUG (get_logger (), " Setting new publish_noise_points to: %d." , publish_noise_points_);
287
+ if (get_param (p, " publish_outlier_pointcloud" , publish_outlier_pointcloud_)) {
288
+ RCLCPP_DEBUG (
289
+ get_logger (), " Setting new publish_outlier_pointcloud to: %d." , publish_outlier_pointcloud_);
286
290
}
287
291
288
292
rcl_interfaces::msg::SetParametersResult result;
0 commit comments