Commit 3ddfc561 authored by Ken Tossell's avatar Ken Tossell
Browse files

beginning of driver with controls, dyn_reconf, caminfo

parent a0e312bf
......@@ -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)
#! /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"))
#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_;
};
};
......@@ -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>
......
#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;
}
};
/** @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;
}
#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;