diff --git a/jsk_tools/sample/config/analyzer.yaml b/jsk_tools/sample/config/analyzer.yaml
new file mode 100644
index 000000000..73bdac918
--- /dev/null
+++ b/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
diff --git a/jsk_tools/sample/sample_camera_check.launch b/jsk_tools/sample/sample_camera_check.launch
new file mode 100644
index 000000000..7fb338de7
--- /dev/null
+++ b/jsk_tools/sample/sample_camera_check.launch
@@ -0,0 +1,18 @@
+
+
+
+
+ topic_names: ["/camera/rgb/image_raw"]
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py
new file mode 100755
index 000000000..9ac22287a
--- /dev/null
+++ b/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()
diff --git a/jsk_tools/src/jsk_tools/nodelet_utils.py b/jsk_tools/src/jsk_tools/nodelet_utils.py
new file mode 100644
index 000000000..a85e306ea
--- /dev/null
+++ b/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
diff --git a/jsk_tools/src/jsk_tools/process_utils.py b/jsk_tools/src/jsk_tools/process_utils.py
new file mode 100644
index 000000000..d13fd4ab9
--- /dev/null
+++ b/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
diff --git a/jsk_tools/src/jsk_tools/usb_utils.py b/jsk_tools/src/jsk_tools/usb_utils.py
new file mode 100644
index 000000000..2d36e7a6d
--- /dev/null
+++ b/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\d+)\s+Device\s+(?P\d+).+ID\s(?P\w+:\w+)\s(?P.+)$", # 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'])