Cannot exit node when pressing ctrl-c in while not rospy.is_shutdown()
Hi everybody,
I am using ROS 20 noetic on ubuntu 20.04. I try using my camera (Arducam IMX477 with usb adapter). I first created a node to acquire the frame and publish them on a topic with two services which allow me to change the resolution and save a frame.
Here is the code ros_IMX477.py :
import rospy
import os
import sys
from pathlib import Path
import cv2
import time
import datetime
import threading
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
bridge = CvBridge()
# Initialize architecture
FILE = Path(sys.argv[0]).resolve() # Get the full path of the file
node_name = os.path.splitext(FILE.name)[0]
import rospkg
rospack = rospkg.RosPack()
package = rospkg.get_package_name(FILE) # Get the package name
ROOT = Path(package).resolve() # Get the full path of the package
from vision.srv import CameraResolution, CameraResolutionResponse, CameraOnOff, CameraOnOffResponse
# Topics to subscribe/publish
camera_topic = "/cam_2/color/image_raw"
# Define a camera handler
class Camera_handler:
def __init__(self):
# Create the camera handler object
try:
entries = os.listdir("/dev")
num = min([int(entry[5:]) for entry in entries if "video" in entry])
src = "/dev/video"+str(num)
self.cap = cv2.VideoCapture(src, cv2.CAP_V4L2)
except cv2.error as error:
print("[Error]: {}".format(error))
if not self.cap.isOpened():
print("Cannot open camera handler")
exit()
# Init parameters
(self.ret, self.img) = self.cap.read()
self.idx = 1 # The number of frame grabbed
self.width = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH)
self.height = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
rospy.set_param('imx477', {'width': self.width, 'height': self.height, 'fps': self.fps})
self.cam_pub = rospy.Publisher(camera_topic, Image, queue_size=1) # Publisher
print("Camera handler capturing %s at (%d,%d) %dFPS" %(src, int(self.width),int(self.height),int(self.fps)))
self.ResolutionService = rospy.Service('setResolution', CameraResolution, self.setResolution)
self.CaptureService = rospy.Service('Capture', CameraOnOff, self.Capture)
self.mutex = threading.Semaphore()
def read(self):
self.mutex.acquire()
(self.ret, self.img) = self.cap.read()
if self.ret:
self.img = cv2.rotate(self.img, cv2.ROTATE_90_CLOCKWISE)
im = bridge.cv2_to_imgmsg(self.img, encoding="bgr8")
# Select one frame according to the index and publish it
im.header.stamp = rospy.get_rostime() # Set a timestamp
im.header.frame_id = "frame_" + str(self.idx)
im.header.seq = self.idx
self.cam_pub.publish(im) # Publish the frame
#rospy.loginfo("frame published")
self.idx+=1
self.mutex.release()
def setResolution(self, req):
resolution = round(req.resolution,1)
if resolution == 0.3:
width, height = 640, 480
elif resolution == 2:
width, height = 1920, 1080
elif resolution == 5:
width, height = 2592, 1944
elif resolution == 8:
width, height = 3840, 2160
elif resolution == 12:
width, height = 4032, 3040
else:
print("Required resolution %.1fMpx unavailable" %resolution)
return CameraResolutionResponse(False)
try :
self.mutex.acquire()
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
# update attributes
self.width = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH)
self.height = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
self.fps = self.cap.get(cv2.CAP_PROP_FPS)
rospy.set_param('imx477', {'width': self.width, 'height': self.height, 'fps': self.fps})
print("Parameters updated to (%d,%d ...