Add Camera check node

jsk_tools/sample/config/analyzer.yaml

@@ -0,0 +1,10 @@
+analyzers:
+  cameras:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Cameras
+    analyzers:
+      xtion:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: xtion
+        find_and_remove_prefix: 'camera_check: '
+        num_items: 2

jsk_tools/sample/sample_camera_check.launch

@@ -0,0 +1,18 @@
+<launch>
+
+  <node name="camera_check"
+        pkg="jsk_tools" type="camera_check.py" >
+    <rosparam>
+      topic_names: ["/camera/rgb/image_raw"]
+      <!-- vid: "1d27" -->
+      <!-- pid: "0601" -->
+    </rosparam>
+  </node>
+
+  <node name="diagnostic_aggregator"
+        pkg="diagnostic_aggregator" type="aggregator_node"
+        clear_params="true" >
+    <rosparam command="load" file="$(find jsk_tools)/sample/config/analyzer.yaml"/>
+  </node>
+
+</launch>

jsk_tools/src/camera_check.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python
+
+from __future__ import division
+
+import os
+import time
+import collections
+import fcntl
+import subprocess
+import traceback
+
+import rostopic
+import rospy
+import diagnostic_updater
+import diagnostic_msgs
+from sensor_msgs.msg import Image
+import cv2
+from cv_bridge import CvBridge
+from cv_bridge import CvBridgeError
+from sound_play.msg import SoundRequest
+
+from jsk_tools.sanity_lib import checkUSBExist
+
+
+class CameraCheck(object):
+
+    def __init__(self, device_path=None):
+        self.bridge = CvBridge()
+        self.topic_names = rospy.get_param('~topic_names', [])
+        self.device_type = rospy.get_param("~device_type", 'usb')
+        self.device_path = rospy.get_param('~device_path', None)
+        self.vendor_id = rospy.get_param('~vid', None)
+        self.product_id = rospy.get_param('~pid', None)
+        self.duration = rospy.get_param('~duration', 1)
+        self.frequency = rospy.get_param('~frequency', None)
+        self.speak_enabled = rospy.get_param("~speak", True)
+        self.speak_pub = rospy.Publisher(
+            "/robotsound", SoundRequest, queue_size=1)
+
+        # nodelet manager name for restarting
+        self.camera_nodelet_manager_name = rospy.get_name(
+            "~camera_nodelet_manager_name", None)
+        if self.camera_nodelet_manager_name is not None:
+            self.camera_nodelet_manager_name = rospy.get_name(
+                "~child_camera_nodelet_manager_name",
+                os.path.basename(self.camera_nodelet_manager_name))
+        self.restart_camera_command = rospy.get_param(
+            '~restart_camera_command', None)
+
+        self.diagnostic_updater = diagnostic_updater.Updater()
+        self.diagnostic_updater.setHardwareID(rospy.get_param("~hardware_id", 'none'))
+        self.diagnostic_updater.add('connection', self.check_connection)
+        self.diagnostic_updater.add('image', self.check_topic)
+
+        self._is_subscribing = False
+
+    def subscribe(self):
+        self.subs = []
+        self.topic_msg_dict = {}
+        for topic_name in self.topic_names:
+            self.topic_msg_dict[topic_name] = []
+            msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
+            sub = rospy.Subscriber(topic_name, msg_class,
+                                   callback=lambda msg : self.callback(topic_name, msg),
+                                   queue_size=1)
+            self.subs.append(sub)
+        self._is_subscribing = True
+
+    def unsubscribe(self):
+        if self._is_subscribing is False:
+            return
+        for sub in self.subs:
+            sub.unregister()
+        self._is_subscribing = False
+
+    def speak(self, speak_str):
+        rospy.logerr(
+            "[%s] %s", self.__class__.__name__, speak_str)
+        if self.speak_enabled:
+            msg = SoundRequest()
+            msg.sound = SoundRequest.SAY
+            msg.command = SoundRequest.PLAY_ONCE
+            msg.arg = speak_str
+            self.speak_pub.publish(msg)
+
+    def callback(self, topic_name, msg):
+        self.topic_msg_dict[topic_name].append(msg)
+
+    def check_connection(self, stat):
+        if self.device_type == 'usb':
+            if self.device_path is not None:
+                if os.path.exists(self.device_path):
+                    stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
+                                 'device exists : {}'.format(self.device_path))
+                else:
+                    stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
+                                 'device not exists : {}'.format(self.device_path))
+            elif (self.vendor_id is not None) and (self.product_id is not None):
+                if checkUSBExist(self.vendor_id, self.product_id):
+                    stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
+                                 'device exists : {}:{}'.format(
+                                     self.vendor_id, self.product_id))
+                else:
+                    stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
+                                 'device exists : {}:{}'.format(
+                                     self.vendor_id, self.product_id))
+        else:
+            raise NotImplementedError("device_type {} is not yet supported".
+                                      format(self.device_type))
+        return stat
+
+    def reset_usb(self):
+        if self.device_path is None:
+            rospy.logwarn('device_path is not exists. '
+                          'Please set device_path')
+            return False
+        fd = os.open(self.device_path, os.O_WROMLY)
+        if fd < 0:
+            rospy.logerr("Could not open {}".format(self.device_path))
+            return False
+        rospy.loginfo("Resetting USB device")
+        # Equivalent of the _IO('U', 20) constant in the linux kernel.
+        USBDEVFS_RESET = ord('U') << (4*2) | 20
+        try:
+            rc = fcntl.ioctl(fd, USBDEVFS_RESET, 0)
+        finally:
+            os.cloose(fd)
+
+    def check_topic(self, stat):
+        for topic_name in self.topic_msg_dict.keys():
+            msgs = self.topic_msg_dict[topic_name]
+            if len(msgs) == 0:
+                stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
+                             'topic {} not available'.format(topic_name))
+            else:
+                if self.frequency == None:
+                    stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
+                                 'topic {} available'.format(topic_name))
+                else:
+                    topic_hz = len(msgs) / self.duration
+                    if topic_hz >= self.frequency:
+                        stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
+                                     'topic {} available'.format(topic_name))
+                    else:
+                        stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
+                                     'topic {} available'.format(topic_name))
+                    stat.add('topic {} {} Hz'.format(topic_name, topic_hz))
+        return stat
+
+    def is_image_valid(self, msg):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+            sum_of_pixels = max(cv2.sumElems(cv_image))
+        except CvBridgeError as e:
+            rospy.logerr(
+                "[%s] failed to convert image to cv",
+                self.__class__.__name__)
+            return False
+        rospy.loginfo(
+            "[%s] sum of pixels is %d at %s",
+            self.__class__.__name__,
+            sum_of_pixels, msg.header.stamp.secs)
+        if sum_of_pixels == 0:
+            return False
+        return True
+
+    def restart_camera_node(self):
+        rospy.logerr("Restart camera node")
+        try:
+            # 1. usbreset...
+            self.speak("resetting u s b")
+            self.reset_usb()
+            time.sleep(10)
+            # 2. kill nodelet manager
+            if self.camera_nodelet_manager_name is not None:
+                self.speak("something wrong with camera node, "
+                           "I'll restart it, killing nodelet manager")
+                retcode = subprocess.call(
+                    'rosnode kill %s' %
+                    (self.camera_nodelet_manager_name), shell=True)
+                time.sleep(10)
+
+                # 3. pkill
+                self.speak("killing child processes")
+                retcode = subprocess.call(
+                    'pkill -f %s' %
+                    self.child_camera_nodelet_manager_name,
+                    shell=True)
+                time.sleep(10)
+
+            # 4 restarting
+            self.speak("restarting processes")
+            retcode = subprocess.call(
+                self.restart_camera_command, shell=True)
+        except Exception as e:
+            rospy.logerr('[%s] Unable to kill kinect node, caught exception:\n%s',
+                         self.__class__.__name__, traceback.format_exc())
+
+    def run(self):
+        while not rospy.is_shutdown():
+            self.subscribe()
+            rospy.sleep(self.duration)
+            self.unsubscribe()
+            self.diagnostic_updater.update()
+
+
+def main():
+    rospy.init_node('camera_check')
+    cc = CameraCheck()
+    cc.run()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

