camera node: publish camera_info topic

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

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]

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

launch/camera.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node name="dope_webcam" pkg="dope" type="camera" output="screen">
+    <param name="camera_info_url" value="package://dope/config/camera_info.yaml" />
+  </node>
+</launch>

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
-

package.xml

@@ -49,6 +49,7 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>camera_info_manager_py</depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>message_filters</depend>

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