00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <usb_cam/usb_cam.h>
00039 #include <image_transport/image_transport.h>
00040 #include <camera_info_manager/camera_info_manager.h>
00041 #include <sstream>
00042
00043 namespace usb_cam {
00044
00045 class UsbCamNode
00046 {
00047 public:
00048
00049 ros::NodeHandle node_;
00050
00051
00052 sensor_msgs::Image img_;
00053 image_transport::CameraPublisher image_pub_;
00054
00055
00056 std::string video_device_name_, io_method_name_, pixel_format_name_, camera_name_, camera_info_url_;
00057 int image_width_, image_height_, framerate_, exposure_, brightness_, contrast_, saturation_, sharpness_, focus_,
00058 white_balance_, gain_;
00059 bool autofocus_, autoexposure_, auto_white_balance_;
00060 boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
00061
00062 UsbCam cam_;
00063
00064 UsbCamNode() :
00065 node_("~")
00066 {
00067
00068 image_transport::ImageTransport it(node_);
00069 image_pub_ = it.advertiseCamera("image_raw", 1);
00070
00071
00072 node_.param("video_device", video_device_name_, std::string("/dev/video0"));
00073 node_.param("brightness", brightness_, -1);
00074 node_.param("contrast", contrast_, -1);
00075 node_.param("saturation", saturation_, -1);
00076 node_.param("sharpness", sharpness_, -1);
00077
00078 node_.param("io_method", io_method_name_, std::string("mmap"));
00079 node_.param("image_width", image_width_, 640);
00080 node_.param("image_height", image_height_, 480);
00081 node_.param("framerate", framerate_, 30);
00082
00083 node_.param("pixel_format", pixel_format_name_, std::string("mjpeg"));
00084
00085 node_.param("autofocus", autofocus_, false);
00086 node_.param("focus", focus_, -1);
00087
00088 node_.param("autoexposure", autoexposure_, true);
00089 node_.param("exposure", exposure_, 100);
00090 node_.param("gain", gain_, -1);
00091
00092 node_.param("auto_white_balance", auto_white_balance_, true);
00093 node_.param("white_balance", white_balance_, 4000);
00094
00095
00096 node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
00097 node_.param("camera_name", camera_name_, std::string("head_camera"));
00098 node_.param("camera_info_url", camera_info_url_, std::string(""));
00099 cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
00100
00101 if (!cinfo_->isCalibrated())
00102 {
00103 cinfo_->setCameraName(video_device_name_);
00104 sensor_msgs::CameraInfo camera_info;
00105 camera_info.header.frame_id = img_.header.frame_id;
00106 camera_info.width = image_width_;
00107 camera_info.height = image_height_;
00108 cinfo_->setCameraInfo(camera_info);
00109 }
00110
00111
00112 ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(), video_device_name_.c_str(),
00113 image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);
00114
00115
00116 UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
00117 if(io_method == UsbCam::IO_METHOD_UNKNOWN)
00118 {
00119 ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
00120 node_.shutdown();
00121 return;
00122 }
00123
00124
00125 UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
00126 if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
00127 {
00128 ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
00129 node_.shutdown();
00130 return;
00131 }
00132
00133
00134 cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
00135 image_height_, framerate_);
00136
00137
00138 if (brightness_ >= 0)
00139 {
00140 cam_.set_v4l_parameter("brightness", brightness_);
00141 }
00142
00143 if (contrast_ >= 0)
00144 {
00145 cam_.set_v4l_parameter("contrast", contrast_);
00146 }
00147
00148 if (saturation_ >= 0)
00149 {
00150 cam_.set_v4l_parameter("saturation", saturation_);
00151 }
00152
00153 if (sharpness_ >= 0)
00154 {
00155 cam_.set_v4l_parameter("sharpness", sharpness_);
00156 }
00157
00158 if (gain_ >= 0)
00159 {
00160 cam_.set_v4l_parameter("gain", gain_);
00161 }
00162
00163
00164 if (auto_white_balance_)
00165 {
00166 cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
00167 }
00168 else
00169 {
00170 cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
00171 cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
00172 }
00173
00174
00175 if (!autoexposure_)
00176 {
00177
00178 cam_.set_v4l_parameter("exposure_auto", 1);
00179
00180 cam_.set_v4l_parameter("exposure_absolute", exposure_);
00181 }
00182
00183
00184 if (autofocus_)
00185 {
00186 cam_.set_auto_focus(1);
00187 cam_.set_v4l_parameter("focus_auto", 1);
00188 }
00189 else
00190 {
00191 cam_.set_v4l_parameter("focus_auto", 0);
00192 if (focus_ >= 0)
00193 {
00194 cam_.set_v4l_parameter("focus_absolute", focus_);
00195 }
00196 }
00197 }
00198
00199 virtual ~UsbCamNode()
00200 {
00201 cam_.shutdown();
00202 }
00203
00204 bool take_and_send_image()
00205 {
00206
00207 cam_.grab_image(&img_);
00208
00209
00210 sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00211 ci->header.frame_id = img_.header.frame_id;
00212 ci->header.stamp = img_.header.stamp;
00213
00214
00215 image_pub_.publish(img_, *ci);
00216
00217 return true;
00218 }
00219
00220 bool spin()
00221 {
00222 ros::Rate loop_rate(this->framerate_);
00223 while (node_.ok())
00224 {
00225 if (!take_and_send_image())
00226 ROS_WARN("USB camera did not respond in time.");
00227 ros::spinOnce();
00228 loop_rate.sleep();
00229 }
00230 return true;
00231 }
00232 };
00233
00234 }
00235
00236 int main(int argc, char **argv)
00237 {
00238 ros::init(argc, argv, "usb_cam");
00239 usb_cam::UsbCamNode a;
00240 a.spin();
00241 return EXIT_SUCCESS;
00242 }