add auto_recover to new cob_helper_tools pkg

cob_command_tools/package.xml

@@ -16,6 +16,7 @@
 
   <exec_depend>cob_command_gui</exec_depend>
   <exec_depend>cob_dashboard</exec_depend>
+  <exec_depend>cob_helper_tools</exec_depend>
   <exec_depend>cob_interactive_teleop</exec_depend>
   <exec_depend>cob_monitoring</exec_depend>
   <exec_depend>cob_script_server</exec_depend>

cob_helper_tools/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cob_helper_tools)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+#############
+## Install ##
+#############
+
+install(PROGRAMS
+  scripts/auto_recover.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

cob_helper_tools/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cob_helper_tools</name>
+  <version>0.6.5</version>
+  <description>Helper scripts for Care-O-bot</description>
+
+  <maintainer email="[email protected]">Felix Messmer</maintainer>
+  <author email="[email protected]">Felix Messmer</author>
+
+  <license>LGPL</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>cob_msgs</exec_depend>
+  <exec_depend>cob_script_server</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+</package>

cob_helper_tools/scripts/auto_recover.py

@@ -0,0 +1,45 @@
+#!/usr/bin/python
+
+import rospy
+
+from cob_msgs.msg import EmergencyStopState
+from diagnostic_msgs.msg import DiagnosticArray
+
+from simple_script_server import *
+sss = simple_script_server()
+
+class AutoRecover():
+
+  def __init__(self):
+    self.components = rospy.get_param('~components', [])
+    self.em_state = 0
+    rospy.Subscriber("/emergency_stop_state", EmergencyStopState, self.em_cb)
+    rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self.diagnostics_cb)
+    self.last_time_recover = rospy.Time.now()
+
+  def em_cb(self, msg):
+    if msg.emergency_state != self.em_state:
+      if msg.emergency_state == 0:
+        rospy.loginfo("auto_recover from scanner stop")
+        self.recover()
+      self.em_state = msg.emergency_state
+
+  def recover(self):
+    # call recover for all components
+    for component in self.components:
+      handle = sss.recover(component)
+    self.last_time_recover = rospy.Time.now()
+
+  # auto recover based on diagnostics
+  def diagnostics_cb(self, msg):
+    for status in msg.status:
+      if status.level > 1: # only recover on error, not on warning status
+        if "Actuators" in status.name and self.em_state == 0 and (rospy.Time.now() - self.last_time_recover) > rospy.Duration(10):
+          self.recover()
+
+if __name__ == "__main__":
+
+  rospy.init_node("auto_recover")
+  AR = AutoRecover()
+  rospy.loginfo("auto recover running")
+  rospy.spin()