jsk_rviz_plugins: motor_states_temperature

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from hrpsys_ros_bridge.msg import MotorStates
00004 from jsk_rviz_plugins.msg import OverlayText
00005 from sensor_msgs.msg import JointState
00006 #from sensor_msgs.msg import JointState as MotorStates
00007 from std_msgs.msg import Float32
00008 import rospy
00009 from urdf_parser_py.urdf import URDF
00010 
00011 
00012 g_publishers = []
00013 g_effort_publishers = {}
00014 g_text_publisher = None
00015 safe_color = (52, 152, 219)     # ~50
00016 warning_color = (230, 126, 34)               # 50~
00017 fatal_color = (231, 76, 60)                   # 70~
00018 def allocatePublishers(num):
00019     global g_publishers
00020     if num > len(g_publishers):
00021         for i in range(len(g_publishers), num):
00022             pub = rospy.Publisher('temperature_%02d' % (i), Float32)
00023             g_publishers.append(pub)
00024 def allocateEffortPublishers(names):
00025     global g_effort_publishers
00026     for name in names:
00027         if not g_effort_publishers.has_key(name):
00028             g_effort_publishers[name] = rospy.Publisher('effort_%s' % (name), Float32)
00029         
00030 def motorStatesCallback(msg):
00031     global g_publishers, g_text_publisher
00032     #values = msg.position
00033     values = msg.temperature
00034     names = msg.name
00035     allocatePublishers(len(values))
00036     max_temparature = 0
00037     max_temparature_name = ""
00038     for name, val, pub in zip(names, values, g_publishers):
00039         pub.publish(Float32(val))
00040         if max_temparature < val:
00041             max_temparature = val
00042             max_temparature_name = name
00043     if max_temparature:
00044         if max_temparature > 70:
00045             color = fatal_color
00046         elif max_temparature > 50:
00047             color = warning_color
00048         else:
00049             color = safe_color
00050         text = OverlayText()
00051         text.fg_color.r = color[0] / 255.0
00052         text.fg_color.g = color[1] / 255.0
00053         text.fg_color.b = color[2] / 255.0
00054         text.fg_color.a = 1.0
00055         text.bg_color.a = 0.0
00056         text.text = "%dC -- (%s)"  % (int(max_temparature), max_temparature_name)
00057         g_text_publisher.publish(text)
00058 
00059 def jointStatesCallback(msg):
00060     global g_effort_publishers, robot_model
00061     values = msg.effort
00062     names = msg.name
00063     allocateEffortPublishers(names)
00064     for val, name in zip(values, names):
00065         # lookup effort limit
00066         candidate_joints = [j for j in robot_model.joints if j.name == name]
00067         if candidate_joints:
00068             if candidate_joints[0].limit:
00069                 limit = candidate_joints[0].limit.effort
00070             else:
00071                 limit = 0
00072             if limit != 0:
00073                 g_effort_publishers[name].publish(Float32(abs(val/limit)))
00074 
00075 if __name__ == "__main__":
00076     rospy.init_node("motor_state_temperature_decomposer")
00077     robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
00078     g_text_publisher = rospy.Publisher("max_temparature_text", OverlayText)
00079     s = rospy.Subscriber("/motor_states", MotorStates, motorStatesCallback, queue_size=1)
00080     s_joint_states = rospy.Subscriber("/joint_states", JointState, jointStatesCallback, queue_size=1)
00081     #s = rospy.Subscriber("joint_states", MotorStates, motorStatesCallback)
00082     rospy.spin()