dynamic_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 // Modified 2013 by Morgan Cormier - change to fit the Corobots needs
00031 // Modified Apr 6, 2010 by Adam Leeper - changed to use "image_transport"
00032 // Modified Aug 4, 2010 by Stefan Diewald - changed to fit Whiteboard robots need
00033 
00034 #include <cstdio>
00035 #include <ros/ros.h>
00036 #include <ros/time.h>
00037 #include "uvc_cam/uvc_cam.h"
00038 #include "sensor_msgs/CameraInfo.h"
00039 #include "sensor_msgs/Image.h"
00040 #include "sensor_msgs/CompressedImage.h"
00041 #include "sensor_msgs/image_encodings.h"
00042 #include <camera_calibration_parsers/parse_ini.h>
00043 #include <dynamic_reconfigure/server.h>
00044 #include <corobot_camera/corobot_cameraConfig.h>
00045 #include <diagnostic_updater/diagnostic_updater.h>
00046 #include <diagnostic_updater/publisher.h>
00047 #include "corobot_diagnostics/diagnostics.h"
00048 
00049 #include <image_transport/image_transport.h>
00050 #include <opencv/cvwimage.h>
00051 #include <opencv/highgui.h>
00052 
00053 //If the ROS version is bellow Groovy, as cv_bridge is not present in groovy anymore
00054 #if not (ROS_VERSION_MINIMUM(1, 9, 0)) 
00055         #include <cv_bridge/CvBridge.h>
00056 #endif
00057 
00058 // get the messages
00059 #include "std_msgs/String.h"
00060 #include "corobot_msgs/videomode.h"
00061 #include "corobot_msgs/state.h"
00062 
00063 sensor_msgs::CameraInfo::Ptr camera_info;
00064 ros::Publisher image_pub;
00065 
00066 // the possible states
00067 enum e_state {PLAYING, STOP, CHANGE_MODE};
00068 char state = STOP;
00069 
00070 // variables to change width and height
00071 int new_width, new_height, new_fps;
00072 
00073 // True if the exposure is automatically set
00074 bool auto_exposure = true;
00075 
00076 //  True if the camera is activated
00077 bool camera_activated = true;
00078 
00079 // This is used for the diagnostic function to know what to report to the user
00080 int camera_state = 0;
00081 
00082 // True if we get jpeg images directly from the camera. 
00083 bool isjpeg = false;
00084 
00085 // pointer to cam
00086 uvc_cam::Cam * cam_ptr;
00087 
00088 /*
00089  *      change the state of the video sender:
00090  *
00091  *              - start: stream images
00092  *              - stop:  stop streaming
00093  */
00094 void stateCallback(const corobot_msgs::state::ConstPtr& msg) {
00095         if (msg->state == std::string("start")) {
00096                 state = PLAYING;
00097                 ROS_INFO("Camera starting...");
00098         }
00099         else if (msg->state == std::string("stop")) {
00100                 state = STOP;
00101                 ROS_INFO("Camera stopping...");
00102         }
00103 }
00104 
00105 /*
00106  *      change the image resolution (width, height) and fps
00107  *
00108  *              As there is no error handling built-in at the moment please be sure that you only set available width, height and fps.
00109  */
00110 void videomodeCallback(const corobot_msgs::videomode::ConstPtr& msg) {
00111         new_width = msg->width;
00112         new_height = msg->height;
00113         new_fps = msg->fps;
00114         auto_exposure = msg->auto_exposure;
00115         
00116         ROS_INFO("Receiced new video parameters: %dx%d, %d fps", new_width, new_height, new_fps);
00117 
00118         if ((bool)msg->immediately)
00119                 state = CHANGE_MODE;
00120 }
00121 
00122 /* Huffman table Header for the jpeg image frame */
00123 const static unsigned char dht_data[] = {
00124 0xff, 0xc4, 0x01, 0xa2, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01,
00125 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02,
00126 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x01, 0x00, 0x03,
00127 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00,
00128 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09,
00129 0x0a, 0x0b, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05,
00130 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7d, 0x01, 0x02, 0x03, 0x00, 0x04,
00131 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22,
00132 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42, 0xb1, 0xc1, 0x15,
00133 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16, 0x17,
00134 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x34, 0x35, 0x36,
00135 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a,
00136 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66,
00137 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
00138 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95,
00139 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8,
00140 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2,
00141 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5,
00142 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
00143 0xe8, 0xe9, 0xea, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9,
00144 0xfa, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05,
00145 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04,
00146 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22,
00147 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33,
00148 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25,
00149 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36,
00150 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a,
00151 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66,
00152 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
00153 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94,
00154 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
00155 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba,
00156 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
00157 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
00158 0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
00159 }; 
00160 
00161 
00162 /* Detect if a jpeg frame has the huffman table */    
00163 int is_huffman(unsigned char *buf)
00164 {
00165     unsigned char *ptbuf;
00166     int i = 0;
00167     ptbuf = buf;
00168     while(((ptbuf[0] << 8) | ptbuf[1]) != 0xffda) {
00169         if(i++ > 2048)
00170             return 0;
00171         if(((ptbuf[0] << 8) | ptbuf[1]) == 0xffc4)
00172             return 1;
00173         ptbuf++;
00174     }
00175     return 0;
00176 } 
00177          
00178 /*
00179  *      creates the uvc_cam object and gets the frames in a loop
00180  */
00181 void mainloop(const char* device, int width, int height, int fps, ros::NodeHandle n, image_transport::CameraPublisher pub, diagnostic_updater::Updater& updater)
00182 {
00183         ROS_INFO("Opening uvc_cam at %s with %dx%d, %d fps, auto-exposure: %s", device, width, height, fps, (auto_exposure)?"true":"false");
00184         ros::Rate r(fps);
00185         // instantiate uvc_cam
00186         uvc_cam::Cam::mode_t mode;
00187         if(isjpeg)
00188                 mode = uvc_cam::Cam::MODE_MJPG;
00189         else
00190                 mode = uvc_cam::Cam::MODE_RGB;
00191 
00192         ros::Time t_prev(ros::Time::now());
00193         uint64_t count = 0, skip_count = 0;
00194 
00195         int buf_idx = 0;
00196         unsigned char *frame = NULL;
00197         uint32_t bytes_used;
00198          unsigned int pair_id = 0;
00199 
00200         bool camera_activated = false;
00201         uvc_cam::Cam *cam;
00202         
00203         // Create the image variable, used only if the mode is not jpeg
00204         sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00205         image->height = height;
00206         image->width = width;
00207         image->step = 3 * width;
00208         image->encoding = sensor_msgs::image_encodings::RGB8;
00209         image->data.resize(image->step * image->height);
00210 
00211         // run as long as ROS is running and videomode should not be changed or camera should not be stopped
00212         while (n.ok() && state == PLAYING) {
00213                 // only publish camera images if there is at least one subscriber
00214                 if(pub.getNumSubscribers() > 0 || image_pub.getNumSubscribers() > 0)
00215                 {
00216                         camera_state = 0;
00217                         if (!camera_activated) // We activate the camera only when there is a subscriber
00218                         {
00219                                 try
00220                                 {
00221                                         cam = new uvc_cam::Cam(device, mode, width, height, fps);
00222                                         cam_ptr = cam;
00223                                         camera_activated = true;
00224                                 }
00225                                 catch (std::runtime_error &ex)
00226                                 {
00227                                         camera_state = 4;
00228                                         camera_activated = false;
00229                                 }
00230                         }
00231 
00232                         if (camera_activated) // we can now grab a frame and publish it
00233                         {
00234                                 // get one frame
00235                                 buf_idx = cam->grab(&frame,bytes_used);
00236                                 if (buf_idx == -1)
00237                                         camera_state = 3;
00238         
00239                                 // if we could grab a frame
00240                                 if (frame) 
00241                                 {
00242                                         ros::Time capture_time = ros::Time::now();
00243 
00244                                         image->header.stamp = capture_time;
00245                                         image->header.seq = pair_id;
00246  
00247                                         std::string frameid = "camera";
00248                                         image->header.frame_id = frameid;
00249                     
00250           // If we have a RGB image, copy the data to the image object and publish
00251           if(!isjpeg)
00252           {
00253                                         memcpy(&image->data[0], frame, width*height * 3);
00254 
00255                                             //publish image & camera_info
00256                                             pub.publish(image, camera_info);
00257                       }
00258                       else // else we've got create the compressed image object, add the huffman table header and publish
00259                       {
00260                   sensor_msgs::CompressedImage msg;
00261               msg.header.frame_id.append("/camera");
00262               msg.header.stamp = ros::Time::now();
00263               msg.format = "jpg";
00264               if(!is_huffman(frame)) { // If the data doesn't have huffman, we add it
00265                   unsigned char *ptdeb, *ptlimit, *ptcur = frame;
00266                   int sizein, pos = 0;
00267                       ptdeb = ptcur = frame;
00268                       ptlimit = frame + bytes_used;
00269                   while((((ptcur[0] << 8) | ptcur[1]) != 0xffc0) && (ptcur < ptlimit))
00270                       ptcur++;
00271                   sizein = ptcur - ptdeb;
00272                   msg.data.insert( msg.data.end(), frame, frame + sizein  );
00273                   msg.data.insert( msg.data.end(), dht_data, dht_data + sizeof(dht_data)  );
00274                   msg.data.insert( msg.data.end(), ptcur, ptcur + bytes_used - sizein  );
00275               } 
00276               else 
00277               {
00278                   msg.data.insert( msg.data.end(), frame, frame + bytes_used  );
00279               } 
00280               image_pub.publish(msg);
00281                       }
00282                                         // the camera sets user settings like exposure time around the 50th frame
00283                                         if (count == 50)
00284                                                 ROS_DEBUG("User settings applied.");
00285 
00286                                         // release the frame
00287                                         cam->release(buf_idx);
00288                                 } else // there was no frame
00289                                         skip_count++;
00290                                 }
00291                                 
00292                         }               
00293                 else // no subscribers
00294                 {
00295                         camera_state = 2;
00296                         if (camera_activated) // the camera is activated but no one is subscribed to the topic, so we deactivate the camera
00297                         {
00298                                 delete cam;
00299                                 cam = NULL;
00300                                 cam_ptr = NULL;
00301                                 camera_activated = false;
00302                         }
00303                         
00304                 }
00305                 // for the callbacks
00306                 updater.update(); // update the diagnostic status
00307                 ros::spinOnce();
00308                 r.sleep();
00309         }
00310 }
00311 
00312 void dynamic_reconfigureCallback(corobot_camera::corobot_cameraConfig &config, uint32_t level)
00316 {
00317                 camera_activated = config.camera_activated;
00318 }
00319 
00320 void webcam_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00324 {
00325         if (camera_state == 0)  
00326                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "The camera is working");
00327         else if (camera_state == 1)
00328         {
00329                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN, "The user has stopped the camera. Activate it again to see the camera view");
00330         }
00331         else if (camera_state == 2)
00332         {
00333                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN, "No subscriber to the camera topic - Start corobot_teleop or another image viewer");
00334         }
00335         else if (camera_state == 3)
00336         {
00337                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not grab the image");
00338                 stat.addf("Recommendation", CAMERA_DISCONNECTED);
00339         }
00340         else if (camera_state == 4)
00341         {
00342                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not initialize the camera");
00343                 stat.addf("Recommendation", ERROR_CAMERA_PARAMETERS);
00344         }
00345 
00346 }
00347 
00348 
00349 int main(int argc, char **argv) {
00350         ros::init(argc, argv, "corobot_camera");
00351         ros::NodeHandle n;
00352         ros::NodeHandle n_private("~");
00353         bool immediately;
00354 
00355         dynamic_reconfigure::Server<corobot_camera::corobot_cameraConfig> server;
00356         dynamic_reconfigure::Server<corobot_camera::corobot_cameraConfig>::CallbackType f;
00357 
00358         f = boost::bind(&dynamic_reconfigureCallback, _1, _2);
00359         server.setCallback(f);
00360 
00361         // image transport with publisher
00362         image_transport::ImageTransport it(n);
00363         image_transport::CameraPublisher pub;
00364 
00365 
00366         ros::Subscriber videomode_sub = n.subscribe("/camera/set_videomode", 1000, videomodeCallback);
00367         ros::Subscriber state_sub = n.subscribe("/camera/set_state", 1000, stateCallback);
00368 
00369         // get the parameters
00370         std::string device;     
00371         std::string out_topic;
00372         n_private.param<std::string>("device", device, "/dev/video0");
00373         n_private.param<std::string>("topic", out_topic, "/camera/image_raw");
00374         n_private.param("width", new_width, 960);
00375         n_private.param("height", new_height, 720);
00376         n_private.param("fps", new_fps, 30); 
00377         n_private.param("isjpeg", isjpeg, false); 
00378 
00379         std::string cameara_parameter_file;
00380         n_private.param<std::string> ("camera_parameter_file", cameara_parameter_file, "../camera_parameters.txt");
00381         
00382         n_private.param("immediately", immediately, true);
00383 
00384         if (immediately)
00385                 state = CHANGE_MODE;
00386         // camera_name is read out of the camera_parameter_file, not further used here
00387         std::string camera_name;
00388         // create a CameraInfo variable
00389         camera_info.reset(new sensor_msgs::CameraInfo);
00390         // read out the "ini" file
00391         if (camera_calibration_parsers::readCalibrationIni(cameara_parameter_file, camera_name, *camera_info)) {
00392                 ROS_INFO("Successfully read camera calibration.");
00393         } else {
00394                 ROS_ERROR("No camera_parameters.txt file found.");
00395         }
00396         // create an image publisher
00397 
00398     if(!isjpeg)
00399             pub = it.advertiseCamera(out_topic.c_str(), 1);
00400         else
00401             image_pub = n.advertise<sensor_msgs::CompressedImage>("image_raw/compressed", 1);
00402         
00403         //create an updater that will send information on the diagnostics topics
00404         diagnostic_updater::Updater updater;
00405         updater.setHardwareIDf("Webcam");
00406         updater.add("Webcam", webcam_diagnostic); //function that will be executed with updater.update()
00407 
00408         /*sleep to overcome the problem of mjpg-streamer (called for 1 seconds in corobot_pantilt node) and this node to change the camera format at the same time. */
00409         sleep(1);
00410 
00411         while (n.ok()) {
00412                 // set it playing again after the parameter were changed
00413                 if (state == CHANGE_MODE)
00414                         state = PLAYING;
00415 
00416                 // start the mainloop which publishes images
00417                 if (state == PLAYING) {
00418                         camera_state = 0;
00419                         mainloop(device.c_str(), new_width, new_height, new_fps, n, pub, updater);
00420                 }
00421                 else
00422                         camera_state = 1;
00423                 updater.update();
00424                 ros::spinOnce();
00425         }
00426         return 0;
00427 }
00428 


corobot_camera
Author(s):
autogenerated on Wed Aug 26 2015 11:09:42