world_canvas_server: get_columns.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import yaml
00005 import uuid
00006 import copy
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 Column, ColumnList
00014 from visualization_msgs.msg import Marker, MarkerArray
00015 from world_canvas_utils.serialization import *
00016 
00017 
00018 def publish(anns, data):
00019     column_list = ColumnList()
00020     marker_list = MarkerArray()    
00021 
00022     marker_id = 1
00023     for a, d in zip(anns, data):
00024         
00025         # Columns
00026         object = deserialize_msg(d.data, Column)
00027         column_list.obstacles.append(object)
00028         
00029         # Markers
00030         marker = Marker()
00031         marker.id = marker_id
00032         marker.header = a.pose.header
00033         marker.type = a.shape
00034         marker.ns = "column_obstacles"
00035         marker.action = Marker.ADD
00036         marker.lifetime = rospy.Duration.from_sec(0)
00037         marker.pose = copy.deepcopy(a.pose.pose.pose)
00038         marker.scale = a.size
00039         marker.color = a.color
00040 
00041         marker_list.markers.append(marker)
00042 
00043         marker_id = marker_id + 1
00044 
00045     marker_pub = rospy.Publisher('column_marker',    MarkerArray, latch=True, queue_size=1)
00046     column_pub = rospy.Publisher('column_pose_list', ColumnList,  latch=True, queue_size=1)
00047 
00048     column_pub.publish(column_list)
00049     marker_pub.publish(marker_list)
00050     
00051     return
00052 
00053 
00054 if __name__ == '__main__':
00055     rospy.init_node('columns_loader')
00056     world    = rospy.get_param('~world')
00057     uuids    = rospy.get_param('~uuids', [])
00058     names    = rospy.get_param('~names', [])
00059     keywords = rospy.get_param('~keywords', [])
00060     related  = rospy.get_param('~relationships', [])
00061 
00062     rospy.loginfo("Waiting for get_annotations service...")
00063     rospy.wait_for_service('get_annotations')
00064 
00065     rospy.loginfo("Loading annotations for world '%s'", world)
00066     get_anns_srv = rospy.ServiceProxy('get_annotations', world_canvas_msgs.srv.GetAnnotations)
00067     respAnns = get_anns_srv(world,
00068                            [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in uuids],
00069                             names, ['yocs_msgs/Column'], keywords,
00070                            [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in related])
00071 
00072     if len(respAnns.annotations) > 0:
00073         rospy.loginfo("Publishing visualization markers for %d retrieved annotations...",
00074                        len(respAnns.annotations))
00075     else:
00076         rospy.loginfo("No annotations found for world '%s' with the given search criteria", world)
00077         sys.exit()
00078 
00079     rospy.loginfo("Loading data for the %d retrieved annotations", len(respAnns.annotations))
00080     get_data_srv = rospy.ServiceProxy('get_annotations_data', world_canvas_msgs.srv.GetAnnotationsData)
00081     respData = get_data_srv([a.data_id for a in respAnns.annotations])
00082 
00083     if len(respData.data) > 0:
00084         rospy.loginfo("Publishing data for %d retrieved annotations...", len(respData.data))
00085         publish(respAnns.annotations, respData.data)
00086     else:
00087         rospy.logwarn("No data found for the %d retrieved annotations", len(respAnns.annotations))
00088         
00089     rospy.loginfo("Done")
00090     rospy.spin()