sender.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Morgan Quigley, Clemens Eppner, Tully Foote
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <cstdio>
00031 #include <ros/ros.h>
00032 #include <ros/time.h>
00033 #include "uvc_cam/uvc_cam.h"
00034 #include "sensor_msgs/Image.h"
00035 #include "sensor_msgs/image_encodings.h"
00036 #include <sensor_msgs/CameraInfo.h>
00037 #include <sensor_msgs/SetCameraInfo.h>
00038 #include <image_transport/image_transport.h>
00039 
00040 sensor_msgs::CameraInfo cam_info;
00041 ros::ServiceServer set_camera_info_service;
00042 using namespace std;
00043 
00044 template<class T>
00045 string arrayToString(T v) {
00046         stringstream ss;
00047         for (size_t i = 0; i < v.size(); i++) {
00048                 ss << v[i] << " ";
00049         }
00050         return (ss.str());
00051 }
00052 
00053 template<class T>
00054 void restoreVec(string s, T &vec) {
00055         stringstream ss(s);
00056         double a = 0;
00057         int i = 0;
00058         while (ss >> a && vec.size()>i) {
00059                 vec[i++] = a;
00060         }
00061 }
00062 
00063 void load_params(ros::NodeHandle &nh) {
00064         string str_calib;
00065         stringstream ss_calib;
00066 
00067         nh.param("D", str_calib, string(""));
00068         restoreVec(str_calib, cam_info.D);
00069 
00070         nh.param("K", str_calib, string(""));
00071         restoreVec(str_calib, cam_info.K);
00072 
00073         nh.param("R", str_calib, string(""));
00074         restoreVec(str_calib, cam_info.R);
00075 
00076         nh.param("P", str_calib, string(""));
00077         restoreVec(str_calib, cam_info.P);
00078         cam_info.header.frame_id = "/camera";
00079 
00080         cout
00081                         << "Current cam_info:"
00082                         << endl;
00083         cout << "    <param name=\"D\" type=\"string\" value=\"" << arrayToString(
00084                         cam_info.D) << "\"/>" << endl;
00085         cout << "    <param name=\"K\" type=\"string\" value=\"" << arrayToString(
00086                         cam_info.K) << "\"/>" << endl;
00087         cout << "    <param name=\"R\" type=\"string\" value=\"" << arrayToString(
00088                         cam_info.R) << "\"/>" << endl;
00089         cout << "    <param name=\"P\" type=\"string\" value=\"" << arrayToString(
00090                         cam_info.P) << "\"/>" << endl;
00091 }
00092 
00093 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req,
00094                 sensor_msgs::SetCameraInfo::Response &rsp) {
00095         ROS_INFO("setCameraInfo: received new camera parameters");
00096 
00097         cam_info = req.camera_info;
00098 
00099         cout
00100                         << "Add these lines to your camera.launch file to make the changes permanent:"
00101                         << endl;
00102         cout << "    <param name=\"D\" type=\"string\" value=\"" << arrayToString(
00103                         cam_info.D) << "\"/>" << endl;
00104         cout << "    <param name=\"K\" type=\"string\" value=\"" << arrayToString(
00105                         cam_info.K) << "\"/>" << endl;
00106         cout << "    <param name=\"R\" type=\"string\" value=\"" << arrayToString(
00107                         cam_info.R) << "\"/>" << endl;
00108         cout << "    <param name=\"P\" type=\"string\" value=\"" << arrayToString(
00109                         cam_info.P) << "\"/>" << endl;
00110 
00111         return 1;
00112 }
00113 
00114 int main(int argc, char **argv) {
00115         ros::init(argc, argv, "uvc_cam");
00116         ros::NodeHandle n;
00117         ros::NodeHandle n_private("~");
00118 
00119         std::string device;
00120         std::string camera_ns;
00121         n_private.param<std::string> ("device", device, "/dev/video0");
00122         n_private.param<std::string> ("camera", camera_ns, "/camera");
00123         int width, height, fps, modetect_lum, modetect_count;
00124         n_private.param("width", width, 640);
00125         n_private.param("height", height, 480);
00126         n_private.param("fps", fps, 30);
00127         n_private.param("motion_threshold_luminance", modetect_lum, 100);
00128         n_private.param("motion_threshold_count", modetect_count, -1);
00129 
00130         load_params(n_private);
00131         ros::NodeHandle ns_camera(camera_ns);
00132         image_transport::ImageTransport it(ns_camera);
00133         image_transport::CameraPublisher camera_publisher = it.advertiseCamera("image_raw", 1);
00134 
00135         set_camera_info_service = ns_camera.advertiseService("set_camera_info",
00136                         &setCameraInfo);
00137 
00138         ROS_INFO("opening uvc_cam at %dx%d, %d fps", width, height, fps);
00139         uvc_cam::Cam
00140                         cam(device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00141         cam.set_motion_thresholds(modetect_lum, modetect_count);
00142 
00143         ros::Time t_prev(ros::Time::now());
00144         int count = 0, skip_count = 0;
00145 
00146         ros::Rate loop_rate(fps);
00147 
00148         while (n.ok()) {
00149                 unsigned char *frame = NULL;
00150                 uint32_t bytes_used;
00151                 int buf_idx = cam.grab(&frame, bytes_used);
00152                 if (count++ % fps == 0) {
00153                         ros::Time t(ros::Time::now());
00154                         ros::Duration d(t - t_prev);
00155                         ROS_INFO("%.1f fps skip %d", (double)fps / d.toSec(), skip_count);
00156                         t_prev = t;
00157                 }
00158                 if (frame) {
00159                         sensor_msgs::Image image;
00160                         image.header.stamp = ros::Time::now();
00161                         image.encoding = sensor_msgs::image_encodings::RGB8;
00162                         image.height = height;
00163                         image.width = width;
00164                         image.step = 3 * width;
00165 
00166                         image.data.resize(image.step * image.height);
00167                         /*
00168                          uint8_t* bgr = &(image.data[0]);
00169                          for (uint32_t y = 0; y < height; y++)
00170                          for (uint32_t x = 0; x < width; x++)
00171                          {
00172                          // hack... flip bgr to rgb
00173                          uint8_t *p = frame + y * width * 3 + x * 3;
00174                          uint8_t *q = bgr   + y * width * 3 + x * 3;
00175                          q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
00176                          }
00177                          */
00178                         memcpy(&image.data[0], frame, width * height * 3);
00179                         image.header.frame_id="/camera";
00180                         cam_info.width = width;
00181                         cam_info.height = height;
00182                         cam_info.roi.width = width;
00183                         cam_info.roi.height = height;
00184                         cam_info.header = image.header;
00185 
00186                         camera_publisher.publish(image, cam_info);
00187                         cam.release(buf_idx);
00188                 } else
00189                         skip_count++;
00190 
00191                 ros::spinOnce();
00192                 loop_rate.sleep();
00193         }
00194         return 0;
00195 }
00196 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


uvc_cam2
Author(s): Morgan Quigley, extended and maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:00