Commit da964a2a authored by Ken Tossell's avatar Ken Tossell
Browse files

use camera_info_manager for a real CameraInfo

parent da4269ce
......@@ -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, ""),
......
......@@ -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_;
};
};
......@@ -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"/>
......
......@@ -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_);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment