-
Notifications
You must be signed in to change notification settings - Fork 0
/
guardian_localization.launch
169 lines (145 loc) · 11.4 KB
/
guardian_localization.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
<?xml version="1.0"?>
<launch>
<!-- ======================================================================================================================== -->
<!-- =============================================================== Maps -->
<arg name="map_file" default="$(find guardian_config)/localization/maps/hipercentro/van_2cm.yaml" />
<arg name="map_file_navigation" default="$(find guardian_config)/localization/maps/hipercentro/van_navigation_4cm.yaml" />
<!-- ======================================================================================================================== -->
<!-- =============================================================== Main configurations -->
<arg name="initial_2d_map_file_slam" default="$(arg map_file)" />
<arg name="initial_2d_map_file_navigation" default="$(arg map_file_navigation)" />
<arg name="robot_initial_x" default="0.0" />
<arg name="robot_initial_y" default="0.0" />
<arg name="robot_initial_yaw" default="0.0" />
<arg name="use_drl" default="true" />
<arg name="use_slam" default="false" />
<arg name="use_original_laser_scans_in_slam" default="true" if="$(arg use_slam)" />
<arg name="use_original_laser_scans_in_slam" default="false" unless="$(arg use_slam)" />
<arg name="update_navigation_map" default="false" />
<arg name="use_back_laser_only" default="false" />
<arg name="use_front_laser_only" default="false" />
<arg name="use_both_lasers" default="true" />
<arg name="laserscans_to_pointclouds_topic" default="/guardian/lasers" />
<arg name="sensor_frame_id" default="base_footprint" if="$(arg use_both_lasers)" />
<arg name="sensor_frame_id" default="hokuyo_back_laser_link" if="$(arg use_back_laser_only)" />
<arg name="sensor_frame_id" default="hokuyo_front_laser_link" if="$(arg use_front_laser_only)" />
<arg name="octomap_sensor_frame_id" default="" if="$(arg use_original_laser_scans_in_slam)" />
<arg name="octomap_sensor_frame_id" default="$(arg sensor_frame_id)" unless="$(arg use_original_laser_scans_in_slam)" />
<arg name="laser_scan_topics" default="/guardian/laser_horizontal_back" if="$(arg use_back_laser_only)" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front" if="$(arg use_front_laser_only)" />
<arg name="laser_scan_topics" default="/guardian/laser_tilt_front+/guardian/laser_horizontal_back" if="$(arg use_both_lasers)" />
<arg name="nodes_respawn" default="false" />
<arg name="nodes_namespace" default="dynamic_robot_localization" />
<!-- ======================================================================================================================== -->
<!-- =============================================================== DRL for localization / mapping -->
<node name="map_server_localization" pkg="map_server" type="map_server" args="$(arg map_file)" respawn="$(arg nodes_respawn)" unless="$(arg use_slam)" >
<param name="frame_id" value="map" />
</node>
<include file="$(find dynamic_robot_localization)/launch/dynamic_robot_localization_system.launch" if="$(arg use_drl)" >
<!-- frame ids -->
<arg name="map_frame_id" default="map" />
<arg name="odom_frame_id" default="odom" />
<arg name="base_link_frame_id" default="base_footprint" />
<arg name="sensor_frame_id" default="$(arg sensor_frame_id)" />
<!-- main configurations -->
<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
<arg name="nodes_namespace" default="$(arg nodes_namespace)" />
<arg name="yaml_configuration_filters_filename" default="$(find dynamic_robot_localization)/yaml/configs/filters/filters_large_map_2d_slam.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_filters_filename" default="$(find guardian_config)/localization/configs/filters_large_map_2d.yaml" unless="$(arg use_slam)" />
<arg name="yaml_configuration_pose_estimation_filename" default="$(find dynamic_robot_localization)/yaml/configs/empty.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_pose_estimation_filename" default="$(find guardian_config)/localization/configs/initial_pose_estimation_large_map_2d.yaml" unless="$(arg use_slam)" />
<arg name="yaml_configuration_tracking_filename" default="$(find dynamic_robot_localization)/yaml/configs/pose_tracking/cluttered_environments_dynamic_large_map_slam_2d.yaml" if="$(arg use_slam)" />
<arg name="yaml_configuration_tracking_filename" default="$(find guardian_config)/localization/configs/tracking_cluttered_environments_dynamic_large_map_2d.yaml" unless="$(arg use_slam)" />
<arg name="yaml_configuration_recovery_filename" default="$(find guardian_config)/localization/configs/tracking_recovery_2d.yaml" />
<!-- reference map data -->
<arg name="reference_pointcloud_available" default="false" if="$(arg use_slam)" />
<arg name="reference_pointcloud_available" default="true" unless="$(arg use_slam)" />
<arg name="reference_pointcloud_type_3d" default="false" />
<arg name="reference_pointcloud_update_mode" default="OutliersIntegration" if="$(arg use_slam)" /> <!-- NoIntegration | FullIntegration | InliersIntegration | OutliersIntegration -->
<arg name="reference_pointcloud_update_mode" default="NoIntegration" unless="$(arg use_slam)" />
<arg name="initial_2d_map_file" default="$(arg initial_2d_map_file_slam)" />
<!-- initial pose setup -->
<arg name="robot_initial_pose_in_base_to_map" default="true" />
<arg name="robot_initial_x" default="$(arg robot_initial_x)" />
<arg name="robot_initial_y" default="$(arg robot_initial_y)" />
<arg name="robot_initial_yaw" default="$(arg robot_initial_yaw)" />
<arg name="publish_initial_pose" default="false" />
<!-- sensor data from point cloud topic -->
<arg name="reference_costmap_topic" default="/map" />
<!-- laser scan assembler -->
<arg name="use_laser_assembler" default="1" />
<arg name="laser_scan_topics" default="$(arg laser_scan_topics)" />
<arg name="number_of_tf_queries_for_spherical_interpolation" default="4" />
<arg name="number_of_scans_to_assemble_per_cloud" default="6" />
<arg name="timeout_for_cloud_assembly" default="1.0" />
<arg name="laser_asssembler_remove_invalid_measurements" default="true" />
<arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="/guardian/odom" />
<arg name="min_number_of_scans_to_assemble_per_cloud" default="6" />
<arg name="max_number_of_scans_to_assemble_per_cloud" default="100" />
<arg name="min_timeout_seconds_for_cloud_assembly" default="0.15" />
<arg name="max_timeout_seconds_for_cloud_assembly" default="2.0" />
<arg name="max_linear_velocity" default="0.005" /> <!-- when the linear velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
<arg name="max_angular_velocity" default="0.04" /> <!-- when the angular velocity of the robot is equal or greater than this value, the number of scans to join and assembly timeout will be set to their minimum values specified above -->
<!-- tf configurations -->
<arg name="publish_pose_tf_rate" default="-1.0" if="$(arg use_slam)" />
<arg name="publish_pose_tf_rate" default="10.0" unless="$(arg use_slam)" />
<arg name="publish_last_pose_tf_timeout_seconds" default="2.0" />
<arg name="invert_tf_transform" default="false" />
<arg name="invert_tf_hierarchy" default="false" />
<!-- map update -->
<arg name="use_octomap_for_dynamic_map_update" default="true" if="$(arg use_slam)" />
<arg name="use_octomap_for_dynamic_map_update" default="false" unless="$(arg use_slam)" />
<arg name="use_octomap_only_to_build_occupancy_grid" default="false" />
<arg name="octomap_pointcloud_topic" default="$(arg laserscans_to_pointclouds_topic)" if="$(arg use_original_laser_scans_in_slam)" />
<arg name="octomap_pointcloud_topic" default="aligned_pointcloud" unless="$(arg use_original_laser_scans_in_slam)" /> <!-- ambient_pointcloud | aligned_pointcloud | aligned_pointcloud_outliers | aligned_pointcloud_inliers -->
<arg name="octomap_file" default="" />
<arg name="octomap_resolution" default="0.02" />
<arg name="octomap_minimum_amount_of_time_between_ros_msg_publishing" default="1.0" />
<arg name="octomap_minimum_number_of_integrations_before_ros_msg_publishing" default="2" />
<arg name="octomap_override_sensor_z" default="true" />
<arg name="octomap_override_sensor_z_value" default="0.0" />
</include>
<include file="$(find guardian_config)/localization/guardian_laser_to_pointcloud.launch" if="$(arg use_original_laser_scans_in_slam)" >
<arg name="laser_topic" default="/guardian/laser_tilt_front" />
<arg name="target_frame" default="hokuyo_front_laser_link" />
<arg name="laserscan_to_pointcloud_topic" default="$(arg laserscans_to_pointclouds_topic)" />
<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
<arg name="nodes_namespace" default="$(arg nodes_namespace)" />
</include>
<include file="$(find guardian_config)/localization/guardian_laser_to_pointcloud.launch" if="$(arg use_original_laser_scans_in_slam)" >
<arg name="laser_topic" default="/guardian/laser_horizontal_back" />
<arg name="target_frame" default="hokuyo_back_laser_link" />
<arg name="laserscan_to_pointcloud_topic" default="$(arg laserscans_to_pointclouds_topic)" />
<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
<arg name="nodes_namespace" default="$(arg nodes_namespace)" />
</include>
<!-- ======================================================================================================================== -->
<!-- =============================================================== Navigation map -->
<group if="$(arg update_navigation_map)" ns="$(arg nodes_namespace)" >
<node name="map_server_navigation_initial" pkg="map_server" type="map_server" args="$(arg initial_2d_map_file_navigation)" respawn="$(arg nodes_respawn)" clear_params="true" output="screen" >
<param name="frame_id" type="str" value="map" />
<remap from="map" to="initial_2d_map_navigation" />
</node>
<include file="$(find dynamic_robot_localization)/launch/octo_map.launch" >
<arg name="occupancy_grid_in" default="initial_2d_map_navigation" />
<arg name="cloud_in" default="$(arg laserscans_to_pointclouds_topic)" if="$(arg use_original_laser_scans_in_slam)" />
<arg name="cloud_in" default="aligned_pointcloud" unless="$(arg use_original_laser_scans_in_slam)" /> <!-- ambient_pointcloud | aligned_pointcloud | aligned_pointcloud_outliers | aligned_pointcloud_inliers -->
<arg name="sensor_frame_id" default="$(arg octomap_sensor_frame_id)" />
<arg name="override_sensor_z" default="true" />
<arg name="override_sensor_z_value" default="0.0" />
<arg name="resolution" default="0.02" />
<arg name="minimum_amount_of_time_between_ros_msg_publishing" default="20" />
<arg name="minimum_number_of_integrations_before_ros_msg_publishing" default="20" />
<arg name="cloud_out_topic" default="/map_navigation_pointcloud" />
<arg name="map_out_topic" default="/map_navigation" />
<arg name="nodes_respawn" default="$(arg nodes_respawn)" />
</include>
</group>
<group unless="$(arg update_navigation_map)" >
<node name="relay_map_to_navigation" pkg="topic_tools" type="relay" args="/map /map_navigation" output="screen" respawn="$(arg nodes_respawn)" if="$(arg use_slam)" />
<node name="map_server_navigation" pkg="map_server" type="map_server" args="$(arg map_file_navigation)" respawn="$(arg nodes_respawn)" unless="$(arg use_slam)" >
<param name="frame_id" value="map" />
<remap from="/map" to="/map_navigation" />
</node>
</group>
</launch>