column_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import yaml
00005 import tf
00006 import copy
00007 
00008 from geometry_msgs.msg import *
00009 from rospy_message_converter import message_converter
00010 from visualization_msgs.msg import *
00011 from yocs_msgs.msg import *
00012 
00013 def publish(filename):
00014     yaml_data = None 
00015     with open(filename) as f:
00016        yaml_data = yaml.load(f)
00017        
00018     column_list = ColumnList()
00019     # Markers
00020     marker_list = MarkerArray()    
00021 
00022     marker_id = 1
00023     for t in yaml_data:
00024         object = Column()
00025         object.name = t['name']
00026         object.radius = float(t['radius'])
00027         object.height = float(t['height'])
00028         object.pose.header.frame_id = t['frame_id']
00029         object.pose.header.stamp = rospy.Time.now()
00030         object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
00031         column_list.obstacles.append(object)
00032         
00033         marker = Marker()
00034         marker.id = marker_id
00035         marker.header = object.pose.header
00036         marker.type = Marker.CYLINDER
00037         marker.ns = "column_obstacles"
00038         marker.action = Marker.ADD
00039         marker.lifetime = rospy.Duration.from_sec(0)
00040         marker.pose = copy.deepcopy(object.pose.pose.pose)
00041         marker.pose.position.z += object.height/2.0
00042         marker.scale.x = object.radius * 2
00043         marker.scale.y = object.radius * 2  
00044         marker.scale.z = object.height
00045         marker.color.r = 0
00046         marker.color.g = 0
00047         marker.color.b = 1.0
00048         marker.color.a = 0.5
00049                                                                                                                                                     
00050         marker_list.markers.append(marker)
00051                                                                                                                                                     
00052         marker_id = marker_id + 1
00053 
00054     column_pub.publish(column_list)
00055     marker_pub.publish(marker_list)
00056     
00057     return
00058 
00059 if __name__ == '__main__':
00060     rospy.init_node('column_loader')
00061     filename = rospy.get_param('~filename')
00062     
00063     marker_pub = rospy.Publisher('column_marker',    MarkerArray, latch = True)
00064     column_pub = rospy.Publisher('column_pose_list', ColumnList,  latch = True)
00065     
00066     rospy.loginfo('Publishing obstacles and visualization markers.')
00067     publish(filename)
00068     rospy.spin()


yocs_virtual_sensor
Author(s): Jorge Santos
autogenerated on Fri Aug 28 2015 13:45:16