jsk_rviz_plugins: link_marker_publisher.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Publish a visualization_marker for specified link
00005 """
00006 
00007 import rospy
00008 from visualization_msgs.msg import Marker
00009 from xml.dom.minidom import parse, parseString
00010 
00011 if __name__ == "__main__":
00012     rospy.init_node("link_marker_publisher")
00013     link_name = rospy.get_param("~link")
00014     rgb = rospy.get_param("~rgb", [1, 0, 0])
00015     alpha = rospy.get_param("~alpha", 1.0)
00016     scale = rospy.get_param("~scale", 1.02)
00017     robot_description = rospy.get_param("/robot_description")
00018     # Parse robot_description using minidom directly
00019     # because urdf_parser_py cannot read PR2 urdf
00020     doc = parseString(robot_description)
00021     links = doc.getElementsByTagName('link')
00022     mesh_file = None
00023     for link in links:
00024         if link_name == link.getAttribute('name'):
00025             visual_mesh = link.getElementsByTagName('visual').item(0).getElementsByTagName('mesh').item(0)
00026             mesh_file = visual_mesh.getAttribute('filename')
00027             break
00028     if not mesh_file:
00029         raise Exception("Cannot find link: {0}".format(link_name))
00030     pub = rospy.Publisher('~marker', Marker)
00031     rate = rospy.Rate(1)
00032     while not rospy.is_shutdown():
00033         marker = Marker()
00034         marker.header.frame_id = link_name
00035         marker.header.stamp = rospy.Time.now()
00036         marker.type = Marker.MESH_RESOURCE
00037         marker.color.a = alpha
00038         marker.color.r = rgb[0]
00039         marker.color.g = rgb[1]
00040         marker.color.b = rgb[2]
00041         marker.scale.x = scale
00042         marker.scale.y = scale
00043         marker.scale.z = scale
00044         marker.mesh_resource = mesh_file
00045         marker.frame_locked = True
00046         pub.publish(marker)
00047         rate.sleep()