Interrupt Python node with multiple threads (after starting other nodes via `roslaunch` system command)

asked 2021-02-16 22:54:25 -0500

fvd gravatar image

updated 2021-02-16 22:55:14 -0500

Background: Our Realsense cameras bug out once in a while which causes their depth stream to fail, but they don't automatically restart when this happens, and there is no restart service or similar. Relaunching their roslaunch file with initial_reset:=true fixes the problem though, so I made a Python node that subscribes to each camera's depth stream to check that it is still alive, and restarts the camera when it fails, like this:

class CameraWatcher(object):
    def __init__(self):
        # [...] (subscribe to camera depth streams, update self.last_msg_times in callback functions)
        self.check_camera_status_loop(self):

    # [...]

    def check_camera_status_loop(self):
        r = rospy.Rate(1)
        while not rospy.is_shutdown():
            now = datetime.datetime.now()
            for cam in self.camera_names:
                time_since_last_msg = now - self.last_msg_times[cam]
                if time_since_last_msg > datetime.timedelta(milliseconds=1000):
                    rospy.logerr("NO DEPTH IMAGE RECEIVED FROM CAMERA: " + cam)
                    rospy.logerr("RESETTING CAMERA: " + cam)
                    # print("time since last message: ", str(time_since_last_msg), "cam: ", cam)

                    # Restart camera by killing the nodes and spawning a roslaunch process
                    os.system("rosnode kill /" + cam + "/realsense2_camera /" + cam + "/realsense2_camera_manager")
                    rospy.sleep(1)
                    command = "roslaunch our_scene_description bringup_cam_" + str(cam) + ".launch initial_reset:=true"
                    rospy.loginfo("Executing command: " + command)
                    thread.start_new_thread(os.system, (command,))

                    rospy.loginfo("Sleeping for 15 seconds to let camera restart")
                    rospy.sleep(15)
                    rospy.loginfo("Camera watcher active again (waited 15 seconds for camera " + str(cam) + " to restart)")
            r.sleep()

        rospy.logerr("Stopping our_camera_watcher (rospy shutdown)")

if __name__ == '__main__':
    rospy.init_node('our_camera_watcher', anonymous=False)
    c = CameraWatcher()
    rospy.loginfo("our_camera_watcher has finished")

However, when I try to kill this node via Ctrl-C, the loop continues. I have to send the KeyboardInterrupt multiple times for it to go through. I am guessing that the first Ctrl-C only kills the process in the new thread, but does not shut down rospy, or something similar. I can't dig any further because this small bug isn't in the way of my progress, but I don't like not understanding the problem.

I found these related posts, but they didn't really solve the issue:
https://answers.ros.org/question/9543...
https://answers.ros.org/question/2648...
https://answers.ros.org/question/2993...

Does anyone know what's really going on? Is there a better practice that I am missing? Could I start up the second thread differently so this problem won't occur? Is there a better approach coming up in ROS2, with Python-based launch files? Thanks for any pointers.

edit retag flag offensive close merge delete