save_tables.py
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):
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         for i in range(0, random.randint(0,11)):
00035             ann.keywords.append('kw'+str(random.randint(1,11)))
00036         if 'prev_id' in vars():
00037             ann.relationships.append(prev_id)
00038         prev_id = ann.id
00039         ann.shape = 3  #CYLINDER
00040         ann.color.r = 0.2
00041         ann.color.g = 0.2
00042         ann.color.b = 0.8
00043         ann.color.a = 0.5
00044         ann.size.x = float(t['radius'])*2
00045         ann.size.y = float(t['radius'])*2
00046         ann.size.z = float(t['height'])
00047         ann.pose.header.frame_id = t['frame_id']
00048         ann.pose.header.stamp = rospy.Time.now()
00049         ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
00050         
00051         # tables are assumed to lay on the floor, so z coordinate is zero;
00052         # but WCF assumes that the annotation pose is the center of the object
00053         ann.pose.pose.pose.position.z += ann.size.z/2.0
00054 
00055         anns_list.append(ann)
00056 
00057         object = Table()
00058         object.name = t['name']
00059         object.radius = float(t['radius'])
00060         object.height = float(t['height'])
00061         object.pose.header.frame_id = t['frame_id']
00062         object.pose.header.stamp = rospy.Time.now()
00063         object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
00064         data = AnnotationData()
00065         data.id = ann.data_id
00066         data.type = ann.type
00067         data.data = serialize_msg(object)
00068         
00069         data_list.append(data)
00070 
00071     return anns_list, data_list
00072 
00073 if __name__ == '__main__':
00074     rospy.init_node('tables_saver')
00075     world = rospy.get_param('~world')
00076     file  = rospy.get_param('~file')
00077     anns, data = read(file)
00078 
00079     rospy.loginfo("Waiting for save_annotations_data service...")
00080     rospy.wait_for_service('save_annotations_data')
00081     save_srv = rospy.ServiceProxy('save_annotations_data', world_canvas_msgs.srv.SaveAnnotationsData)
00082 
00083     rospy.loginfo("Saving virtual tables from file '%s' for world '%s'" % (file, world))
00084     save_srv(anns, data)
00085     rospy.loginfo("Done")


world_canvas_server
Author(s): Jorge Santos Simón
autogenerated on Thu Jun 6 2019 21:25:07