world_canvas_server: save_map.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import genpy
00004 import rospy
00005 import yaml
00006 import uuid
00007 import unique_id
00008 import world_canvas_msgs.msg
00009 import world_canvas_msgs.srv
00010 
00011 from nav_msgs.msg import *
00012 from rospy_message_converter import message_converter
00013 from world_canvas_msgs.msg import Annotation, AnnotationData
00014 from world_canvas_utils.serialization import *
00015 
00016 
00017 def read(file):
00018     '''
00019     Parse a yaml file containing a single map message
00020     @param file Target file path
00021     '''
00022     yaml_data = None 
00023     with open(file) as f:
00024        yaml_data = yaml.load(f)
00025       
00026     ann = Annotation()
00027     ann.timestamp = rospy.Time.now()
00028     ann.data_id = unique_id.toMsg(unique_id.fromRandom())
00029     ann.id = unique_id.toMsg(unique_id.fromRandom())
00030     ann.world = world
00031     ann.name = map_name
00032     ann.type = 'nav_msgs/OccupancyGrid'
00033     ann.keywords.append(map_name)
00034     ann.shape = 1 # CUBE
00035     ann.color.r = 0.2
00036     ann.color.g = 0.2
00037     ann.color.b = 0.2
00038     ann.color.a = 0.01
00039     ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution']
00040     ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution']
00041     ann.size.z = 0.000001
00042     ann.pose.header.frame_id = '/map'
00043     ann.pose.header.stamp = rospy.Time.now()
00044     ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',
00045                                                                               yaml_data['info']['origin'])
00046 
00047     object = OccupancyGrid()
00048     genpy.message.fill_message_args(object, yaml_data)
00049     map = AnnotationData()
00050     map.id = ann.data_id
00051     map.type = ann.type
00052     map.data = serialize_msg(object)
00053     
00054     return [ann], [map]  # return as lists, as is what expects save_annotations_data service
00055 
00056 if __name__ == '__main__':
00057     rospy.init_node('map_saver')
00058     file     = rospy.get_param('~file')
00059     world    = rospy.get_param('~world')
00060     map_name = rospy.get_param('~map_name')
00061     ann, map = read(file)
00062 
00063     rospy.loginfo("Waiting for save_annotations_data service...")
00064     rospy.wait_for_service('save_annotations_data')
00065     save_srv = rospy.ServiceProxy('save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
00066 
00067     rospy.loginfo("Saving map from file '%s' for world '%s'" % (file, world))
00068     save_srv(ann, map)
00069     rospy.loginfo("Done")