beginning of driver with controls, dyn_reconf, caminfo

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)
 

libuvc_camera/cfg/UVCCamera.cfg

+#! /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"))

libuvc_camera/include/libuvc_camera/camera_driver.h

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

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>

libuvc_camera/src/camera_driver.cpp

+#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;
+}
+
+};

libuvc_camera/src/libuvc_camera.cpp

-/** @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;
-}
-

libuvc_camera/src/main.cpp

+#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;
+}