Hi all,
I am using two cameras and I have a .yaml file having the parameter list like fps, Width and length of the image, Pixels and few more.
I have created a parameter server and have stored the parameters to access it in real time.
The problem is that I am able to change the parameters in real time but is not getting reflected in the camera output feed.
For example...If my camera is producing output at 30 fps and I have changed my fps to 10 in real time so there is a significant difference between the fps but this is not being reflected in the output but my paramter server is getting updated.
How to update the camera parameters in real time.
I am using getParam() to read the values.
Below is my Publisher code.
#include "ros/ros.h"
#include "usb_cam/usb_cam.h"
#include "image_transport/image_transport.h"
#include "camera_info_manager/camera_info_manager.h"
#include "sstream"
#include
namespace usb_cam {
class UsbCamNode
{
public:
// private ROS node handle
ros::NodeHandle node_;
// shared image message
sensor_msgs::Image img_;
image_transport::CameraPublisher image_pub_;
// parameters
std::string video_device_name_, io_method_name_, pixel_format_name_, camera_name_, camera_info_url_;
//std::string start_service_name_, start_service_name_;
bool streaming_status_;
int image_width_, image_height_, framerate_, exposure_, brightness_, contrast_, saturation_, sharpness_, focus_, white_balance_, gain_;
bool autofocus_, autoexposure_, auto_white_balance_;
boost::shared_ptr<:camerainfomanager> cinfo_;
UsbCam cam_;
ros::ServiceServer service_start_, service_stop_;
bool service_start_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res )
{
cam_.start_capturing();
return true;
}
bool service_stop_cap( std_srvs::Empty::Request &req, std_srvs::Empty::Response &res )
{
cam_.stop_capturing();
return true;
}
UsbCamNode() :
node_("~")
{
// advertise the main image topic
image_transport::ImageTransport it(node_);
image_pub_ = it.advertiseCamera("image_raw", 0);
// 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_.getParam("/io_method", io_method_name_, std::string("mmap"));
node_.getParam("/io_method", io_method_name_);
node_.getParam("/image_width", image_width_);
node_.getParam("/image_height", image_height_);
//node_.param("framerate", framerate_, 20);
node_.getParam("/framerate", framerate_);
// possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
// node_.param("/pixel_format", pixel_format_name_, std::string("mjpeg"));
node_.getParam("/pixel_format", pixel_format_name_);
// enable/disable autofocus
// 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_));
// create Services
service_start_ = node_.advertiseService("start_capture", &UsbCamNode::service_start_cap, this);
service_stop_ = node_.advertiseService("stop_capture", &UsbCamNode::service_stop_cap, this);
// 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_);
}
}
}
virtual ~UsbCamNode()
{
cam_.shutdown();
}
bool take_and_send_image()
{
node_.getParam("/framerate", framerate_);
node_.getParam("/io_method", io_method_name_);
node_.getParam("/pixel_format", pixel_format_name_);
UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
cam_.start_capturing();
ros::Rate loop_rate(/*this->*/framerate_);
ROS_INFO("Frame Rate : %d ", framerate_);
cam_.fps(framerate_);
// grab the image
cam_.grab_image(&img_);
// grab the camera info
sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
ci->header.frame_id = img_.header.frame_id;
ci->header.stamp = img_.header.stamp;
// publish the image
image_pub_.publish(img_, *ci);
return true;
}
bool spin()
{
ros::Rate loop_rate(this->framerate_);
// ROS_INFO("Frame Rate : %d ", framerate_);
while (node_.ok())
{
if (cam_.is_capturing())
{
if (!take_and_send_image())
ROS_WARN("USB camera did not respond in time.");
}
ros::spinOnce();
loop_rate.sleep();
}
return true;
}
};
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "usb_cam");
usb_cam::UsbCamNode a;
a.spin();
return EXIT_SUCCESS;
}
Kindly suggest a method or a better and simple code to update the parameters and publish the data in real time.
Thanks,
