usb_cam_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2014, Robert Bosch LLC.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Robert Bosch nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
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   // private ROS node handle
00049   ros::NodeHandle node_;
00050 
00051   // shared image message
00052   sensor_msgs::Image img_;
00053   image_transport::CameraPublisher image_pub_;
00054 
00055   // parameters
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     // advertise the main image topic
00068     image_transport::ImageTransport it(node_);
00069     image_pub_ = it.advertiseCamera("image_raw", 1);
00070 
00071     // grab the parameters
00072     node_.param("video_device", video_device_name_, std::string("/dev/video0"));
00073     node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
00074     node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
00075     node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
00076     node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
00077     // possible values: mmap, read, userptr
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     // possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
00083     node_.param("pixel_format", pixel_format_name_, std::string("mjpeg"));
00084     // enable/disable autofocus
00085     node_.param("autofocus", autofocus_, false);
00086     node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
00087     // enable/disable autoexposure
00088     node_.param("autoexposure", autoexposure_, true);
00089     node_.param("exposure", exposure_, 100);
00090     node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
00091     // enable/disable auto white balance temperature
00092     node_.param("auto_white_balance", auto_white_balance_, true);
00093     node_.param("white_balance", white_balance_, 4000);
00094 
00095     // load the camera info
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     // check for default camera info
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     // set the IO method
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     // set the pixel format
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     // start the camera
00134     cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
00135                      image_height_, framerate_);
00136 
00137     // set camera parameters
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     // check auto white balance
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     // check auto exposure
00175     if (!autoexposure_)
00176     {
00177       // turn down exposure control (from max of 3)
00178       cam_.set_v4l_parameter("exposure_auto", 1);
00179       // change the exposure level
00180       cam_.set_v4l_parameter("exposure_absolute", exposure_);
00181     }
00182 
00183     // check auto focus
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     // grab the image
00207     cam_.grab_image(&img_);
00208 
00209     // grab the camera info
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     // publish the image
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 }


usb_cam
Author(s): Benjamin Pitzer
autogenerated on Fri Feb 12 2016 00:27:27