16
16
#define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_
17
17
18
18
#include " pointcloud_preprocessor/filter.hpp"
19
+ #include " pointcloud_preprocessor/transform_info.hpp"
19
20
20
21
#include < vehicle_info_util/vehicle_info.hpp>
21
22
@@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
50
51
// classified point label
51
52
// (0: not classified, 1: ground, 2: not ground, 3: follow previous point,
52
53
// 4: unkown(currently not used), 5: virtual ground)
53
- enum class PointLabel {
54
+ enum class PointLabel : uint16_t {
54
55
INIT = 0 ,
55
56
GROUND,
56
57
NON_GROUND,
@@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
59
60
VIRTUAL_GROUND,
60
61
OUT_OF_RANGE
61
62
};
62
- struct PointRef
63
+
64
+ struct PointData
63
65
{
64
- float grid_size; // radius of grid
65
- uint16_t grid_id; // id of grid in vertical
66
- float radius; // cylindrical coords on XY Plane
67
- float theta; // angle deg on XY plane
68
- size_t radial_div; // index of the radial division to which this point belongs to
66
+ float radius; // cylindrical coords on XY Plane
69
67
PointLabel point_state{PointLabel::INIT};
70
-
68
+ uint16_t grid_id; // id of grid in vertical
71
69
size_t orig_index; // index of this point in the source pointcloud
72
- pcl::PointXYZ * orig_point;
73
70
};
74
- using PointCloudRefVector = std::vector<PointRef >;
71
+ using PointCloudVector = std::vector<PointData >;
75
72
76
73
struct GridCenter
77
74
{
@@ -144,33 +141,57 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
144
141
145
142
pcl::PointIndices getIndices () { return pcl_indices; }
146
143
std::vector<float > getHeightList () { return height_list; }
144
+
145
+ pcl::PointIndices & getIndicesRef () { return pcl_indices; }
146
+ std::vector<float > & getHeightListRef () { return height_list; }
147
147
};
148
148
149
149
void filter (
150
150
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override ;
151
151
152
+ // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes
153
+ // conform to new API
154
+ virtual void faster_filter (
155
+ const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
156
+ const pointcloud_preprocessor::TransformInfo & transform_info);
157
+
152
158
tf2_ros::Buffer tf_buffer_{get_clock ()};
153
159
tf2_ros::TransformListener tf_listener_{tf_buffer_};
154
160
161
+ int x_offset_;
162
+ int y_offset_;
163
+ int z_offset_;
164
+ int intensity_offset_;
165
+ bool offset_initialized_;
166
+
167
+ void set_field_offsets (const PointCloud2ConstPtr & input);
168
+
169
+ void get_point_from_global_offset (
170
+ const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset);
171
+
155
172
const uint16_t gnd_grid_continual_thresh_ = 3 ;
156
173
bool elevation_grid_mode_;
157
174
float non_ground_height_threshold_;
158
175
float grid_size_rad_;
176
+ float tan_grid_size_rad_;
159
177
float grid_size_m_;
160
178
float low_priority_region_x_;
161
179
uint16_t gnd_grid_buffer_size_;
162
180
float grid_mode_switch_grid_id_;
163
181
float grid_mode_switch_angle_rad_;
164
182
float virtual_lidar_z_;
165
183
float detection_range_z_max_;
166
- float center_pcl_shift_; // virtual center of pcl to center mass
167
- float grid_mode_switch_radius_; // non linear grid size switching distance
168
- double global_slope_max_angle_rad_; // radians
169
- double local_slope_max_angle_rad_; // radians
184
+ float center_pcl_shift_; // virtual center of pcl to center mass
185
+ float grid_mode_switch_radius_; // non linear grid size switching distance
186
+ double global_slope_max_angle_rad_; // radians
187
+ double local_slope_max_angle_rad_; // radians
188
+ double global_slope_max_ratio_;
189
+ double local_slope_max_ratio_;
170
190
double radial_divider_angle_rad_; // distance in rads between dividers
171
191
double split_points_distance_tolerance_; // distance in meters between concentric divisions
172
- double // minimum height threshold regardless the slope,
173
- split_height_distance_; // useful for close points
192
+ double split_points_distance_tolerance_square_;
193
+ double // minimum height threshold regardless the slope,
194
+ split_height_distance_; // useful for close points
174
195
bool use_virtual_ground_point_;
175
196
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
176
197
size_t radial_dividers_num_;
@@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
186
207
*/
187
208
188
209
/* !
189
- * Convert pcl::PointCloud to sorted PointCloudRefVector
210
+ * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector
190
211
* @param[in] in_cloud Input Point Cloud to be organized in radial segments
191
212
* @param[out] out_radial_ordered_points_manager Vector of Points Clouds,
192
213
* each element will contain the points ordered
193
214
*/
194
215
void convertPointcloud (
195
- const pcl::PointCloud<pcl::PointXYZ>:: Ptr in_cloud,
196
- std::vector<PointCloudRefVector > & out_radial_ordered_points_manager);
216
+ const PointCloud2ConstPtr & in_cloud,
217
+ std::vector<PointCloudVector > & out_radial_ordered_points_manager);
197
218
void convertPointcloudGridScan (
198
- const pcl::PointCloud<pcl::PointXYZ>:: Ptr in_cloud,
199
- std::vector<PointCloudRefVector > & out_radial_ordered_points_manager);
219
+ const PointCloud2ConstPtr & in_cloud,
220
+ std::vector<PointCloudVector > & out_radial_ordered_points_manager);
200
221
/* !
201
222
* Output ground center of front wheels as the virtual ground point
202
223
* @param[out] point Virtual ground origin point
203
224
*/
204
225
void calcVirtualGroundOrigin (pcl::PointXYZ & point);
205
226
227
+ float calcGridSize (const PointData & p);
228
+
206
229
/* !
207
230
* Classifies Points in the PointCloud as Ground and Not Ground
208
231
* @param in_radial_ordered_clouds Vector of an Ordered PointsCloud
@@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
214
237
void initializeFirstGndGrids (
215
238
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);
216
239
217
- void checkContinuousGndGrid (PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
218
- void checkDiscontinuousGndGrid (PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
219
- void checkBreakGndGrid (PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
240
+ void checkContinuousGndGrid (
241
+ PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
242
+ void checkDiscontinuousGndGrid (
243
+ PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
244
+ void checkBreakGndGrid (
245
+ PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
220
246
void classifyPointCloud (
221
- std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
247
+ const PointCloud2ConstPtr & in_cloud_ptr,
248
+ std::vector<PointCloudVector> & in_radial_ordered_clouds,
222
249
pcl::PointIndices & out_no_ground_indices);
223
250
void classifyPointCloudGridScan (
224
- std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
251
+ const PointCloud2ConstPtr & in_cloud_ptr,
252
+ std::vector<PointCloudVector> & in_radial_ordered_clouds,
225
253
pcl::PointIndices & out_no_ground_indices);
226
254
/* !
227
255
* Re-classifies point of ground cluster based on their height
@@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
237
265
* and the other removed as indicated in the indices
238
266
* @param in_cloud_ptr Input PointCloud to which the extraction will be performed
239
267
* @param in_indices Indices of the points to be both removed and kept
240
- * @param out_object_cloud_ptr Resulting PointCloud with the indices kept
268
+ * @param out_object_cloud Resulting PointCloud with the indices kept
241
269
*/
242
270
void extractObjectPoints (
243
- const pcl::PointCloud<pcl::PointXYZ>:: Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
244
- pcl::PointCloud<pcl::PointXYZ>:: Ptr out_object_cloud_ptr );
271
+ const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices,
272
+ PointCloud2 & out_object_cloud );
245
273
246
274
/* * \brief Parameter service callback result : needed to be hold */
247
275
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
0 commit comments