From 48f295409284eec074cea2605536f86b369147aa Mon Sep 17 00:00:00 2001 From: iory Date: Wed, 22 Aug 2018 14:26:40 +0900 Subject: [PATCH 01/11] [jsk_tools] Add camera_check.py --- jsk_tools/src/camera_check.py | 112 ++++++++++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100755 jsk_tools/src/camera_check.py diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py new file mode 100755 index 000000000..d758bf840 --- /dev/null +++ b/jsk_tools/src/camera_check.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +from __future__ import division + +import os +import collections + +import rostopic +import rospy +import diagnostic_updater +import diagnostic_msgs +from sensor_msgs.msg import Image + +from jsk_tools.sanity_lib import checkUSBExist + + +class CameraCheck(object): + + def __init__(self, device_path=None): + 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.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.subscribe() + + 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) + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + 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 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 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() From e1d75d67422c3b03c1651d0f1958d3a7219179c4 Mon Sep 17 00:00:00 2001 From: iory Date: Wed, 22 Aug 2018 14:27:05 +0900 Subject: [PATCH 02/11] [jsk_tools/camera_check.py] Add sample --- jsk_tools/sample/config/analyzer.yaml | 10 ++++++++++ jsk_tools/sample/sample_camera_check.launch | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 jsk_tools/sample/config/analyzer.yaml create mode 100644 jsk_tools/sample/sample_camera_check.launch 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"] + + + + + + + + + + From 39ae19ce3f5944fca7aee302348b44c683f3a4b7 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 24 Aug 2018 11:11:56 +0900 Subject: [PATCH 03/11] [jsk_tools/camera_check.py] Delete arg --- jsk_tools/src/camera_check.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index d758bf840..841324787 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -16,7 +16,7 @@ class CameraCheck(object): - def __init__(self, device_path=None): + def __init__(self): 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) From b3695f54581350424d9587d42093f60df1eb9d15 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 24 Aug 2018 11:59:29 +0900 Subject: [PATCH 04/11] [jsk_tools/camera_check.py] Add subscibe lock --- jsk_tools/src/camera_check.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index 841324787..ebc63d5a3 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -29,7 +29,7 @@ def __init__(self): self.diagnostic_updater.add('connection', self.check_connection) self.diagnostic_updater.add('image', self.check_topic) - self.subscribe() + self._is_subscribing = False def subscribe(self): self.subs = [] @@ -41,10 +41,14 @@ def subscribe(self): 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 callback(self, topic_name, msg): self.topic_msg_dict[topic_name].append(msg) From d33cd9cf813845d5a578c29d5f03dbd9f57ea82a Mon Sep 17 00:00:00 2001 From: iory Date: Sat, 3 Nov 2018 13:24:53 +0900 Subject: [PATCH 05/11] [jsk_tools/camera_check.py] Add is_image_valid function to check camera is valid --- jsk_tools/src/camera_check.py | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index ebc63d5a3..3e2349a8b 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -10,13 +10,17 @@ 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 jsk_tools.sanity_lib import checkUSBExist class CameraCheck(object): - def __init__(self): + 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) @@ -97,6 +101,23 @@ def check_topic(self, stat): 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 run(self): while not rospy.is_shutdown(): self.subscribe() From e83d03774c7608fcd96b8b73ef3016ee5ee3f7e0 Mon Sep 17 00:00:00 2001 From: iory Date: Sat, 3 Nov 2018 13:35:48 +0900 Subject: [PATCH 06/11] [jsk_tools/camera_check.py] Add speak function --- jsk_tools/src/camera_check.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index 3e2349a8b..f50c8878d 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -13,6 +13,7 @@ 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 @@ -28,6 +29,20 @@ def __init__(self, device_path=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) @@ -54,6 +69,16 @@ def unsubscribe(self): 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) From fb5dccf1bd29ec7ac5ebe1bee7dbdcb6a5aaddea Mon Sep 17 00:00:00 2001 From: iory Date: Sat, 3 Nov 2018 13:45:02 +0900 Subject: [PATCH 07/11] [jsk_tools/camera_check.py] Add reset_usb function to reset usb device --- jsk_tools/src/camera_check.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index f50c8878d..5a58716fc 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -4,6 +4,7 @@ import os import collections +import fcntl import rostopic import rospy @@ -105,6 +106,23 @@ def check_connection(self, stat): 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] From 5f4b5efefb0ead41a989eb369d01cba5315929f4 Mon Sep 17 00:00:00 2001 From: iory Date: Sat, 3 Nov 2018 14:06:07 +0900 Subject: [PATCH 08/11] [jsk_tools/camera_check.py] Add restart camera launch function --- jsk_tools/src/camera_check.py | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/jsk_tools/src/camera_check.py b/jsk_tools/src/camera_check.py index 5a58716fc..9ac22287a 100755 --- a/jsk_tools/src/camera_check.py +++ b/jsk_tools/src/camera_check.py @@ -3,8 +3,11 @@ from __future__ import division import os +import time import collections import fcntl +import subprocess +import traceback import rostopic import rospy @@ -161,6 +164,38 @@ def is_image_valid(self, msg): 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() From 54762d3d5728914d8a34fb05545e76b8232efeb0 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 22 Jul 2022 08:15:34 +0900 Subject: [PATCH 09/11] [jsk_tools] Add usb_utils lib --- jsk_tools/src/jsk_tools/usb_utils.py | 61 ++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 jsk_tools/src/jsk_tools/usb_utils.py 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']) From 6ddd29d509d56e4d2756922fd108f7c7f22f13cd Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 22 Jul 2022 08:16:01 +0900 Subject: [PATCH 10/11] [jsk_tools] Add process_utils to manipulate process --- jsk_tools/src/jsk_tools/process_utils.py | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 jsk_tools/src/jsk_tools/process_utils.py 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 From ecb70e1f3e3d040c5d9f53ae98402bc6e33212cd Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 22 Jul 2022 08:17:26 +0900 Subject: [PATCH 11/11] [jsk_tools] Add nodelet_utils to kill nodelet list --- jsk_tools/src/jsk_tools/nodelet_utils.py | 77 ++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 jsk_tools/src/jsk_tools/nodelet_utils.py 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