link_marker_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 Publish a visualization_marker for specified link
5 """
6 
7 import rospy
8 from visualization_msgs.msg import Marker
9 from xml.dom.minidom import parse, parseString
10 
11 if __name__ == "__main__":
12  rospy.init_node("link_marker_publisher")
13  link_name = rospy.get_param("~link")
14  rgb = rospy.get_param("~rgb", [1, 0, 0])
15  alpha = rospy.get_param("~alpha", 1.0)
16  scale = rospy.get_param("~scale", 1.02)
17  robot_description = rospy.get_param("/robot_description")
18  # Parse robot_description using minidom directly
19  # because urdf_parser_py cannot read PR2 urdf
20  doc = parseString(robot_description)
21  links = doc.getElementsByTagName('link')
22  mesh_file = None
23  for link in links:
24  if link_name == link.getAttribute('name'):
25  visual_mesh = link.getElementsByTagName('visual').item(0).getElementsByTagName('mesh').item(0)
26  mesh_file = visual_mesh.getAttribute('filename')
27  break
28  if not mesh_file:
29  raise Exception("Cannot find link: {0}".format(link_name))
30  pub = rospy.Publisher('~marker', Marker)
31  rate = rospy.Rate(1)
32  while not rospy.is_shutdown():
33  marker = Marker()
34  marker.header.frame_id = link_name
35  marker.header.stamp = rospy.Time.now()
36  marker.type = Marker.MESH_RESOURCE
37  marker.color.a = alpha
38  marker.color.r = rgb[0]
39  marker.color.g = rgb[1]
40  marker.color.b = rgb[2]
41  marker.scale.x = scale
42  marker.scale.y = scale
43  marker.scale.z = scale
44  marker.mesh_resource = mesh_file
45  marker.frame_locked = True
46  pub.publish(marker)
47  rate.sleep()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58