From 6235ac2ee0976569c23d0b8b173ae6de3d0e2042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 28 Jun 2019 10:37:10 +0200 Subject: [PATCH] camera node: publish camera_info topic --- CMakeLists.txt | 1 + config/camera_info.yaml | 20 +++++++++++ config/config_pose.yaml | 4 +-- launch/camera.launch | 5 +++ nodes/camera | 77 ++++++++++++++++++++++++++--------------- package.xml | 1 + readme.md | 4 +-- 7 files changed, 81 insertions(+), 31 deletions(-) create mode 100644 config/camera_info.yaml create mode 100644 launch/camera.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b8a03c7..580a9ab9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,7 @@ project(dope) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + camera_info_manager_py cv_bridge geometry_msgs message_filters diff --git a/config/camera_info.yaml b/config/camera_info.yaml new file mode 100644 index 00000000..2d638127 --- /dev/null +++ b/config/camera_info.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: dope_webcam_0 +camera_matrix: + rows: 3 + cols: 3 + data: [641.5, 0, 320.0, 0, 641.5, 240.0, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0, 0, 0, 0, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [641.5, 0, 320.0, 0, 0, 641.5, 240.0, 0, 0, 0, 1, 0] diff --git a/config/config_pose.yaml b/config/config_pose.yaml index 7115c678..1eb3a2b9 100644 --- a/config/config_pose.yaml +++ b/config/config_pose.yaml @@ -1,5 +1,5 @@ -topic_camera: "/dope/webcam_rgb_raw" -topic_camera_info: "/dope/camera_info" +topic_camera: "/dope/webcam/image_raw" +topic_camera_info: "/dope/webcam/camera_info" topic_publishing: "dope" input_is_rectified: True # Whether the input image is rectified (strongly suggested!) downscale_height: 500 # if the input image is larger than this, scale it down to this pixel height diff --git a/launch/camera.launch b/launch/camera.launch new file mode 100644 index 00000000..21716077 --- /dev/null +++ b/launch/camera.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/nodes/camera b/nodes/camera index 27803462..3a718f94 100755 --- a/nodes/camera +++ b/nodes/camera @@ -8,45 +8,68 @@ This file opens an RGB camera and publishes images via ROS. It uses OpenCV to capture from camera 0. """ -from __future__ import print_function - import cv2 import rospy +from camera_info_manager import CameraInfoManager from cv_bridge import CvBridge -from sensor_msgs.msg import Image as Image_msg - -# Global variables -cam_index = 0 # index of camera to capture -topic = '/dope/webcam_rgb_raw' # topic for publishing -cap = cv2.VideoCapture(cam_index) -if not cap.isOpened(): - print("ERROR: Unable to open camera for capture. Is camera plugged in?") - exit(1) - -def publish_images(freq=5): - rospy.init_node('dope_webcam_rgb_raw', anonymous=True) - images_out = rospy.Publisher(topic, Image_msg, queue_size=10) - rate = rospy.Rate(freq) +from sensor_msgs.msg import Image, CameraInfo +import sys + + +def publish_images(freq=100): + cam_index = 0 # index of camera to capture + + ### initialize ROS publishers etc. + rospy.init_node('dope_webcam') + camera_ns = rospy.get_param('camera', 'dope/webcam') + img_topic = '{}/image_raw'.format(camera_ns) + info_topic = '{}/camera_info'.format(camera_ns) + image_pub = rospy.Publisher(img_topic, Image, queue_size=10) + info_pub = rospy.Publisher(info_topic, CameraInfo, queue_size=10) + info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(cam_index), + namespace=camera_ns) + try: + camera_info_url = rospy.get_param('~camera_info_url') + if not info_manager.setURL(camera_info_url): + rospy.logwarn('Camera info URL invalid: %s', camera_info_url) + except KeyError: + # we don't have a camera_info_url, so we'll keep the + # default ('file://${ROS_HOME}/camera_info/${NAME}.yaml') + pass - print ("Publishing images from camera {} to topic '{}'...".format( - cam_index, - topic - ) - ) - print ("Ctrl-C to stop") + info_manager.loadCameraInfo() + if not info_manager.isCalibrated(): + rospy.logwarn('Camera is not calibrated, please supply a valid camera_info_url parameter!') + + ### open camera + cap = cv2.VideoCapture(cam_index) + if not cap.isOpened(): + rospy.logfatal("ERROR: Unable to open camera for capture. Is camera plugged in?") + sys.exit(1) + + rospy.loginfo("Publishing images from camera %s to topic '%s'...", cam_index, img_topic) + rospy.loginfo("Ctrl-C to stop") + + ### publish images + rate = rospy.Rate(freq) while not rospy.is_shutdown(): ret, frame = cap.read() if ret: - msg_frame_edges = CvBridge().cv2_to_imgmsg(frame, "bgr8") - images_out.publish(msg_frame_edges) + image = CvBridge().cv2_to_imgmsg(frame, "bgr8") + image.header.frame_id = 'dope_webcam' + image.header.stamp = rospy.Time.now() + image_pub.publish(image) + # we need to call getCameraInfo() every time in case it was updated + camera_info = info_manager.getCameraInfo() + camera_info.header = image.header + info_pub.publish(camera_info) rate.sleep() + if __name__ == "__main__": - - try : + try: publish_images() except rospy.ROSInterruptException: pass - diff --git a/package.xml b/package.xml index 70e3f460..043e5221 100644 --- a/package.xml +++ b/package.xml @@ -49,6 +49,7 @@ catkin + camera_info_manager_py cv_bridge geometry_msgs message_filters diff --git a/readme.md b/readme.md index 9d1d3ccd..cb963ddb 100644 --- a/readme.md +++ b/readme.md @@ -59,12 +59,12 @@ This is the official DOPE ROS package for detection and 6-DoF pose estimation of 2. **Start camera node** (or start your own camera node) ``` - $ rosrun dope camera # Publishes RGB images to `/dope/webcam_rgb_raw` + $ roslaunch dope camera.launch # Publishes RGB images to `/dope/webcam_rgb_raw` ``` The camera must publish a correct `camera_info` topic to enable DOPE to compute the correct poses. Basically all ROS drivers have a `camera_info_url` parameter where you can set the calibration info (but most ROS drivers include a reasonable default). - For details see the [camera tutorial](doc/camera_tutorial.md). + For details on calibration and rectification of your camera see the [camera tutorial](doc/camera_tutorial.md). 3. **Edit config info** (if desired) in `~/catkin_ws/src/dope/config/config_pose.yaml` * `topic_camera`: RGB topic to listen to