jsk_rviz_plugins: contact_state_marker.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Colorize link according to hrpsys_ros_bridge/ContactState using
00005 visualization_msgs/Marker
00006 """
00007 
00008 import rospy
00009 from hrpsys_ros_bridge.msg import ContactState, ContactStateStamped, ContactStatesStamped
00010 from visualization_msgs.msg import Marker, MarkerArray
00011 from xml.dom.minidom import parse, parseString
00012 from geometry_msgs.msg import Pose
00013 from tf.transformations import quaternion_from_euler
00014 from jsk_rviz_plugins.cfg import ContactStateMarkerConfig
00015 from dynamic_reconfigure.server import Server
00016 from urdf_parser_py.urdf import URDF
00017 
00018 g_config = None
00019 def config_callback(config, level):
00020     global g_config
00021     g_config = config
00022     return config
00023 
00024 def find_mesh(link_name):
00025     "find mesh file from specified link_name"
00026     global g_config
00027     for link in g_links:
00028         if link_name == link.getAttribute('name'):
00029             visual = link.getElementsByTagName('visual').item(0)
00030             visual_mesh = visual.getElementsByTagName('mesh').item(0)
00031             if len(visual.getElementsByTagName("origin")) > 0:
00032                 origin = visual.getElementsByTagName("origin").item(0)
00033                 pose = Pose()
00034                 if origin.getAttribute("xyz"):
00035                     pose.position.x, pose.position.y, pose.position.z = [float(v) for v in origin.getAttribute("xyz").split()]
00036                 if origin.getAttribute("rpy"):
00037                     r, p, y = [float(v) for v in origin.getAttribute("rpy").split()]
00038                     q = quaternion_from_euler(r, p, y)
00039                     pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = q
00040                     return (visual_mesh.getAttribute('filename'), pose)
00041             else:
00042                 pose = Pose()
00043                 pose.orientation.w = 1
00044                 return (visual_mesh.getAttribute('filename'), pose)
00045     raise Exception("Cannot find link: {0}".format(link_name))
00046 
00047 def callback(msgs):
00048     "msgs = ContactStatesStamped"
00049     global g_config
00050     if g_config.use_parent_link:
00051         urdf_robot = URDF.from_parameter_server()
00052     marker_array = MarkerArray()
00053     for msg, i in zip(msgs.states, range(len(msgs.states))):
00054         marker = Marker()
00055         link_name = msg.header.frame_id
00056         if g_config.use_parent_link:
00057             # lookup parent link
00058             chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name)
00059             link_name = chain[-3]
00060         mesh_file, offset = find_mesh(link_name)
00061         marker.header.frame_id = link_name
00062         marker.header.stamp = rospy.Time.now()
00063         marker.type = Marker.MESH_RESOURCE
00064         if msg.state.state == ContactState.ON:
00065             marker.color.a = g_config.on_alpha
00066             marker.color.r = g_config.on_red
00067             marker.color.g = g_config.on_green
00068             marker.color.b = g_config.on_blue
00069         elif msg.state.state == ContactState.OFF:
00070             marker.color.a = g_config.off_alpha
00071             marker.color.r = g_config.off_red
00072             marker.color.g = g_config.off_green
00073             marker.color.b = g_config.off_blue
00074         marker.scale.x = g_config.marker_scale
00075         marker.scale.y = g_config.marker_scale
00076         marker.scale.z = g_config.marker_scale
00077         marker.pose = offset
00078         marker.mesh_resource = mesh_file
00079         marker.frame_locked = True
00080         marker.id = i
00081         if msg.state.state == ContactState.OFF:
00082             if not g_config.visualize_off:
00083                 marker.action = Marker.DELETE
00084         marker_array.markers.append(marker)
00085     pub.publish(marker_array)
00086 
00087 if __name__ == "__main__":
00088     rospy.init_node("contact_state_marker")
00089     rgb = rospy.get_param("~rgb", [1, 0, 0])
00090     alpha = rospy.get_param("~alpha", 0.5)
00091     scale = rospy.get_param("~scale", 1.02)
00092     robot_description = rospy.get_param("/robot_description")
00093     
00094     # Parse robot_description using minidom directly
00095     # because urdf_parser_py cannot read PR2 urdf
00096     doc = parseString(robot_description)
00097     g_links = doc.getElementsByTagName('link')
00098     srv = Server(ContactStateMarkerConfig, config_callback)
00099     pub = rospy.Publisher('~marker', MarkerArray)
00100     sub = rospy.Subscriber("~input", ContactStatesStamped, callback)
00101     rospy.spin()