use camera_info_manager for a real CameraInfo

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, ""),

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_;
 };
 
 };

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"/>

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_);