UsbCamNode() :
node_("~")
{
// specify settings for images and camera for Realsense camera
image_width_ = 640;
image_height_ = 480;
framerate_ = 30;
// advertise the main image topic
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 1);
// grab the parameters
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
// possible values: mmap, read, userptr
node_.param("io_method", io_method_name_, std::string("mmap"));
// load the camera info
node_.param("camera_frame_id", img_.header.frame_id, std::string("realsense_camera"));
node_.param("camera_name", camera_name_, std::string("realsense_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
// check for default camera info
if (camera_info_url_.size() == 0)
{
cinfo_->setCameraName(video_device_name_);
sensor_msgs::CameraInfo camera_info;
camera_info.header.frame_id = img_.header.frame_id;
camera_info.width = image_width_;
camera_info.height = image_height_;
cinfo_->setCameraInfo(camera_info);
}
ROS_INFO("Starting '%s' (%s) at %dx%d, %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
image_width_, image_height_, framerate_);
// set the IO method
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
if(io_method == UsbCam::IO_METHOD_UNKNOWN)
{
ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
node_.shutdown();
return;
}
// start the camera
cam_.start(video_device_name_.c_str(), io_method, image_width_, image_height_, framerate_);
}
UsbCamNode() :
node_("~")
{
// advertise the main image topic
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 1);
// grab the parameters
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
// possible values: mmap, read, userptr
node_.param("io_method", io_method_name_, std::string("mmap"));
node_.param("image_width", image_width_, 800);
node_.param("image_height", image_height_, 600);
node_.param("framerate", framerate_, 20);
// possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
node_.param("pixel_format", pixel_format_name_, std::string("yuyv"));
// enable/disable autofocus
node_.param("autofocus", autofocus_, false);
node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
// enable/disable autoexposure
node_.param("autoexposure", autoexposure_, true);
node_.param("exposure", exposure_, 100);
node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
// enable/disable auto white balance temperature
node_.param("auto_white_balance", auto_white_balance_, true);
node_.param("white_balance", white_balance_, 4000);
// load the camera info
node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
node_.param("camera_name", camera_name_, std::string("head_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
// check for default camera info
if (!cinfo_->isCalibrated())
{
cinfo_->setCameraName(video_device_name_);
sensor_msgs::CameraInfo camera_info;
camera_info.header.frame_id = img_.header.frame_id;
camera_info.width = image_width_;
camera_info.height = image_height_;
cinfo_->setCameraInfo(camera_info);
}
ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);
// set the IO method
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
if(io_method == UsbCam::IO_METHOD_UNKNOWN)
{
ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
node_.shutdown();
return;
}
// set the pixel format
UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
{
ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
node_.shutdown();
return;
}
// start the camera
cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
image_height_, framerate_);
// set camera parameters
if (brightness_ >= 0)
{
cam_.set_v4l_parameter("brightness", brightness_);
}
if (contrast_ >= 0)
{
cam_.set_v4l_parameter("contrast", contrast_);
}
if (saturation_ >= 0)
{
cam_.set_v4l_parameter("saturation", saturation_);
}
if (sharpness_ >= 0)
{
cam_.set_v4l_parameter("sharpness", sharpness_);
}
if (gain_ >= 0)
{
cam_.set_v4l_parameter("gain", gain_);
}
// check auto white balance
if (auto_white_balance_)
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
}
else
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
}
// check auto exposure
if (!autoexposure_)
{
// turn down exposure control (from max of 3)
cam_.set_v4l_parameter("exposure_auto", 1);
// change the exposure level
cam_.set_v4l_parameter("exposure_absolute", exposure_);
}
// check auto focus
if (autofocus_)
{
cam_.set_auto_focus(1);
cam_.set_v4l_parameter("focus_auto", 1);
}
else
{
cam_.set_v4l_parameter("focus_auto", 0);
if (focus_ >= 0)
{
cam_.set_v4l_parameter("focus_absolute", focus_);
}
}
}
