diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt
index cf7f803aefa588246c2f9f02ab3e8d8d6a5d4862..272cdfa112cd72e4e646087b526bffe4ef506f33 100644
--- a/libuvc_camera/CMakeLists.txt
+++ b/libuvc_camera/CMakeLists.txt
@@ -21,6 +21,10 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined services
#rosbuild_gensrv()
+rosbuild_find_ros_package(dynamic_reconfigure)
+include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
+gencfg()
+
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
@@ -29,6 +33,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
-rosbuild_add_executable(libuvc_camera src/libuvc_camera.cpp)
-target_link_libraries(libuvc_camera uvc)
+rosbuild_add_executable(camera_node src/main.cpp src/camera_driver.cpp)
+target_link_libraries(camera_node uvc)
diff --git a/libuvc_camera/cfg/UVCCamera.cfg b/libuvc_camera/cfg/UVCCamera.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..cbcb06376295b4afd584b994a24232ea478463f5
--- /dev/null
+++ b/libuvc_camera/cfg/UVCCamera.cfg
@@ -0,0 +1,166 @@
+#! /usr/bin/env python
+# Derived from camera1394 cfg
+
+PACKAGE='libuvc_camera'
+import roslib; roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator import *
+from driver_base.msg import SensorLevels
+
+gen = ParameterGenerator()
+
+# Name, Type, Reconfiguration level, Description, Default, Min, Max
+
+gen.add("vendor", str_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Vendor ID, hex digits (use camera of any vendor if null).",
+ "")
+
+gen.add("product", str_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Product ID, hex digits (use camera of any model if null).",
+ "")
+
+gen.add("serial", str_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Serial number, arbitrary string (use camera of any serial number if null).",
+ "")
+
+gen.add("index", int_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Index into the list of cameras that match the above parameters.",
+ 0)
+
+gen.add("width", int_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Image width.", 640)
+
+gen.add("height", int_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Image height.", 480)
+
+gen.add("frame_rate", double_t, SensorLevels.RECONFIGURE_CLOSE,
+ "Camera speed, frames per second.", 15.0, 0.1, 1000.0)
+
+# TODO: video mode -- uncompressed, yuyv, uyvy, rgb, compressed, jpeg, ...
+
+gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING,
+ "ROS tf frame of reference, resolved with tf_prefix unless absolute.",
+ "camera")
+
+# Camera Terminal controls
+
+scanning_modes = gen.enum([gen.const("Interlaced", int_t, 0, ""),
+ gen.const("Progressive", int_t, 1, "")],
+ "Scanning modes")
+
+gen.add("scanning_mode", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Scanning mode.", 0, 0, 1,
+ edit_method = scanning_modes)
+
+auto_exposure_modes = gen.enum([gen.const("Manual", int_t, 0, "Manual exposure, manual iris"),
+ gen.const("Auto", int_t, 1, "Auto exposure, auto iris"),
+ gen.const("Shutter_Priority", int_t, 2, "manual exposure, auto iris"),
+ gen.const("Aperture_Priority", int_t, 3, "auto exposure, manual iris")],
+ "Auto-exposure modes")
+
+gen.add("auto_exposure", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Auto exposure mode.",
+ 1, 0, 3, edit_method = auto_exposure_modes)
+
+gen.add("auto_exposure_priority", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "In auto mode or shutter priority mode, allow the device to vary frame rate.",
+ 0, 0, 1)
+
+gen.add("exposure_absolute", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Length of exposure, seconds.", 0., 0.0001, 10.0)
+
+# TODO: relative exposure time
+
+gen.add("iris_absolute", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Aperture, f.", 0., 0., 655.35)
+
+# TODO: relative iris
+
+gen.add("auto_focus", bool_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Maintain focus automatically.", False)
+
+gen.add("focus_absolute", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Absolute focal distance, millimeters.", 0, 0, 65536)
+
+# TODO: relative focus
+
+# TODO: zoom
+
+gen.add("pan_absolute", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Pan (clockwise), arc seconds.", 0, -180*3600, 180*3600)
+
+gen.add("tilt_absolute", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Tilt (up), arc seconds.", 0, -180*3600, 180*3600)
+
+# TODO: relative pan/tilt
+
+gen.add("roll_absolute", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Roll (clockwise), degrees.", 0, -180, 180)
+
+# TODO: relative roll
+
+gen.add("privacy", bool_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Image capture disabled.", False)
+
+# Processing Unit controls
+
+gen.add("backlight_compensation", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Backlight compensation, device-dependent (zero for none, increasing compensation above zero).",
+ 0, 0, 65536)
+
+
+gen.add("brightness", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Brightness, device dependent.", 0, -32768, 32767)
+
+gen.add("contrast", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Contrast, device dependent.", 0, -32768, 32767)
+
+gen.add("gain", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Gain, device dependent.", 0, 0, 65536)
+
+power_line_frequency_modes = gen.enum([gen.const("Disabled", int_t, 0, "Disabled"),
+ gen.const("Freq_50", int_t, 1, "50 Hz"),
+ gen.const("Freq_60", int_t, 1, "60 Hz")],
+ "Power line frequency modes")
+
+gen.add("power_line_frequency", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Power line frequency anti-flicker processing.",
+ 0, 0, 2,
+ edit_method = power_line_frequency_modes)
+
+gen.add("auto_hue", bool_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Automatic hue control.", False)
+
+gen.add("hue", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Hue, degrees.", 0., -180., 180.)
+
+gen.add("saturation", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Saturation, device dependent (zero for grayscale).", 0, 0, 65536)
+
+gen.add("sharpness", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Image sharpness, device dependent.",
+ 0, 0, 65536)
+
+# TODO: check range definition
+gen.add("gamma", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Gamma.", 1.0, 0.01, 5.0)
+
+gen.add("auto_white_balance", bool_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Automatic white balance.", False)
+
+gen.add("white_balance_temperature", int_t, SensorLevels.RECONFIGURE_RUNNING,
+ "White balance temperature, degrees.", 0, 0, 65536)
+
+gen.add("white_balance_BU", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Blue or U component of white balance, device-dependent.",
+ 0, 0, 65536)
+
+gen.add("white_balance_RV", double_t, SensorLevels.RECONFIGURE_RUNNING,
+ "Red or V component of white balance, device-dependent.",
+ 0, 0, 65536)
+
+# TODO: digital multiplier {,limit}
+
+# TODO: analog video standard, analog video lock
+
+exit(gen.generate(PACKAGE, "libuvc_camera", "UVCCamera"))
diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a4280795e14ec8a634eb8861c3e3910437f7ff4a
--- /dev/null
+++ b/libuvc_camera/include/libuvc_camera/camera_driver.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <libuvc/libuvc.h>
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/camera_publisher.h>
+#include <dynamic_reconfigure/server.h>
+#include <boost/thread/mutex.hpp>
+
+#include <libuvc_camera/UVCCameraConfig.h>
+
+namespace libuvc_camera {
+
+class CameraDriver {
+public:
+ CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh);
+ ~CameraDriver();
+
+ bool Start();
+ void Stop();
+
+private:
+ enum State {
+ kInitial = 0,
+ kStopped = 1,
+ kRunning = 2,
+ };
+
+ void OpenCamera(UVCCameraConfig &new_config);
+ void CloseCamera();
+
+ // Accept a reconfigure request from a client
+ void ReconfigureCallback(UVCCameraConfig &config, uint32_t level);
+ // Accept changes in values of automatically updated controls
+ void AutoControlsCallback();
+ static void AutoControlsCallbackAdapter(void *ptr);
+ // Accept a new image frame from the camera
+ void ImageCallback(uvc_frame_t *frame);
+ static void ImageCallbackAdapter(uvc_frame_t *frame, void *ptr);
+
+ ros::NodeHandle nh_, priv_nh_;
+
+ State state_;
+ boost::mutex mutex_;
+
+ uvc_context_t *ctx_;
+ uvc_device_t *dev_;
+ uvc_device_handle_t *devh_;
+ uvc_frame_t *rgb_frame_;
+
+ image_transport::ImageTransport it_;
+ image_transport::CameraPublisher cam_pub_;
+
+ dynamic_reconfigure::Server<UVCCameraConfig> config_server_;
+ UVCCameraConfig config_;
+};
+
+};
diff --git a/libuvc_camera/manifest.xml b/libuvc_camera/manifest.xml
index c953fe8943550780ae00658083877d20dc94af0b..8347f7379e6ee82ea086d2f275c13e463e62af4d 100644
--- a/libuvc_camera/manifest.xml
+++ b/libuvc_camera/manifest.xml
@@ -9,6 +9,9 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/libuvc_camera</url>
<depend package="roscpp"/>
+ <depend package="driver_base"/>
+ <depend package="dynamic_reconfigure"/>
+ <depend package="image_transport"/>
<depend package="sensor_msgs"/>
<depend package="libuvc"/>
</package>
diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c882a342cea000ae6d72a60a30d9de83000c288
--- /dev/null
+++ b/libuvc_camera/src/camera_driver.cpp
@@ -0,0 +1,208 @@
+#include "libuvc_camera/camera_driver.h"
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <image_transport/camera_publisher.h>
+#include <dynamic_reconfigure/server.h>
+#include <dynamic_reconfigure/SensorLevels.h>
+#include <libuvc/libuvc.h>
+
+namespace libuvc_camera {
+
+CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
+ : nh_(nh), priv_nh_(priv_nh),
+ state_(kInitial),
+ ctx_(NULL), dev_(NULL), devh_(NULL), rgb_frame_(NULL),
+ it_(nh_),
+ config_server_(priv_nh_) {
+ cam_pub_ = it_.advertiseCamera("image_raw", 1, false);
+}
+
+CameraDriver::~CameraDriver() {
+ if (rgb_frame_)
+ uvc_free_frame(rgb_frame_);
+
+ if (ctx_)
+ uvc_exit(ctx_); // Destroys dev_, devh_, etc.
+}
+
+bool CameraDriver::Start() {
+ uvc_error_t err;
+
+ err = uvc_init(&ctx_, NULL);
+
+ if (err != UVC_SUCCESS) {
+ uvc_perror(err, "ERROR: uvc_init");
+ return false;
+ }
+
+ state_ = kStopped;
+
+ config_server_.setCallback(boost::bind(&CameraDriver::ReconfigureCallback, this, _1, _2));
+
+ return state_ == kRunning;
+}
+
+void CameraDriver::Stop() {
+ boost::mutex::scoped_lock(mutex_);
+
+ if (state_ == kRunning) {
+ CloseCamera();
+ state_ = kInitial;
+ }
+}
+
+void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) {
+ boost::mutex::scoped_lock(mutex_);
+
+ if (level & dynamic_reconfigure::SensorLevels::RECONFIGURE_CLOSE) {
+ if (state_ == kRunning)
+ CloseCamera();
+ }
+
+ if (state_ == kStopped) {
+ OpenCamera(new_config);
+ }
+
+ if (state_ == kRunning) {
+ // TODO: scanning_mode
+ // TODO: auto_exposure
+ // TODO: auto_exposure_priority
+ // TODO: exposure_absolute
+ // TODO: iris_absolute
+ // TODO: auto_focus
+ // TODO: focus_absolute
+ // TODO: pan_absolute
+ // TODO: tilt_absolute
+ // TODO: roll_absolute
+ // TODO: privacy
+ // TODO: backlight_compensation
+ // TODO: brightness
+ // TODO: contrast
+ // TODO: gain
+ // TODO: power_line_frequency
+ // TODO: auto_hue
+ // TODO: saturation
+ // TODO: sharpness
+ // TODO: gamma
+ // TODO: auto_white_balance
+ // TODO: white_balance_temperature
+ // TODO: white_balance_BU
+ // TODO: white_balance_RV
+ }
+
+ config_ = new_config;
+}
+
+void CameraDriver::ImageCallback(uvc_frame_t *frame) {
+ boost::mutex::scoped_lock(mutex_);
+
+ assert(state_ == kRunning);
+ assert(rgb_frame_);
+
+ uvc_error_t conv_ret = uvc_any2rgb(frame, rgb_frame_);
+
+ if (conv_ret != UVC_SUCCESS) {
+ uvc_perror(conv_ret, "Couldn't convert frame to RGB");
+ return;
+ }
+
+ sensor_msgs::Image image;
+ image.width = config_.width;
+ image.height = config_.height;
+ image.encoding = "rgb8";
+ image.step = image.width * 3;
+ 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());
+}
+
+/* static */ void CameraDriver::ImageCallbackAdapter(uvc_frame_t *frame, void *ptr) {
+ CameraDriver *driver = static_cast<CameraDriver*>(ptr);
+
+ driver->ImageCallback(frame);
+}
+
+void CameraDriver::AutoControlsCallback() {
+ boost::mutex::scoped_lock(mutex_);
+
+}
+
+/* static */ void CameraDriver::AutoControlsCallbackAdapter(void *ptr) {
+ CameraDriver *driver = static_cast<CameraDriver*>(ptr);
+
+ driver->AutoControlsCallback();
+}
+
+void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
+ assert(state_ == kStopped);
+
+ int vendor_id = strtol(new_config.vendor.c_str(), NULL, 0);
+ int product_id = strtol(new_config.product.c_str(), NULL, 0);
+
+ ROS_INFO("Opening camera with vendor=0x%x, product=0x%x, serial=\"%s\", index=%d",
+ vendor_id, product_id, new_config.serial.c_str(), new_config.index);
+
+ uvc_error_t find_err = uvc_find_device(
+ ctx_, &dev_,
+ vendor_id,
+ product_id,
+ new_config.serial.empty() ? NULL : new_config.serial.c_str());
+
+ // TODO: index
+
+ if (find_err != UVC_SUCCESS) {
+ uvc_perror(find_err, "uvc_find_device");
+ return;
+ }
+
+ uvc_error_t open_err = uvc_open(dev_, &devh_);
+ if (open_err != UVC_SUCCESS) {
+ uvc_perror(open_err, "uvc_open");
+ uvc_unref_device(dev_);
+ return;
+ }
+
+ uvc_stream_ctrl_t ctrl;
+ uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
+ devh_, &ctrl,
+ UVC_COLOR_FORMAT_UNCOMPRESSED,
+ new_config.width, new_config.height,
+ new_config.frame_rate);
+
+ if (mode_err != UVC_SUCCESS) {
+ uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
+ uvc_close(devh_);
+ uvc_unref_device(dev_);
+ return;
+ }
+
+ uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this);
+
+ if (stream_err != UVC_SUCCESS) {
+ uvc_perror(stream_err, "uvc_start_iso_streaming");
+ uvc_close(devh_);
+ uvc_unref_device(dev_);
+ return;
+ }
+
+ rgb_frame_ = uvc_allocate_frame(new_config.width * new_config.height * 3);
+ assert(rgb_frame_);
+
+ state_ = kRunning;
+}
+
+void CameraDriver::CloseCamera() {
+ assert(state_ == kRunning);
+
+ uvc_close(devh_);
+ devh_ = NULL;
+
+ uvc_unref_device(dev_);
+ dev_ = NULL;
+
+ state_ = kStopped;
+}
+
+};
diff --git a/libuvc_camera/src/libuvc_camera.cpp b/libuvc_camera/src/libuvc_camera.cpp
deleted file mode 100644
index 58f52194f0916b5fae63cd77825b97fcbab84eff..0000000000000000000000000000000000000000
--- a/libuvc_camera/src/libuvc_camera.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/** @file test_ros_ctrls.cpp Example/test usage of libuvc */
-#include <ros/ros.h>
-#include <sensor_msgs/Image.h>
-#include "libuvc/libuvc.h"
-
-using std::string;
-
-ros::Publisher pub;
-
-void cb(uvc_frame_t *frame) {
- static uvc_frame_t *rgb_frame = NULL;
- uvc_error_t uvc_ret;
-
- if (!rgb_frame)
- rgb_frame = uvc_allocate_frame(frame->width * frame->height * 3);
-
- uvc_ret = uvc_any2rgb(frame, rgb_frame);
-
- if (uvc_ret) {
- uvc_perror(uvc_ret, "Couldn't convert frame to RGB");
- return;
- }
-
- sensor_msgs::Image image;
- image.width = rgb_frame->width;
- image.height = rgb_frame->height;
- image.encoding = "rgb8";
- image.step = image.width * 3;
- image.data.resize(image.step * image.height);
- memcpy(&(image.data[0]), rgb_frame->data, rgb_frame->data_bytes);
-
- pub.publish(image);
-}
-
-int main (int argc, char **argv) {
- ros::init(argc, argv, "libuvc_camera");
- ros::NodeHandle nh;
- ros::NodeHandle priv_nh("~");
-
- pub = nh.advertise<sensor_msgs::Image>("image_raw", 1);
-
- uvc_context_t *ctx;
- uvc_error_t res;
- uvc_device_t *dev;
- uvc_device_handle_t *devh;
- uvc_stream_ctrl_t ctrl;
-
- res = uvc_init(&ctx, NULL);
-
- if (res < 0) {
- uvc_perror(res, "uvc_init");
- return res;
- }
-
- puts("UVC initialized");
-
- int vendor_id;
- string vendor_id_str;
- priv_nh.getParam("vendor", vendor_id_str);
- vendor_id = strtol(vendor_id_str.c_str(), NULL, 0);
-
- int product_id;
- string product_id_str;
- priv_nh.getParam("product", product_id_str);
- product_id = strtol(product_id_str.c_str(), NULL, 0);
-
- string serial_num;
- priv_nh.getParam("serial", serial_num);
-
- printf("pid: %d\n", product_id);
- printf("vid: %d\n", vendor_id);
-
- res = uvc_find_device(
- ctx, &dev,
- vendor_id,
- product_id,
- serial_num.empty() ? NULL : serial_num.c_str());
-
- if (res < 0) {
- uvc_perror(res, "uvc_find_device");
- } else {
- res = uvc_open(dev, &devh);
- if (res < 0) {
- uvc_perror(res, "uvc_open");
- } else {
- puts("Device opened");
-
- uvc_print_diag(devh, stderr);
-
- res = uvc_get_stream_ctrl_format_size(
- devh, &ctrl,
- UVC_COLOR_FORMAT_UNCOMPRESSED, 640, 480, 30);
-
- uvc_print_stream_ctrl(&ctrl, stderr);
-
- if (res < 0) {
- uvc_perror(res, "get_mode");
- } else {
- res = uvc_start_iso_streaming(devh, &ctrl, cb);
-
- if (res < 0) {
- uvc_perror(res, "start_streaming");
- } else {
- puts("Streaming...");
- ros::spin();
- uvc_stop_streaming(devh);
- }
- }
-
- uvc_close(devh);
- puts("Device closed");
- }
- }
-
- uvc_exit(ctx);
- puts("UVC exited");
-
- return 0;
-}
-
diff --git a/libuvc_camera/src/main.cpp b/libuvc_camera/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48bb9175457f75accde7add334d7de4203695406
--- /dev/null
+++ b/libuvc_camera/src/main.cpp
@@ -0,0 +1,20 @@
+#include <ros/ros.h>
+
+#include "libuvc_camera/camera_driver.h"
+
+int main (int argc, char **argv) {
+ ros::init(argc, argv, "libuvc_camera");
+ ros::NodeHandle nh;
+ ros::NodeHandle priv_nh("~");
+
+ libuvc_camera::CameraDriver driver(nh, priv_nh);
+
+ if (!driver.Start())
+ return -1;
+
+ ros::spin();
+
+ driver.Stop();
+
+ return 0;
+}