From da964a2a3da9162092efa5acdafa5a266c1b5d19 Mon Sep 17 00:00:00 2001
From: Ken Tossell <ken@tossell.net>
Date: Sat, 11 Feb 2012 19:24:59 -0500
Subject: [PATCH] use camera_info_manager for a real CameraInfo
---
libuvc_camera/cfg/UVCCamera.cfg | 3 +++
libuvc_camera/include/libuvc_camera/camera_driver.h | 3 +++
libuvc_camera/manifest.xml | 1 +
libuvc_camera/src/camera_driver.cpp | 8 ++++++--
4 files changed, 13 insertions(+), 2 deletions(-)
diff --git a/libuvc_camera/cfg/UVCCamera.cfg b/libuvc_camera/cfg/UVCCamera.cfg
index cbcb063..5f02b0b 100755
--- a/libuvc_camera/cfg/UVCCamera.cfg
+++ b/libuvc_camera/cfg/UVCCamera.cfg
@@ -42,6 +42,9 @@ gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING,
"ROS tf frame of reference, resolved with tf_prefix unless absolute.",
"camera")
+gen.add("camera_info_url", str_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Path to camera calibration file.", "")
+
# Camera Terminal controls
scanning_modes = gen.enum([gen.const("Interlaced", int_t, 0, ""),
diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h
index 0a7c624..ea6cb6f 100644
--- a/libuvc_camera/include/libuvc_camera/camera_driver.h
+++ b/libuvc_camera/include/libuvc_camera/camera_driver.h
@@ -6,6 +6,7 @@
#include <image_transport/image_transport.h>
#include <image_transport/camera_publisher.h>
#include <dynamic_reconfigure/server.h>
+#include <camera_info_manager/camera_info_manager.h>
#include <boost/thread/mutex.hpp>
#include <libuvc_camera/UVCCameraConfig.h>
@@ -64,6 +65,8 @@ private:
dynamic_reconfigure::Server<UVCCameraConfig> config_server_;
UVCCameraConfig config_;
bool config_changed_;
+
+ camera_info_manager::CameraInfoManager cinfo_manager_;
};
};
diff --git a/libuvc_camera/manifest.xml b/libuvc_camera/manifest.xml
index 8347f73..6bf8c9e 100644
--- a/libuvc_camera/manifest.xml
+++ b/libuvc_camera/manifest.xml
@@ -9,6 +9,7 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/libuvc_camera</url>
<depend package="roscpp"/>
+ <depend package="camera_info_manager"/>
<depend package="driver_base"/>
<depend package="dynamic_reconfigure"/>
<depend package="image_transport"/>
diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
index 37610bc..0f89b95 100644
--- a/libuvc_camera/src/camera_driver.cpp
+++ b/libuvc_camera/src/camera_driver.cpp
@@ -15,7 +15,8 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
ctx_(NULL), dev_(NULL), devh_(NULL), rgb_frame_(NULL),
it_(nh_),
config_server_(mutex_, priv_nh_),
- config_changed_(false) {
+ config_changed_(false),
+ cinfo_manager_(nh) {
cam_pub_ = it_.advertiseCamera("image_raw", 1, false);
}
@@ -67,6 +68,9 @@ void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t lev
OpenCamera(new_config);
}
+ if (new_config.camera_info_url != config_.camera_info_url)
+ cinfo_manager_.loadCameraInfo(new_config.camera_info_url);
+
if (state_ == kRunning) {
// TODO: scanning_mode
// TODO: auto_exposure
@@ -122,7 +126,7 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
image.data.resize(image.step * image.height);
memcpy(&(image.data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
- cam_pub_.publish(image, sensor_msgs::CameraInfo());
+ cam_pub_.publish(image, cinfo_manager_.getCameraInfo());
if (config_changed_) {
config_server_.updateConfig(config_);
--
GitLab