Go to the documentation of this file.00001 from __future__ import print_function
00002
00003 import copy
00004
00005 import rospy
00006 from visualization_msgs.msg import Marker
00007
00008 import pysdf
00009
00010
00011
00012 protoMarkerMsg = Marker()
00013 protoMarkerMsg.frame_locked = True
00014 protoMarkerMsg.id = 0
00015 protoMarkerMsg.action = Marker.ADD
00016 protoMarkerMsg.mesh_use_embedded_materials = True
00017 protoMarkerMsg.color.a = 0.0
00018 protoMarkerMsg.color.r = 0.0
00019 protoMarkerMsg.color.g = 0.0
00020 protoMarkerMsg.color.b = 0.0
00021 supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
00022
00023
00024 def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)):
00025 marker_msg = None
00026 linkpart = None
00027 if use_collision:
00028 linkpart = getattr(link, 'collision')
00029 else:
00030 linkpart = getattr(link, 'visual')
00031
00032 if not linkpart.geometry_type in supported_geometry_types:
00033 return
00034
00035 marker_msg = copy.deepcopy(protoMarkerMsg)
00036 marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
00037 marker_msg.header.stamp = rospy.get_rostime()
00038 marker_msg.lifetime = lifetime
00039 marker_msg.ns = pysdf.sdf2tfname(full_linkname)
00040 marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
00041
00042 if linkpart.geometry_type == 'mesh':
00043 marker_msg.type = Marker.MESH_RESOURCE
00044 marker_msg.mesh_resource = linkpart.geometry_data['uri'].replace('model://', 'file://' + pysdf.models_path)
00045 scale = (float(val) for val in linkpart.geometry_data['scale'].split())
00046 marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
00047 else:
00048 marker_msg.color.a = 1
00049 marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
00050
00051 if linkpart.geometry_type == 'box':
00052 marker_msg.type = Marker.CUBE
00053 scale = (float(val) for val in linkpart.geometry_data['size'].split())
00054 marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
00055 elif linkpart.geometry_type == 'sphere':
00056 marker_msg.type = Marker.SPHERE
00057 marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius'])
00058 elif linkpart.geometry_type == 'cylinder':
00059 marker_msg.type = Marker.CYLINDER
00060 marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius'])
00061 marker_msg.scale.z = float(linkpart.geometry_data['length'])
00062
00063
00064 return marker_msg