def _save_map_request(self, req): if req.directory: map_path = req.directory + "/" else: map_path = os.path.expanduser( '~') + '/.pal/' + self._robot + '_maps/' map_name = 'config' shutil.rmtree(map_path + map_name, ignore_errors=True) os.makedirs(map_path + map_name) rospy.logdebug("Saving map with map_name '" + map_name + "' and map_path '" + map_path + "'") # Save map (image and meta-data). filename = 'submap_0' pal_launch.execute_command('rosrun', 'map_server', 'map_saver', '-f', filename) # Update /mmap/numberOfSubMaps parameter # [from saverFunctions::configSaver] rospy.set_param('/mmap/numberOfSubMaps', 1) map_data_file = os.path.join(map_path + map_name + '/', 'mmap.yaml') if rospy.has_param('/mmap'): rosparam.dump_params(map_data_file, '/mmap') else: try: os.remove(map_data_file) except OSError: pass map_data_file = None # Move map to maps folder # (this is required to have the image relative path in map.yaml): shutil.move(filename + '.pgm', map_path + map_name) shutil.move(filename + '.yaml', map_path + map_name + '/map.yaml') # Create nice map shutil.copyfile(os.path.join(map_path + map_name, 'submap_0.pgm'), os.path.join(map_path + map_name, 'map.pgm')) with open(os.path.join(map_path + map_name, 'transformation.xml'), 'w') as transf: transf.write(DEFAULT_TRANSFORMATION) # Set permisions self._chmod(map_path, 0777) # Save initial pose of the robot in the map rospack = rospkg.RosPack() shutil.copyfile( rospack.get_path('pal_navigation_sm') + '/config/pose.yaml', map_path + '../pose.yaml') return SaveMapResponse(True, map_name, map_path, 'Map saved: %s' % map_name)
def _save_map_request(self, req): if req.directory: map_path = req.directory + "/" else: map_path = os.path.expanduser('~') + '/.pal/' + self._robot + '_maps/' map_name = 'config' shutil.rmtree(map_path+map_name, ignore_errors=True) os.makedirs(map_path+map_name) rospy.logdebug("Saving map with map_name '" + map_name + "' and map_path '" + map_path + "'") # Save map (image and meta-data). filename = 'submap_0' pal_launch.execute_command('rosrun', 'map_server', 'map_saver', '-f', filename) # Update /mmap/numberOfSubMaps parameter # [from saverFunctions::configSaver] rospy.set_param('/mmap/numberOfSubMaps', 1) map_data_file = os.path.join(map_path+map_name+'/', 'mmap.yaml') if rospy.has_param('/mmap'): rosparam.dump_params(map_data_file, '/mmap') else: try: os.remove(map_data_file) except OSError: pass map_data_file = None # Move map to maps folder # (this is required to have the image relative path in map.yaml): shutil.move(filename + '.pgm', map_path+map_name) shutil.move(filename + '.yaml',map_path+map_name+'/map.yaml') # Create nice map shutil.copyfile(os.path.join(map_path+map_name, 'submap_0.pgm'), os.path.join(map_path+map_name, 'map.pgm')) with open(os.path.join(map_path+map_name, 'transformation.xml'), 'w') as transf: transf.write(DEFAULT_TRANSFORMATION) # Set permisions self._chmod(map_path, 0777) # Save initial pose of the robot in the map rospack = rospkg.RosPack() shutil.copyfile(rospack.get_path('pal_navigation_sm') + '/config/pose.yaml', map_path+'../pose.yaml') return SaveMapResponse(True, map_name, map_path, 'Map saved: %s' % map_name)