world_canvas_server: save_tables

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import yaml
00005 import random
00006 import uuid
00007 import unique_id
00008 import world_canvas_msgs.msg
00009 import world_canvas_msgs.srv
00010 
00011 from geometry_msgs.msg import *
00012 from rospy_message_converter import message_converter
00013 from yocs_msgs.msg import Table, TableList
00014 from world_canvas_msgs.msg import Annotation, AnnotationData
00015 from world_canvas_utils.serialization import *
00016 
00017 
00018 def read(file, world):
00019     yaml_data = None 
00020     with open(file) as f:
00021        yaml_data = yaml.load(f)
00022 
00023     anns_list = []
00024     data_list = []
00025 
00026     for t in yaml_data:
00027         ann = Annotation()
00028         ann.timestamp = rospy.Time.now()
00029         ann.data_id = unique_id.toMsg(unique_id.fromRandom())
00030         ann.id = unique_id.toMsg(unique_id.fromRandom())
00031         ann.world = world
00032         ann.name = t['name']
00033         ann.type = 'yocs_msgs/Table'
00034         ann.keywords.append(str(world))
00035         if 'prev_id' in vars():
00036             ann.relationships.append(prev_id)
00037         prev_id = ann.id
00038         ann.shape = 3  #CYLINDER
00039         ann.color.r = 0.2
00040         ann.color.g = 0.2
00041         ann.color.b = 0.8
00042         ann.color.a = 0.5
00043         ann.size.x = float(t['radius'])*2
00044         ann.size.y = float(t['radius'])*2
00045         ann.size.z = float(t['height'])
00046         ann.pose.header.frame_id = t['pose']['header']['frame_id']
00047         ann.pose.header.stamp = rospy.Time.now()
00048         ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']['pose']['pose'])
00049         
00050         # tables are assumed to lay on the floor, so z coordinate is zero;
00051         # but WCF assumes that the annotation pose is the center of the object
00052         ann.pose.pose.pose.position.z += ann.size.z/2.0
00053 
00054         anns_list.append(ann)
00055 
00056         object = Table()
00057         object.name = t['name']
00058         object.radius = float(t['radius'])
00059         object.height = float(t['height'])
00060         object.pose.header.frame_id = t['pose']['header']['frame_id']
00061         object.pose.header.stamp = rospy.Time.now()
00062         object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose']['pose']['pose'])
00063         data = AnnotationData()
00064         data.id = ann.data_id
00065         data.type = ann.type
00066         data.data = serialize_msg(object)
00067         
00068         data_list.append(data)
00069 
00070     return anns_list, data_list
00071 
00072 if __name__ == '__main__':
00073     rospy.init_node('tables_saver')
00074     world = rospy.get_param('~world')
00075     file  = rospy.get_param('~file')
00076     anns, data = read(file, world)
00077 
00078     rospy.loginfo("Waiting for save_annotations_data service...")
00079     rospy.wait_for_service('save_annotations_data')
00080     save_srv = rospy.ServiceProxy('save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
00081 
00082     rospy.loginfo("Saving virtual tables from file '%s' for world '%s'" % (file, world))
00083     save_srv(anns, data)
00084     rospy.loginfo("Done")