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 #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"));
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"));
00077 node_.param("autofocus", autofocus_, false);
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
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 }
