-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathobject_recognition.launch
264 lines (241 loc) · 28.9 KB
/
object_recognition.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
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<!-- control of nodes startup -->
<arg name="load_object_recognition_skill_server_node" default="true" />
<arg name="load_object_recognition_skill_tf_publisher_node" default="false" />
<arg name="enable_respawn" default="true" />
<!-- namespace for parameters -->
<arg name="namespace" default="object_recognition" />
<arg name="namespace_object_recognition_skill_server" default="object_recognition_skill_server" />
<arg name="namespace_tf_publisher" default="pose_to_tf_publisher" />
<!-- topics -->
<arg name="pose_with_covariance_stamped_topic" default="initial_pose" /> <!-- rviz topic -->
<arg name="pose_stamped_publish_topic" default="localization_pose" />
<arg name="ambient_pointcloud_topic" default="/camera/depth_registered/points" />
<!-- frame ids -->
<arg name="sensor_frame_id" default="camera_link" />
<arg name="base_link_frame_id" default="$(arg sensor_frame_id)" />
<arg name="odom_frame_id" default="$(arg base_link_frame_id)" />
<arg name="object_frame_id" default="object" />
<arg name="object_frame_id_for_transforming_pointclouds" default="$(arg base_link_frame_id)" />
<arg name="object_frame_id_for_publishing_pointclouds" default="$(arg base_link_frame_id)" />
<!-- initial pose in object frame -->
<arg name="initial_pose_available" default="false" />
<arg name="initial_pose_x" default="0.0" />
<arg name="initial_pose_y" default="0.0" />
<arg name="initial_pose_z" default="0.0" />
<arg name="initial_pose_roll" default="0.0" />
<arg name="initial_pose_pitch" default="0.0" />
<arg name="initial_pose_yaw" default="0.0" />
<!-- 3d processing pipeline parameters for cloud storage -->
<arg name="use_cloud_storage_mode" default="false" />
<arg name="use_voxel_grid_segmentation_in_cloud_storage_mode" default="false" />
<arg name="use_random_sample_segmentation_in_cloud_storage_mode" default="true" />
<!-- 3d processing pipeline parameters for cloud registration -->
<arg name="use_voxel_grid" default="true" />
<arg name="use_random_sample" default="false" />
<arg name="use_noise_removal" default="true" />
<arg name="use_normal_estimation" default="true" />
<arg name="use_plane_segmentation" default="true" />
<arg name="use_region_growing_segmentation" default="false" />
<arg name="use_euclidean_clustering_segmentation" default="true" />
<arg name="use_feature_matching_for_initial_alignment" default="true" />
<arg name="use_curvature_estimation" default="$(arg use_feature_matching_for_initial_alignment)" />
<arg name="use_pca_for_initial_alignment" default="false" />
<arg name="use_filters_for_outlier_detection" default="false" />
<arg name="load_initial_pose_estimation_pipeline_using_pca" default="$(eval use_pca_for_initial_alignment and not use_feature_matching_for_initial_alignment)" />
<arg name="use_cloud_matching" default="true" />
<arg name="use_iterative_closest_point_with_normals" default="true" />
<arg name="load_tracking_pipeline" default="false" />
<arg name="compute_keypoints_when_tracking_pose" default="false" />
<arg name="feature_matching_timeout_for_initial_alignment" default="3.0" />
<arg name="cloud_matching_timeout_for_intermediate_alignment" default="1.8" />
<arg name="cloud_matching_timeout_for_final_alignment" default="0.7" />
<arg name="use_transformation_aligner" default="false" />
<arg name="max_registration_outliers" default="0.1" />
<arg name="max_registration_outliers_reference_pointcloud" default="0.7" />
<!-- configuration files (empty paths can be given for avoiding the rosparam load) -->
<arg name="yaml_configuration_configs_filename" default="$(find object_recognition_skill_server)/yaml/configs.yaml" />
<arg name="yaml_configuration_filters_filename" default="" />
<arg name="yaml_configuration_filters_roi_filename" default="$(find object_recognition_skill_server)/yaml/filters_roi.yaml" />
<arg name="yaml_configuration_filters_ambient_pointcloud_outlier_detection_filename" default="$(find object_recognition_skill_server)/yaml/filters_ambient_pointcloud_outlier_detection.yaml" />
<arg name="yaml_configuration_filters_ambient_pointcloud_integration_voxel_grid_filename" default="$(find object_recognition_skill_server)/yaml/filters_ambient_pointcloud_integration_voxel_grid.yaml" />
<arg name="yaml_configuration_filters_ambient_pointcloud_integration_random_sample_filename" default="$(find object_recognition_skill_server)/yaml/filters_ambient_pointcloud_integration_random_sample.yaml" />
<arg name="yaml_configuration_filters_voxel_grid_filename" default="$(find object_recognition_skill_server)/yaml/filters_voxel_grid.yaml" />
<arg name="yaml_configuration_filters_random_sample_filename" default="$(find object_recognition_skill_server)/yaml/filters_random_sample.yaml" />
<arg name="yaml_configuration_filters_noise_removal_filename" default="$(find object_recognition_skill_server)/yaml/filters_noise_removal.yaml" />
<arg name="yaml_configuration_filters_plane_segmentation_filename" default="$(find object_recognition_skill_server)/yaml/filters_plane_segmentation.yaml" />
<arg name="yaml_configuration_filters_euclidean_clustering_filename" default="$(find object_recognition_skill_server)/yaml/filters_euclidean_clustering.yaml" />
<arg name="yaml_configuration_filters_region_growing_filename" default="$(find object_recognition_skill_server)/yaml/filters_region_growing.yaml" />
<arg name="yaml_configuration_normal_estimators_filename" default="$(find object_recognition_skill_server)/yaml/normal_estimators.yaml" />
<arg name="yaml_configuration_curvature_estimators_filename" default="$(find object_recognition_skill_server)/yaml/curvature_estimators.yaml" />
<arg name="yaml_configuration_keypoint_detectors_filename" default="$(find object_recognition_skill_server)/yaml/keypoint_detectors.yaml" />
<arg name="yaml_configuration_initial_pose_estimators_feature_matchers_filename" default="$(find object_recognition_skill_server)/yaml/initial_pose_estimators_feature_matchers.yaml" />
<arg name="yaml_configuration_initial_pose_estimators_pca_matchers_filename" default="$(find object_recognition_skill_server)/yaml/initial_pose_estimators_pca_matchers.yaml" if="$(eval base_link_frame_id == 'camera_link')" />
<arg name="yaml_configuration_initial_pose_estimators_pca_matchers_filename" default="$(find object_recognition_skill_server)/yaml/initial_pose_estimators_pca_matchers_with_base_link_axes_flip.yaml" unless="$(eval base_link_frame_id == 'camera_link')" />
<arg name="yaml_configuration_initial_pose_estimators_point_matchers_filename" default="$(find object_recognition_skill_server)/yaml/initial_pose_estimators_point_matchers_with_normals.yaml" if="$(arg use_iterative_closest_point_with_normals)" />
<arg name="yaml_configuration_initial_pose_estimators_point_matchers_filename" default="$(find object_recognition_skill_server)/yaml/initial_pose_estimators_point_matchers.yaml" unless="$(arg use_iterative_closest_point_with_normals)" />
<arg name="yaml_configuration_transformation_aligner_filename" default="$(find object_recognition_skill_server)/yaml/transformation_aligner.yaml" />
<arg name="yaml_configuration_outlier_detectors_filename" default="$(find object_recognition_skill_server)/yaml/outlier_detectors.yaml" if="$(eval not load_initial_pose_estimation_pipeline_using_pca or (load_initial_pose_estimation_pipeline_using_pca and use_pca_for_initial_alignment))" />
<arg name="yaml_configuration_outlier_detectors_filename" default="" if="$(eval load_initial_pose_estimation_pipeline_using_pca and not use_pca_for_initial_alignment)" />
<arg name="yaml_configuration_transformation_validators_filename" default="$(find object_recognition_skill_server)/yaml/transformation_validators.yaml" />
<arg name="yaml_configuration_tracking_filename" default="$(find object_recognition_skill_server)/yaml/pose_tracking.yaml" />
<arg name="yaml_configuration_recovery_filename" default="$(find object_recognition_skill_server)/yaml/pose_recovery.yaml" />
<arg name="yaml_configuration_custom_filename" default="" /> <!-- extra custom configurations loaded last -->
<!-- tfs -->
<arg name="feature_matching_publish_tf" default="true" />
<arg name="feature_matching_tf_broadcaster_frame_id_suffix" default="feature_matching" />
<arg name="pca_matching_publish_tf" default="true" />
<arg name="pca_tf_broadcaster_frame_id_suffix" default="pca_matching" />
<arg name="pca_matching_ns" default="1_principal_component_analysis" />
<arg name="point_matcher_intermediate_ns" default="2_iterative_closest_point_with_normals" if="$(arg use_iterative_closest_point_with_normals)" />
<arg name="point_matcher_intermediate_ns" default="2_iterative_closest_point" unless="$(arg use_iterative_closest_point_with_normals)" />
<arg name="point_matcher_final_ns" default="3_iterative_closest_point_with_normals" if="$(arg use_iterative_closest_point_with_normals)" />
<arg name="point_matcher_final_ns" default="3_iterative_closest_point" unless="$(arg use_iterative_closest_point_with_normals)" />
<arg name="point_matching_publish_tf" default="true" />
<arg name="point_matching_intermediate_tf_broadcaster_frame_id_suffix" default="intermediate_point_matching" />
<arg name="point_matching_tf_broadcaster_frame_id_suffix" default="point_matching" />
<arg name="publish_pose_tf_rate" default="-5.0" />
<!-- configurations -->
<arg name="action_server_name" default="ObjectRecognitionSkill"/>
<arg name="use_object_model_caching" default="true"/>
<arg name="number_of_recognition_retries" default="0" if="$(arg use_cloud_storage_mode)"/>
<arg name="number_of_recognition_retries" default="3" unless="$(arg use_cloud_storage_mode)"/>
<arg name="on_failure_try_perception_on_other_clusters" default="true"/>
<arg name="clustering_module_parameter_server_namespace" default="filters/ambient_pointcloud_filters_after_normal_estimation/3_euclidean_clustering/cluster_selector/" if="$(eval use_euclidean_clustering_segmentation)" />
<arg name="clustering_module_parameter_server_namespace" default="filters/ambient_pointcloud_filters_after_normal_estimation/4_region_growing/cluster_selector/" if="$(eval use_region_growing_segmentation)" />
<arg name="clustering_module_parameter_server_namespace" default="" if="$(eval not use_euclidean_clustering_segmentation and not use_region_growing_segmentation)" />
<arg name="principal_component_analysis_module_parameter_server_namespace" default="initial_pose_estimators_matchers/point_matchers/1_principal_component_analysis/" if="$(arg load_initial_pose_estimation_pipeline_using_pca)" />
<arg name="principal_component_analysis_module_parameter_server_namespace" default="" unless="$(arg load_initial_pose_estimation_pipeline_using_pca)" />
<!-- reference map data -->
<arg name="reference_pointclouds_database_folder_path" default="$(find object_recognition_skill_server)/models/" />
<arg name="reference_pointcloud_filename" default="" />
<arg name="reference_pointcloud_preprocessed_save_filename" default="" />
<arg name="reference_pointcloud_available" default="$(eval bool(reference_pointcloud_filename))" />
<arg name="reference_pointcloud_required" default="$(eval (use_feature_matching_for_initial_alignment or use_pca_for_initial_alignment or use_cloud_matching or bool(yaml_configuration_outlier_detectors_filename)) and not use_cloud_storage_mode)" />
<arg name="reference_pointcloud_type" default="3D" />
<arg name="reference_pointcloud_update_mode" default="NoIntegration" />
<arg name="reference_pointcloud_keypoints_filename" default="" />
<arg name="reference_pointcloud_keypoints_save_filename" default="" />
<arg name="reference_pointcloud_descriptors_filename" default="" />
<arg name="reference_pointcloud_descriptors_save_filename" default="" />
<group ns="$(arg namespace)">
<group ns="$(arg namespace_object_recognition_skill_server)" clear_params="true">
<param name="action_server_name" type="str" value="$(arg action_server_name)" />
<param name="use_object_model_caching" type="bool" value="$(arg use_object_model_caching)" />
<param name="number_of_recognition_retries" type="int" value="$(arg number_of_recognition_retries)" />
<param name="on_failure_try_perception_on_other_clusters" type="bool" value="$(arg on_failure_try_perception_on_other_clusters)" />
<param name="clustering_module_parameter_server_namespace" type="str" value="$(arg clustering_module_parameter_server_namespace)" />
<param name="principal_component_analysis_module_parameter_server_namespace" type="str" value="$(arg principal_component_analysis_module_parameter_server_namespace)" />
<param name="frame_ids/map_frame_id" type="str" value="$(arg object_frame_id)" />
<param name="frame_ids/map_frame_id_for_transforming_pointclouds" type="str" value="$(arg object_frame_id_for_transforming_pointclouds)" />
<param name="frame_ids/map_frame_id_for_publishing_pointclouds" type="str" value="$(arg object_frame_id_for_publishing_pointclouds)" />
<param name="frame_ids/odom_frame_id" type="str" value="$(arg odom_frame_id)" />
<param name="frame_ids/base_link_frame_id" type="str" value="$(arg base_link_frame_id)" />
<param name="frame_ids/sensor_frame_id" type="str" value="$(arg sensor_frame_id)" />
<param name="initial_pose/robot_initial_pose_available" type="bool" value="$(arg initial_pose_available)" />
<param name="initial_pose/position/x" type="double" value="$(arg initial_pose_x)" />
<param name="initial_pose/position/y" type="double" value="$(arg initial_pose_y)" />
<param name="initial_pose/position/z" type="double" value="$(arg initial_pose_z)" />
<param name="initial_pose/orientation_rpy/roll" type="double" value="$(arg initial_pose_roll)" />
<param name="initial_pose/orientation_rpy/pitch" type="double" value="$(arg initial_pose_pitch)" />
<param name="initial_pose/orientation_rpy/yaw" type="double" value="$(arg initial_pose_yaw)" />
<param name="subscribe_topic_names/pose_topic" type="str" value="" />
<param name="subscribe_topic_names/pose_stamped_topic" type="str" value="" />
<param name="subscribe_topic_names/pose_with_covariance_stamped_topic" type="str" value="$(arg pose_with_covariance_stamped_topic)" />
<param name="subscribe_topic_names/ambient_pointcloud_topic" type="str" value="$(arg ambient_pointcloud_topic)" />
<param name="subscribe_topic_names/reference_costmap_topic" type="str" value="" />
<param name="subscribe_topic_names/reference_pointcloud_topic" type="str" value="" />
<param name="publish_topic_names/pose_with_covariance_stamped_publish_topic" type="str" value="" />
<param name="publish_topic_names/pose_stamped_publish_topic" type="str" value="$(arg pose_stamped_publish_topic)" />
<param name="reference_pointclouds_database_folder_path" type="str" value="$(arg reference_pointclouds_database_folder_path)" />
<param name="reference_pointclouds/reference_pointcloud_filename" type="str" value="$(arg reference_pointcloud_filename)" />
<param name="reference_pointclouds/reference_pointcloud_preprocessed_save_filename" type="str" value="$(arg reference_pointcloud_preprocessed_save_filename)" />
<param name="reference_pointclouds/reference_pointcloud_type" type="str" value="$(arg reference_pointcloud_type)" />
<param name="reference_pointclouds/reference_pointcloud_available" type="bool" value="$(arg reference_pointcloud_available)" />
<param name="reference_pointclouds/reference_pointcloud_required" type="bool" value="$(arg reference_pointcloud_required)" />
<param name="reference_pointclouds/reference_pointcloud_update_mode" type="str" value="$(arg reference_pointcloud_update_mode)" />
<rosparam command="load" file="$(arg yaml_configuration_configs_filename)" subst_value="true" if="$(eval bool(yaml_configuration_configs_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_filters_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_filters_roi_filename)" subst_value="false" if="$(eval bool(yaml_configuration_filters_roi_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_filters_noise_removal_filename)" subst_value="true" ns="filters/ambient_pointcloud_integration_filters_map_frame" if="$(eval bool(yaml_configuration_filters_noise_removal_filename) and use_noise_removal and use_cloud_storage_mode)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_noise_removal_filename)" subst_value="true" ns="filters/ambient_pointcloud_map_frame_feature_registration" if="$(eval bool(yaml_configuration_filters_noise_removal_filename) and use_noise_removal and not use_cloud_storage_mode)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_plane_segmentation_filename)" subst_value="false" if="$(eval bool(yaml_configuration_filters_plane_segmentation_filename) and use_plane_segmentation)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_euclidean_clustering_filename)" subst_value="false" if="$(eval bool(yaml_configuration_filters_euclidean_clustering_filename) and use_euclidean_clustering_segmentation)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_region_growing_filename)" subst_value="false" if="$(eval bool(yaml_configuration_filters_region_growing_filename) and use_region_growing_segmentation)" />
<rosparam command="load" file="$(arg yaml_configuration_normal_estimators_filename)" subst_value="true" if="$(eval bool(yaml_configuration_normal_estimators_filename) and use_normal_estimation)" />
<rosparam command="load" file="$(arg yaml_configuration_custom_filename)" subst_value="true" if="$(eval bool(yaml_configuration_custom_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_filters_ambient_pointcloud_outlier_detection_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_ambient_pointcloud_outlier_detection_filename) and use_filters_for_outlier_detection)" />
<group if="$(arg use_cloud_storage_mode)" >
<rosparam command="load" file="$(arg yaml_configuration_filters_ambient_pointcloud_integration_voxel_grid_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_ambient_pointcloud_integration_voxel_grid_filename) and use_voxel_grid_segmentation_in_cloud_storage_mode)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_ambient_pointcloud_integration_random_sample_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_ambient_pointcloud_integration_random_sample_filename) and use_random_sample_segmentation_in_cloud_storage_mode)" />
</group>
<group unless="$(arg use_cloud_storage_mode)">
<group if="$(arg use_feature_matching_for_initial_alignment)">
<param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_filename" type="str" value="$(arg reference_pointcloud_keypoints_filename)" />
<param name="keypoint_detectors/reference_pointcloud/reference_pointcloud_keypoints_save_filename" type="str" value="$(arg reference_pointcloud_keypoints_save_filename)" />
<param name="keypoint_detectors/ambient_pointcloud/compute_keypoints_when_tracking_pose" type="bool" value="$(arg compute_keypoints_when_tracking_pose)" />
<param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_filename" type="str" value="$(arg reference_pointcloud_descriptors_filename)" />
<param name="initial_pose_estimators_matchers/feature_matchers/reference_pointcloud_descriptors_save_filename" type="str" value="$(arg reference_pointcloud_descriptors_save_filename)" />
<param name="initial_pose_estimators_matchers/feature_matchers/matchers/sample_consensus_initial_alignment_prerejective/tf_publisher/publish_tf" type="bool" value="$(arg feature_matching_publish_tf)" />
<param name="initial_pose_estimators_matchers/feature_matchers/matchers/sample_consensus_initial_alignment_prerejective/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg base_link_frame_id)" />
<param name="initial_pose_estimators_matchers/feature_matchers/matchers/sample_consensus_initial_alignment_prerejective/tf_publisher/tf_broadcaster_child_frame_id" type="str" value="$(arg object_frame_id)__$(arg feature_matching_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/feature_matchers/matchers/sample_consensus_initial_alignment_prerejective/reference_cloud_publish_topic_frame_id" type="str" value="$(arg object_frame_id)__$(arg feature_matching_tf_broadcaster_frame_id_suffix)" />
<rosparam command="load" file="$(arg yaml_configuration_keypoint_detectors_filename)" subst_value="true" if="$(eval bool(yaml_configuration_keypoint_detectors_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_initial_pose_estimators_feature_matchers_filename)" subst_value="true" if="$(eval bool(yaml_configuration_initial_pose_estimators_feature_matchers_filename))" />
<param name="initial_pose_estimators_matchers/feature_matchers/matchers/sample_consensus_initial_alignment_prerejective/convergence_time_limit_seconds" type="double" value="$(arg feature_matching_timeout_for_initial_alignment)" />
</group>
<group if="$(arg load_initial_pose_estimation_pipeline_using_pca)">
<rosparam command="load" file="$(arg yaml_configuration_initial_pose_estimators_pca_matchers_filename)" subst_value="true" if="$(eval bool(yaml_configuration_initial_pose_estimators_pca_matchers_filename))" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg pca_matching_ns)/compute_offset_to_reference_pointcloud_pca" type="bool" value="$(arg use_pca_for_initial_alignment)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg pca_matching_ns)/tf_publisher/publish_tf" type="bool" value="$(arg pca_matching_publish_tf)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg pca_matching_ns)/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg base_link_frame_id)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg pca_matching_ns)/tf_publisher/tf_broadcaster_child_frame_id" type="str" value="$(arg object_frame_id)__$(arg pca_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg pca_matching_ns)/reference_cloud_publish_topic_frame_id" type="str" value="$(arg object_frame_id)__$(arg pca_tf_broadcaster_frame_id_suffix)" />
</group>
<group if="$(arg use_cloud_matching)">
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/tf_publisher/publish_tf" type="bool" value="$(arg point_matching_publish_tf)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg object_frame_id)__$(arg feature_matching_tf_broadcaster_frame_id_suffix)" if="$(eval use_feature_matching_for_initial_alignment)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg object_frame_id)__$(arg pca_tf_broadcaster_frame_id_suffix)" if="$(eval load_initial_pose_estimation_pipeline_using_pca)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg base_link_frame_id)" if="$(eval not use_feature_matching_for_initial_alignment and not use_pca_for_initial_alignment)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/tf_publisher/tf_broadcaster_child_frame_id" type="str" value="$(arg object_frame_id)__$(arg point_matching_intermediate_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/reference_cloud_publish_topic_frame_id" type="str" value="$(arg object_frame_id)__$(arg point_matching_intermediate_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_final_ns)/tf_publisher/publish_tf" type="bool" value="$(arg point_matching_publish_tf)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_final_ns)/tf_publisher/tf_broadcaster_frame_id" type="str" value="$(arg object_frame_id)__$(arg point_matching_intermediate_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_final_ns)/tf_publisher/tf_broadcaster_child_frame_id" type="str" value="$(arg object_frame_id)__$(arg point_matching_tf_broadcaster_frame_id_suffix)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_final_ns)/reference_cloud_publish_topic_frame_id" type="str" value="$(arg object_frame_id)__$(arg point_matching_tf_broadcaster_frame_id_suffix)" />
<rosparam command="load" file="$(arg yaml_configuration_curvature_estimators_filename)" subst_value="true" if="$(eval use_curvature_estimation and bool(yaml_configuration_curvature_estimators_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_initial_pose_estimators_point_matchers_filename)" subst_value="true" if="$(eval bool(yaml_configuration_initial_pose_estimators_point_matchers_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_transformation_aligner_filename)" subst_value="true" if="$(eval use_transformation_aligner and bool(yaml_configuration_transformation_aligner_filename))" />
<param name="transformation_aligner/anchor_tf_frame_id" type="str" value="$(arg sensor_frame_id)" if="$(eval use_transformation_aligner and bool(yaml_configuration_transformation_aligner_filename))" />
<rosparam command="load" file="$(arg yaml_configuration_outlier_detectors_filename)" subst_value="true" if="$(eval bool(yaml_configuration_outlier_detectors_filename) and reference_pointcloud_required)" />
<rosparam command="load" file="$(arg yaml_configuration_transformation_validators_filename)" subst_value="true" if="$(eval bool(yaml_configuration_transformation_validators_filename))" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_intermediate_ns)/convergence_time_limit_seconds" type="double" value="$(arg cloud_matching_timeout_for_intermediate_alignment)" />
<param name="initial_pose_estimators_matchers/point_matchers/$(arg point_matcher_final_ns)/convergence_time_limit_seconds" type="double" value="$(arg cloud_matching_timeout_for_final_alignment)" />
<param name="transformation_validators_initial_alignment/euclidean_transformation_validator/max_outliers_percentage" type="double" value="$(arg max_registration_outliers)" />
<param name="transformation_validators_initial_alignment/euclidean_transformation_validator/max_outliers_percentage_reference_pointcloud" type="double" value="$(arg max_registration_outliers_reference_pointcloud)" />
</group>
<rosparam command="load" file="$(arg yaml_configuration_filters_voxel_grid_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_voxel_grid_filename) and use_voxel_grid)" />
<rosparam command="load" file="$(arg yaml_configuration_filters_random_sample_filename)" subst_value="true" if="$(eval bool(yaml_configuration_filters_random_sample_filename) and use_random_sample)" />
<rosparam command="load" file="$(arg yaml_configuration_tracking_filename)" subst_value="true" if="$(eval bool(yaml_configuration_tracking_filename) and load_tracking_pipeline)" />
<rosparam command="load" file="$(arg yaml_configuration_recovery_filename)" subst_value="true" if="$(eval bool(yaml_configuration_recovery_filename) and load_tracking_pipeline)" />
</group>
</group>
<node if="$(arg load_object_recognition_skill_server_node)" name="$(arg namespace_object_recognition_skill_server)" pkg="object_recognition_skill_server" type="object_recognition_skill_server_node" respawn="$(arg enable_respawn)" output="screen" />
<include file="$(find object_recognition_skill_server)/launch/tf_publisher_configuration.launch">
<arg name="namespace" default="$(arg namespace_tf_publisher)" if="$(arg load_object_recognition_skill_tf_publisher_node)"/>
<arg name="namespace" default="$(arg namespace_object_recognition_skill_server)/$(arg namespace_tf_publisher)" unless="$(arg load_object_recognition_skill_tf_publisher_node)"/>
<arg name="object_frame_id" default="$(arg object_frame_id)" />
<arg name="sensor_frame_id" default="$(arg sensor_frame_id)" />
<arg name="publish_initial_pose" default="$(arg initial_pose_available)" />
<arg name="robot_initial_x" default="$(arg initial_pose_x)" />
<arg name="robot_initial_y" default="$(arg initial_pose_y)" />
<arg name="robot_initial_z" default="$(arg initial_pose_z)" />
<arg name="robot_initial_roll" default="$(arg initial_pose_roll)" />
<arg name="robot_initial_pitch" default="$(arg initial_pose_pitch)" />
<arg name="robot_initial_yaw" default="$(arg initial_pose_yaw)" />
<arg name="publish_pose_tf_rate" default="$(arg publish_pose_tf_rate)" />
</include>
<node if="$(arg load_object_recognition_skill_tf_publisher_node)" name="pose_to_tf_publisher" pkg="pose_to_tf_publisher" type="pose_to_tf_publisher_node" respawn="$(arg enable_respawn)" output="screen" />
</group>
</launch>