#!/usr/bin/env python
#
# Monitor efforts on actuators and report monitoring result to
# diagnostics topic.
#
# Monitoring relies on the maximum effort provided by the URDF robot
# description which must be loaded into the robot_description key of
# the parameter server.
#

import roslib; roslib.load_manifest('walk_tools')

import rospy

import sys

from sensor_msgs.msg import JointState
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

import urdf_parser_py

efforts_warning_threshold = {}
efforts_error_threshold = {}

rospy.init_node("effort_monitor")

try:
    robot = urdf_parser_py.URDF.load_from_parameter_server()
    for k, joint in robot.joints.iteritems():
        if joint.limits:
            efforts_warning_threshold[joint.name] = \
                joint.limits.effort * .85
            efforts_error_threshold[joint.name] = joint.limits.effort
        else:
            rospy.logdebug(
                "no maximum effort for joint {0}, discarding".format(
                    joint.name))
except Exception as e:
    rospy.logfatal("cannot load robot in"
                   + " model_description parameter server key, failing..."
                   + "\n{0}".format(e))
    sys.exit(1)

joint_state = None

def jointStateCb(data):
    global joint_state
    joint_state = data

def effort_monitor():
    pub = rospy.Publisher("/diagnostics", DiagnosticArray)
    rospy.Subscriber("joint_states", JointState, jointStateCb)

    stat_joints = DiagnosticStatus()
    stat_joints.level = 0
    stat_joints.name = "Joint effort monitor"
    stat_joints.message = "OK"
    stat_joints.hardware_id = "none"
    stat_joints.values = []

    rospy.loginfo("start monitoring...")
    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        # Check efforts.
        stat_joints.level = DiagnosticStatus.OK
        stat_joints.message = "OK"
        stat_joints.values = []
        if joint_state:
            for i in xrange(len(joint_state.effort)):
                # Retrieve joint name.
                jointName = str(i)
                if len(joint_state.name) > i:
                    jointName = joint_state.name[i]

                if not jointName in efforts_error_threshold or \
                        not jointName in efforts_warning_threshold:
                    continue

                # Fill key.
                key = "Effort on joint {0}".format(jointName)
                stat_joints.values.append(KeyValue(key,
		 str(joint_state.effort[i])))
                if stat_joints.level == DiagnosticStatus.OK and \
                        abs(joint_state.effort[i]) > \
			efforts_warning_threshold[jointName]:
                    stat_joints.level = DiagnosticStatus.WARN
                    stat_joints.message = "High effort on {0} (> {1})".format(
                        jointName, efforts_warning_threshold[jointName])

                if stat_joints.level != DiagnosticStatus.ERROR and \
                        abs(joint_state.effort[i]) > \
			efforts_error_threshold[jointName]:
                    stat_joints.level = DiagnosticStatus.ERROR
                    stat_joints.message = \
                        "Effort too high on {0} (> {1})".format(
                        jointName, efforts_error_threshold[jointName])

        msg = DiagnosticArray()
        msg.header.stamp = rospy.get_rostime()
        msg.status = [stat_joints]
        pub.publish(msg)
        rate.sleep()

if __name__ == "__main__":
    try:
        effort_monitor()
    except KeyboardInterrupt: pass
    except SystemExit: pass
    except:
        import traceback
        traceback.print_exc()
