contact_state_marker.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 """
4 Colorize link according to hrpsys_ros_bridge/ContactState using
5 visualization_msgs/Marker
6 """
7 
8 import rospy
9 from hrpsys_ros_bridge.msg import ContactState, ContactStateStamped, ContactStatesStamped
10 from visualization_msgs.msg import Marker, MarkerArray
11 from xml.dom.minidom import parse, parseString
12 from geometry_msgs.msg import Pose
13 from tf.transformations import quaternion_from_euler
14 from jsk_rviz_plugins.cfg import ContactStateMarkerConfig
15 from dynamic_reconfigure.server import Server
16 from urdf_parser_py.urdf import URDF
17 
18 g_config = None
19 def config_callback(config, level):
20  global g_config
21  g_config = config
22  return config
23 
24 def find_mesh(link_name):
25  "find mesh file from specified link_name"
26  global g_config
27  for link in g_links:
28  if link_name == link.getAttribute('name'):
29  visual = link.getElementsByTagName('visual').item(0)
30  visual_mesh = visual.getElementsByTagName('mesh').item(0)
31  if len(visual.getElementsByTagName("origin")) > 0:
32  origin = visual.getElementsByTagName("origin").item(0)
33  pose = Pose()
34  if origin.getAttribute("xyz"):
35  pose.position.x, pose.position.y, pose.position.z = [float(v) for v in origin.getAttribute("xyz").split()]
36  if origin.getAttribute("rpy"):
37  r, p, y = [float(v) for v in origin.getAttribute("rpy").split()]
38  q = quaternion_from_euler(r, p, y)
39  pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = q
40  return (visual_mesh.getAttribute('filename'), pose)
41  else:
42  pose = Pose()
43  pose.orientation.w = 1
44  return (visual_mesh.getAttribute('filename'), pose)
45  raise Exception("Cannot find link: {0}".format(link_name))
46 
47 def callback(msgs):
48  "msgs = ContactStatesStamped"
49  global g_config
50  if g_config.use_parent_link:
51  urdf_robot = URDF.from_parameter_server()
52  marker_array = MarkerArray()
53  for msg, i in zip(msgs.states, range(len(msgs.states))):
54  marker = Marker()
55  link_name = msg.header.frame_id
56  if g_config.use_parent_link:
57  # lookup parent link
58  chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name)
59  link_name = chain[-3]
60  mesh_file, offset = find_mesh(link_name)
61  marker.header.frame_id = link_name
62  marker.header.stamp = rospy.Time.now()
63  marker.type = Marker.MESH_RESOURCE
64  if msg.state.state == ContactState.ON:
65  marker.color.a = g_config.on_alpha
66  marker.color.r = g_config.on_red
67  marker.color.g = g_config.on_green
68  marker.color.b = g_config.on_blue
69  elif msg.state.state == ContactState.OFF:
70  marker.color.a = g_config.off_alpha
71  marker.color.r = g_config.off_red
72  marker.color.g = g_config.off_green
73  marker.color.b = g_config.off_blue
74  marker.scale.x = g_config.marker_scale
75  marker.scale.y = g_config.marker_scale
76  marker.scale.z = g_config.marker_scale
77  marker.pose = offset
78  marker.mesh_resource = mesh_file
79  marker.frame_locked = True
80  marker.id = i
81  if msg.state.state == ContactState.OFF:
82  if not g_config.visualize_off:
83  marker.action = Marker.DELETE
84  marker_array.markers.append(marker)
85  pub.publish(marker_array)
86 
87 if __name__ == "__main__":
88  rospy.init_node("contact_state_marker")
89  rgb = rospy.get_param("~rgb", [1, 0, 0])
90  alpha = rospy.get_param("~alpha", 0.5)
91  scale = rospy.get_param("~scale", 1.02)
92  robot_description = rospy.get_param("/robot_description")
93 
94  # Parse robot_description using minidom directly
95  # because urdf_parser_py cannot read PR2 urdf
96  doc = parseString(robot_description)
97  g_links = doc.getElementsByTagName('link')
98  srv = Server(ContactStateMarkerConfig, config_callback)
99  pub = rospy.Publisher('~marker', MarkerArray)
100  sub = rospy.Subscriber("~input", ContactStatesStamped, callback)
101  rospy.spin()
def config_callback(config, level)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18