usb_cam_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2009, 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 #include <stdio.h>
00037 #include <iostream>
00038 
00039 #include <ros/ros.h>
00040 #include <sensor_msgs/fill_image.h>
00041 #include <usb_cam/usb_cam.h>
00042 #include <self_test/self_test.h>
00043 #include <image_transport/image_transport.h>
00044 
00045 class UsbCamNode
00046 {
00047 public:
00048   ros::NodeHandle node_;
00049   sensor_msgs::Image img_;
00050 
00051   std::string video_device_name_;
00052   std::string io_method_name_;
00053   int image_width_,image_height_;
00054   std::string pixel_format_name_;
00055   bool autofocus_;
00056 
00057   sensor_msgs::CameraInfo info_;
00058 
00059   ros::Time next_time_;
00060   int count_;
00061 
00062   usb_cam_camera_image_t* camera_image_;
00063 
00064   image_transport::CameraPublisher image_pub_;
00065 
00066   UsbCamNode() :
00067       node_("~")
00068   {
00069     image_transport::ImageTransport it(node_);
00070     image_pub_ = it.advertiseCamera("image_raw", 1);
00071 
00072     node_.param("video_device", video_device_name_, std::string("/dev/video0"));
00073     node_.param("io_method", io_method_name_, std::string("mmap")); // possible values: mmap, read, userptr
00074     node_.param("image_width", image_width_, 640);
00075     node_.param("image_height", image_height_, 480);
00076     node_.param("pixel_format", pixel_format_name_, std::string("mjpeg")); // possible values: yuyv, uyvy, mjpeg
00077     node_.param("autofocus", autofocus_, false); // enable/disable autofocus
00078 
00079     {
00080       XmlRpc::XmlRpcValue double_list;
00081       info_.height = image_height_;
00082       info_.width = image_width_;
00083 
00084       node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
00085       info_.header.frame_id = img_.header.frame_id;
00086 
00087       node_.getParam("K", double_list);
00088       if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
00089           (double_list.size() == 9)) {
00090         for (int i=0; i<9; i++) {
00091           ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00092           info_.K[i] = double_list[i];
00093         }
00094       }
00095 
00096       node_.getParam("D", double_list);
00097 
00098       if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray)) {
00099         info_.D.resize(double_list.size());
00100         for (int i=0; i<double_list.size(); i++) {
00101           ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00102           info_.D[i] = double_list[i];
00103         }
00104       }
00105 
00106       node_.getParam("R", double_list);
00107 
00108       if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
00109           (double_list.size() == 9)) {
00110         for (int i=0; i<9; i++) {
00111           ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00112           info_.R[i] = double_list[i];
00113         }
00114       }
00115 
00116       node_.getParam("P", double_list);
00117 
00118       if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
00119           (double_list.size() == 12)) {
00120         for (int i=0; i<12; i++) {
00121           ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00122           info_.P[i] = double_list[i];
00123         }
00124       }
00125     }
00126 
00127     printf("usb_cam video_device set to [%s]\n", video_device_name_.c_str());
00128     printf("usb_cam io_method set to [%s]\n", io_method_name_.c_str());
00129     printf("usb_cam image_width set to [%d]\n", image_width_);
00130     printf("usb_cam image_height set to [%d]\n", image_height_);
00131     printf("usb_cam pixel_format set to [%s]\n", pixel_format_name_.c_str());
00132     printf("usb_cam auto_focus set to [%d]\n", autofocus_);
00133 
00134     usb_cam_io_method io_method;
00135     if(io_method_name_ == "mmap")
00136       io_method = IO_METHOD_MMAP;
00137     else if(io_method_name_ == "read")
00138       io_method = IO_METHOD_READ;
00139     else if(io_method_name_ == "userptr")
00140       io_method = IO_METHOD_USERPTR;
00141     else {
00142       ROS_FATAL("Unknown io method.");
00143       node_.shutdown();
00144       return;
00145     }
00146 
00147     usb_cam_pixel_format pixel_format;
00148     if(pixel_format_name_ == "yuyv")
00149       pixel_format = PIXEL_FORMAT_YUYV;
00150     else if(pixel_format_name_ == "uyvy")
00151       pixel_format = PIXEL_FORMAT_UYVY;
00152     else if(pixel_format_name_ == "mjpeg") {
00153       pixel_format = PIXEL_FORMAT_MJPEG;
00154     }
00155     else {
00156       ROS_FATAL("Unknown pixel format.");
00157       node_.shutdown();
00158       return;
00159     }
00160 
00161     camera_image_ = usb_cam_camera_start(video_device_name_.c_str(),
00162         io_method,
00163         pixel_format,
00164         image_width_,
00165         image_height_);
00166 
00167     if(autofocus_) {
00168       usb_cam_camera_set_auto_focus(1);
00169     }
00170 
00171     next_time_ = ros::Time::now();
00172     count_ = 0;
00173   }
00174 
00175   virtual ~UsbCamNode()
00176   {
00177 
00178     usb_cam_camera_shutdown();
00179   }
00180 
00181   bool take_and_send_image()
00182   {
00183     usb_cam_camera_grab_image(camera_image_);
00184     fillImage(img_, "rgb8", camera_image_->height, camera_image_->width, 3 * camera_image_->width, camera_image_->image);
00185 
00186     img_.header.stamp = ros::Time::now();
00187     info_.header.stamp = img_.header.stamp;
00188     image_pub_.publish(img_, info_);
00189     return true;
00190   }
00191 
00192 
00193   bool spin()
00194   {
00195     while (node_.ok())
00196     {
00197       if (take_and_send_image())
00198       {
00199         count_++;
00200         ros::Time now_time = ros::Time::now();
00201         if (now_time > next_time_) {
00202           std::cout << count_ << " frames/sec at " << now_time << std::endl;
00203           count_ = 0;
00204           next_time_ = next_time_ + ros::Duration(1,0);
00205         }
00206       } else {
00207         ROS_ERROR("couldn't take image.");
00208         usleep(1000000);
00209       }
00210 //      self_test_.checkTest();
00211     }
00212     return true;
00213   }
00214 };
00215 
00216 int main(int argc, char **argv)
00217 {
00218   ros::init(argc, argv, "usb_cam");
00219   UsbCamNode a;
00220   a.spin();
00221   return 0;
00222 }


usb_cam
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:09:47