[rtm-ros-robotics] r3156 committed - split diagnostics.py into diagnostics.py and motor_states.py

[rtm-ros-...@googlecode.com]

Revision: 3156
Author: kei.okada
Date: Fri Sep 14 04:29:10 2012
Log: split diagnostics.py into diagnostics.py and motor_states.py
http://code.google.com/p/rtm-ros-robotics/source/detail?r=3156

Added:
/trunk/rtmros_common/hrpsys_ros_bridge/scripts/motor_states.py
Modified:
/trunk/rtmros_common/hrpsys_ros_bridge/scripts/diagnostics.py

=======================================
--- /dev/null
+++ /trunk/rtmros_common/hrpsys_ros_bridge/scripts/motor_states.py Fri Sep
14 04:29:10 2012
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('hrpsys_ros_bridge')
+import rospy
+import time
+import copy
+from std_msgs.msg import String
+from diagnostic_msgs.msg import *
+from hrpsys_ros_bridge.msg import MotorStates
+
+motor_states = None
+def states_cb(msg) :
+ global motor_states
+ motor_states = msg
+
+publish_motor_id = 0
+def publish_motor_states_diagnostics(msg) :
+ global publish_motor_id
+ diagnostic = DiagnosticArray()
+ diagnostic.header.stamp = msg.header.stamp
+
+ # motor
+ i = publish_motor_id % len(msg.name)
+ status = DiagnosticStatus(name = 'Motor ('+str(i)+":"+msg.name[i]+')',
level = DiagnosticStatus.OK, message = "OK")
+ if ( msg.servo_alarm[i] != 0 ) :
+ status.message = "NG"
+ status.level = DiagnosticStatus.WARN
+ status.values.append(KeyValue(key = "Calib State", value =
str(msg.calib_state[i])))
+ status.values.append(KeyValue(key = "Servo State", value =
str(msg.servo_state[i])))
+ status.values.append(KeyValue(key = "Power State", value =
str(msg.power_state[i])))
+ status.values.append(KeyValue(key = "Servo Alarm", value =
str(msg.servo_alarm[i])))
+ status.values.append(KeyValue(key = "Driver Temprature", value =
str(msg.driver_temp[i])))
+ diagnostic.status.append(status)
+
+ pub.publish(diagnostic)
+
+ publish_motor_id += 1
+
+if __name__ == '__main__':
+ try:
+ last_message_stamp = 0
+ rospy.init_node('motor_state_diagnostics')
+ sub = rospy.Subscriber('motor_states', MotorStates, states_cb)
+ pub = rospy.Publisher('diagnostics', DiagnosticArray)
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ if motor_states and motor_states.header.stamp !=
last_message_stamp :
+ last_message_stamp = copy.copy(motor_states.header.stamp)
+ publish_motor_states_diagnostics(motor_states)
+ r.sleep()
+ except rospy.ROSInterruptException: pass
=======================================
--- /trunk/rtmros_common/hrpsys_ros_bridge/scripts/diagnostics.py Fri Sep
14 00:41:05 2012
+++ /trunk/rtmros_common/hrpsys_ros_bridge/scripts/diagnostics.py Fri Sep
14 04:29:10 2012
@@ -2,6 +2,7 @@
import roslib; roslib.load_manifest('hrpsys_ros_bridge')
import rospy
import time
+import copy
from std_msgs.msg import String
from diagnostic_msgs.msg import *
from hrpsys_ros_bridge.msg import MotorStates
@@ -21,7 +22,9 @@
0x800 : 'SS_OTHER'
}

-def states_cb(msg):
+diagnostic = None
+def states_cb(msg) :
+ global diagnostic
diagnostic = DiagnosticArray()
diagnostic.header.stamp = msg.header.stamp

@@ -57,27 +60,18 @@
status.level = DiagnosticStatus.WARN
diagnostic.status.append(status)

-
- # error
- for i in range(len(msg.name)) :
- status = DiagnosticStatus(name = 'Motor ('+msg.name[i]+')', level
= DiagnosticStatus.OK, message = "OK")
- if ( msg.servo_alarm[i] != 0 ) :
- status.message = "NG"
- status.level = DiagnosticStatus.WARN
- status.values.append(KeyValue(key = "Calib State", value =
str(msg.calib_state[i])))
- status.values.append(KeyValue(key = "Servo State", value =
str(msg.servo_state[i])))
- status.values.append(KeyValue(key = "Power State", value =
str(msg.power_state[i])))
- status.values.append(KeyValue(key = "Servo Alarm", value =
str(msg.servo_alarm[i])))
- status.values.append(KeyValue(key = "Driver Temprature", value =
str(msg.driver_temp[i])))
- diagnostic.status.append(status)
-
- pub.publish(diagnostic)
+ return diagnostic

if __name__ == '__main__':
try:
- global initial_flag, time_latest
- rospy.init_node('hrpsys_diagnostics')
+ last_message_stamp = 0
+ rospy.init_node('operation_mode_diagnostics')
sub = rospy.Subscriber('motor_states', MotorStates, states_cb)
pub = rospy.Publisher('diagnostics', DiagnosticArray)
- rospy.spin()
+ r = rospy.Rate(10)
+ while not rospy.is_shutdown():
+ if diagnostic and diagnostic.header.stamp !=
last_message_stamp :
+ last_message_stamp = copy.copy(diagnostic.header.stamp)
+ pub.publish(diagnostic)
+ r.sleep()
except rospy.ROSInterruptException: pass