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