jsk_tools/src/jsk_tools/nodelet_utils.py

@@ -0,0 +1,77 @@
+import os.path as osp
+import signal
+import subprocess
+
+from nodelet.srv import NodeletList
+import psutil
+import rosgraph
+import rosnode
+import rospy
+
+
+try:
+    from xmlrpc.client import ServerProxy
+except ImportError:
+    from xmlrpclib import ServerProxy
+
+from jsk_tools.process_utils import search_pid_from_process_cmd
+
+
+def kill_nodelets(manager_name, manager_namespace, nodelet_list=None,
+                  child_process=None, wait_duration=3.0):
+    """Kill nodelets and restart nodelet manager.
+
+    To respawn nodelets, we assumes respawn=true is set.
+    """
+    if nodelet_list is None:
+        nodelet_list = rospy.ServiceProxy(
+            osp.join(manager_namespace, manager_name, 'list'),
+            NodeletList)().nodelets
+
+    if child_process is None:
+        pid = search_pid_from_process_cmd('__name:=' + manager_name)
+        if pid is not None:
+            rospy.loginfo('Found nodelet manager process ({})'.format(pid))
+            process = psutil.Process(pid)
+            if process.is_running():
+                process.send_signal(signal.SIGINT)
+            else:
+                raise Exception('manager process is not running')
+            # wait until manager is dead
+            start = rospy.Time.now()
+            while psutil.pid_exists(pid) and (
+                    rospy.Time.now() - start < rospy.Duration(wait_duration)):
+                rospy.sleep(0.1)
+            if process.is_running():
+                process.send_signal(signal.SIGKILL)
+        else:
+            rospy.loginfo('Not found nodelet manager process.')
+    else:
+        # manager process is launching by respawner as its child process
+        try:
+            rospy.loginfo('kill child process {}').format(child_process)
+            child_process.terminate()
+            child_process.wait()
+        except Exception as e:
+            rospy.logwarn('{}'.format(str(e)))
+
+    # restart nodelet manager
+    respawn_manager_cmd = ["rosrun", "nodelet",
+                           "nodelet", "manager",
+                           "__name:=" + manager_name,
+                           "__ns:=" + manager_namespace]
+    child_process = subprocess.Popen(respawn_manager_cmd)
+    name = manager_name + '/respawner'
+    # Kill nodelet. This assumes respawn=true is set.
+    master = rosgraph.Master(name)
+    for nodelet in nodelet_list:
+        try:
+            pid = ServerProxy(rosnode.get_api_uri(
+                master, nodelet, skip_cache=True)).getPid(name)[2]
+        except TypeError:
+            rospy.logwarn("Failed killing nodelet: %s", nodelet)
+            continue
+        rospy.loginfo("Killing nodelet: %s(%d)", nodelet, pid)
+        process = psutil.Process(pid)
+        process.send_signal(signal.SIGKILL)
+    return child_process

