kinect_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('sensor_msgs')
00004 
00005 import subprocess
00006 import threading
00007 import traceback
00008 import time
00009 
00010 import rospy
00011 
00012 from sensor_msgs.msg import CameraInfo
00013 
00014 # Restart Kinect node if it stops publishing
00015 
00016 class KinectMonitor(object):
00017     def __init__(self):
00018         self._lock = threading.RLock()
00019 
00020         self._last_cam_info   = time.time()
00021         self._last_spawn_time = time.time()
00022 
00023         self._kinect_node_name = rospy.get_param('~kinect_node_name', 'openni_node1')
00024         self._grace_period     = rospy.get_param('~grace_period',     30)
00025 
00026         self._sub = rospy.Subscriber('camera_info', CameraInfo, self._cb)
00027 
00028     def _cb(self, msg):
00029         with self._lock:
00030             self._last_cam_info = time.time()
00031 
00032     def _kill_kinect_node(self):
00033         rospy.logwarn('Trying to kill kinect node "%s". Node is stale' % self._kinect_node_name)
00034         
00035         retcode = -1
00036         try:
00037             retcode = subprocess.call('rosnode kill %s' % self._kinect_node_name, shell=True)
00038         except Exception, e:
00039             rospy.logerr('Unable to kill kinect node, caught exception:\n%s', traceback.format_exc())
00040             return
00041 
00042         if retcode != 0:
00043             rospy.logerr('Unable to kill kinect node, returned error code %d' % retcode)
00044             
00045     def update(self):
00046         with self._lock:
00047             if time.time() - self._last_cam_info > self._grace_period and time.time() - self._last_spawn_time > self._grace_period:
00048                 self._kill_kinect_node()
00049                 self._last_cam_info = time.time()
00050 
00051 if __name__ == '__main__':
00052     rospy.init_node('kinect_monitor')
00053 
00054     monitor = KinectMonitor()
00055     
00056     rate = rospy.Rate(1.0)
00057     try:
00058         while not rospy.is_shutdown():
00059             rate.sleep()
00060             monitor.update()
00061     except KeyboardInterrupt:
00062         pass
00063     except Exception, e:
00064         traceback.print_exc()


pr2_object_manipulation_launch
Author(s): Matei Ciocarlie
autogenerated on Mon Oct 6 2014 12:29:27