-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsac.launch
73 lines (69 loc) · 2.48 KB
/
sac.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
<launch>
<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
<!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
<remap from="~input" to="/camera/depth/points_throttle" />
<rosparam>
filter_field_name: z
filter_limit_min: 0.01
filter_limit_max: 1.5
filter_limit_negative: False
leaf_size: 0.075
</rosparam>
</node>
<!-- Estimate point normals -->
<node pkg="nodelet" type="nodelet" name="normal_estimation" args="load pcl/NormalEstimation pcl_manager" output="screen">
<remap from="~input" to="/voxel_grid/output" />
<rosparam>
# -[ Mandatory parameters
k_search: 0
radius_search: 0.15
# Set the spatial locator. Possible values are: 0 (ANN), 1 (FLANN), 2 (organized)
spatial_locator: 0
</rosparam>
</node>
<!-- Segment the table plane -->
<node pkg="nodelet" type="nodelet" name="planar_segmentation" args="load pcl/SACSegmentationFromNormals pcl_manager" output="screen">
<remap from="~input" to="/voxel_grid/output" />
<remap from="~normals" to="/normal_estimation/output" />
<rosparam>
# -[ Mandatory parameters
# model_type:
# 0: SACMODEL_PLANE
# 1: SACMODEL_LINE
# 2: SACMODEL_CIRCLE2D
# 3: SACMODEL_CIRCLE3D
# 4: SACMODEL_SPHERE
# 5: SACMODEL_CYLINDER
# 6: SACMODEL_CONE
# 7: SACMODEL_TORUS
# 8: SACMODEL_PARALLEL_LINE
# 9: SACMODEL_PERPENDICULAR_PLANE
# 10: SACMODEL_PARALLEL_LINES
# 11: SACMODEL_NORMAL_PLANE
# 12: SACMODEL_NORMAL_SPHERE
# 13: SACMODEL_REGISTRATION
# 14: SACMODEL_REGISTRATION_2D
# 15: SACMODEL_PARALLEL_PLANE
# 16: SACMODEL_NORMAL_PARALLEL_PLANE
# 17: SACMODEL_STICK
model_type: 9
distance_threshold: 0.05
max_iterations: 1000
method_type: 0
optimize_coefficients: true
normal_distance_weight: 0.1
eps_angle: 0.2 #0.09
axis: [0.0,1.0,0.0]
</rosparam>
</node>
<node pkg="nodelet" type="nodelet" name="extract_plane_outliers" args="load pcl/ExtractIndices pcl_manager" output="screen">
<remap from="~input" to="/voxel_grid/output" />
<remap from="~indices" to="/planar_segmentation/inliers" />
<remap from="~output" to="plane_outliers" />
<rosparam>
negative: true
approximate_sync: true
</rosparam>
</node>
</launch>