def _initialize_global_localization(self, namespace):
ros_utils.wait_for_rosout_message(
node_name='%s/amcl' % namespace,
desired_message='Done initializing likelihood field model.')
ros_utils.launch(package='localization',
launch_file='global_localization_init.launch',
argv={'ns': namespace})
def _launch_amcl(self, namespace):
ros_utils.launch(package='localization',
launch_file='amcl.launch',
argv={
'ns': namespace,
'min_particles': self.params['min_amcl_particles']
})
ros_utils.wait_for_rosout_message(
node_name='%s/amcl' % namespace,
desired_message='Done initializing likelihood field model.')
def _launch_map_server(self, map_yaml_path, namespace):
ros_utils.launch(package='localization',
launch_file='map.launch',
argv={
'ns': namespace,
'map_yaml_path': map_yaml_path
})
ros_utils.wait_for_rosout_message(
node_name='%s/map_server' % namespace,
desired_message=r'Read a \d+ X \d+ map @ \d\.?\d+? m/cell',
is_regex=True)
argv={
'frame_id': 'base_link',
'child_frame_id': 'contours_scan_link'
})
# Launch map server
ros_utils.launch(package='localization',
launch_file='map.launch',
argv={
'map_yaml_path':
r'/home/omer/Downloads/panorama_cropped_map.yaml'
})
# Wait for map server to load
ros_utils.wait_for_rosout_message(
node_name='map_server',
desired_message=r'Read a \d+ X \d+ map @ \d\.?\d+? m/cell',
is_regex=True)
# Launch AMCL
ros_utils.launch(package='localization', launch_file='amcl.launch')
# Wait for AMCL to load
ros_utils.wait_for_rosout_message(
node_name='amcl',
desired_message='Done initializing likelihood field model.')
# Launch ICP
ros_utils.launch(package='localization', launch_file='scan_matcher.launch')
# Start recording output bag
ros_utils.start_recording_bag(