jsk_tools/src/jsk_tools/process_utils.py

@@ -0,0 +1,7 @@
+import psutil
+
+
+def search_pid_from_process_cmd(cmd):
+    for p in psutil.process_iter():
+        if cmd in p.cmdline():
+            return p.pid

jsk_tools/src/jsk_tools/usb_utils.py

@@ -0,0 +1,61 @@
+import fcntl
+import os
+import re
+import subprocess
+
+import rospy
+
+
+device_re = re.compile(
+    "Bus\s+(?P<bus>\d+)\s+Device\s+(?P<device>\d+).+ID\s(?P<id>\w+:\w+)\s(?P<tag>.+)$",  # NOQA
+    re.I)
+
+
+def reset_usb(device_path):
+    if device_path is None:
+        rospy.logwarn('device_path is not exists. '
+                      'Please set device_path')
+        return False
+    fd = os.open(device_path, os.O_WRONLY)
+    if fd < 0:
+        rospy.logerr("Could not open {}".format(device_path))
+        return False
+    # Equivalent of the _IO('U', 20) constant in the linux kernel.
+    USBDEVFS_RESET = ord('U') << (4 * 2) | 20
+    try:
+        fcntl.ioctl(fd, USBDEVFS_RESET, 0)
+    finally:
+        os.close(fd)
+    return True
+
+
+def list_devices():
+    df = subprocess.check_output("lsusb")
+    devices = []
+    if isinstance(df, bytes):
+        df = df.decode('utf-8')
+    for i in df.split('\n'):
+        if i:
+            info = device_re.match(i)
+            if info:
+                dinfo = info.groupdict()
+                dinfo['device'] = '/dev/bus/usb/%s/%s' % (dinfo.pop('bus'),
+                                                          dinfo.pop('device'))
+                devices.append(dinfo)
+    return devices
+
+
+def reset_usb_from_matched_pattern(pattern):
+    """USB reset from name pattern.
+
+    """
+    devices = list_devices()
+    filtered_devices = []
+    for device in devices:
+        if re.match(pattern, device['tag']):
+            filtered_devices.append(device)
+    for device in filtered_devices:
+        rospy.loginfo(
+            "Resetting USB device '{}' '{}'".format(device['tag'],
+                                                    device['device']))
+        reset_usb(device['device'])