wall_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     wall_list = WallList()
00019     # Markers
00020     marker_list = MarkerArray()    
00021 
00022     marker_id = 1
00023     for t in yaml_data:
00024         object = Wall()
00025         object.name = t['name']
00026         object.length = float(t['length'])
00027         object.width  = float(t['width'])
00028         object.height = float(t['height'])
00029         object.pose.header.frame_id = t['frame_id']
00030         object.pose.header.stamp = rospy.Time.now()
00031         object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
00032         wall_list.obstacles.append(object)
00033 
00034         marker = Marker()
00035         marker.id = marker_id
00036         marker.header = object.pose.header
00037         marker.type = Marker.CUBE
00038         marker.ns = "wall_obstacles"
00039         marker.action = Marker.ADD
00040         marker.lifetime = rospy.Duration.from_sec(0)
00041         marker.pose = copy.deepcopy(object.pose.pose.pose)
00042         marker.pose.position.z += object.height/2.0
00043         marker.scale.x = object.width
00044         marker.scale.y = object.length
00045         marker.scale.z = object.height
00046         marker.color.r = 0.2
00047         marker.color.g = 0.4
00048         marker.color.b = 0.4
00049         marker.color.a = 0.5
00050                                                                                                                                                     
00051         marker_list.markers.append(marker)
00052                                                                                                                                                     
00053         marker_id = marker_id + 1
00054 
00055     wall_pub.publish(wall_list)
00056     marker_pub.publish(marker_list)
00057     
00058     return
00059 
00060 if __name__ == '__main__':
00061     rospy.init_node('wall_loader')
00062     filename = rospy.get_param('~filename')
00063     
00064     marker_pub = rospy.Publisher('wall_marker',    MarkerArray, latch = True)
00065     wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch = True)
00066 
00067     rospy.loginfo('Publishing obstacles and visualization markers.')
00068     publish(filename)
00069     rospy.spin()


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