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