Fawkes API  Fawkes Development Version
image_thread.cpp
00001 
00002 /***************************************************************************
00003  *  image_thread.cpp - Thread to exchange point clouds
00004  *
00005  *  Created: Tue Apr 10 22:12:38 2012
00006  *  Copyright  2011-2012  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "image_thread.h"
00023 
00024 #include <core/threading/mutex_locker.h>
00025 #include <fvutils/ipc/shm_image.h>
00026 #include <fvutils/color/conversions.h>
00027 #include <sensor_msgs/image_encodings.h>
00028 
00029 using namespace fawkes;
00030 using namespace firevision;
00031 
00032 /** @class RosImagesThread "image_thread.h"
00033  * Thread to export Fawkes images to ROS.
00034  * @author Tim Niemueller
00035  */
00036 
00037 /** Constructor. */
00038 RosImagesThread::RosImagesThread()
00039   : Thread("RosImagesThread", Thread::OPMODE_WAITFORWAKEUP),
00040     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
00041 {
00042 }
00043 
00044 /** Destructor. */
00045 RosImagesThread::~RosImagesThread()
00046 {
00047 }
00048 
00049 
00050 
00051 void
00052 RosImagesThread::init()
00053 {
00054   it_ = new image_transport::ImageTransport(**rosnode);
00055   last_update_ = new Time(clock);
00056   now_ = new Time(clock);
00057   update_images();
00058 }
00059 
00060 void
00061 RosImagesThread::finalize()
00062 {
00063   delete it_;
00064 
00065   std::map<std::string, PublisherInfo>::iterator p;
00066   for (p = pubs_.begin(); p != pubs_.end(); ++p) {
00067     logger->log_info(name(), "Closing image %s", p->first.c_str());
00068     p->second.pub.shutdown();
00069     delete p->second.img;
00070   }
00071   pubs_.clear();
00072 }
00073 
00074 
00075 void
00076 RosImagesThread::loop()
00077 {
00078   now_->stamp();
00079   if (*now_ - last_update_ >= 5.0) {
00080     *last_update_ = now_;
00081     update_images();
00082   }
00083 
00084   std::map<std::string, PublisherInfo>::iterator p;
00085   for (p = pubs_.begin(); p != pubs_.end(); ++p) {
00086     PublisherInfo &pubinfo = p->second;
00087 
00088     fawkes::Time cap_time = pubinfo.img->capture_time();
00089     if ((pubinfo.last_sent != cap_time) && (pubinfo.pub.getNumSubscribers() > 0)) {
00090       pubinfo.last_sent = cap_time;
00091 
00092       //logger->log_debug(name(), "Need to send %s", p->first.c_str());
00093       pubinfo.msg.header.seq += 1;
00094       pubinfo.msg.header.stamp =
00095         ros::Time(cap_time.get_sec(), cap_time.get_usec() * 1000);
00096       convert(pubinfo.img->colorspace(), RGB,
00097               pubinfo.img->buffer(), &pubinfo.msg.data[0],
00098               pubinfo.msg.width, pubinfo.msg.height);
00099 
00100       pubinfo.pub.publish(pubinfo.msg);
00101     }
00102   }
00103  
00104 }
00105 
00106 
00107 void
00108 RosImagesThread::update_images()
00109 {
00110   std::set<std::string> missing_images;
00111   std::set<std::string> unbacked_images;
00112   get_sets(missing_images, unbacked_images);
00113 
00114   if (! unbacked_images.empty()) {
00115     std::set<std::string>::iterator i;
00116     for (i = unbacked_images.begin(); i != unbacked_images.end(); ++i) {
00117       logger->log_info(name(), "Shutting down publisher for no longer available image %s",
00118                        i->c_str());
00119       PublisherInfo &pubinfo = pubs_[*i];
00120       pubinfo.pub.shutdown();
00121       delete pubinfo.img;
00122       pubs_.erase(*i);
00123     }
00124   }
00125 
00126   if (! missing_images.empty()) {
00127     std::set<std::string>::iterator i;
00128     for (i = missing_images.begin(); i != missing_images.end(); ++i) {
00129       logger->log_info(name(), "Creating publisher for new image %s",
00130                        i->c_str());
00131 
00132       std::string topic_name = std::string("fawkes_imgs/") + *i;
00133       std::string::size_type pos = 0;
00134       while ((pos = topic_name.find("-", pos)) != std::string::npos) {
00135         topic_name.replace(pos, 1, "_");
00136       }
00137       for (pos = 0;(pos = topic_name.find(".", pos)) != std::string::npos;) {
00138         topic_name.replace(pos, 1, "_");
00139       }
00140 
00141       PublisherInfo &pubinfo = pubs_[*i];
00142       pubinfo.pub = it_->advertise(topic_name, 1);
00143       pubinfo.img = new SharedMemoryImageBuffer(i->c_str());
00144 
00145       pubinfo.msg.header.frame_id = pubinfo.img->frame_id();
00146       pubinfo.msg.height          = pubinfo.img->height();
00147       pubinfo.msg.width           = pubinfo.img->width();
00148       pubinfo.msg.encoding        = sensor_msgs::image_encodings::RGB8;
00149       pubinfo.msg.step            = pubinfo.msg.width * 3; // for RGB
00150       pubinfo.msg.data.resize(colorspace_buffer_size(RGB, pubinfo.msg.width, pubinfo.msg.height));
00151     }
00152   }
00153 }
00154 
00155 
00156 void
00157 RosImagesThread::get_sets(std::set<std::string> &missing_images,
00158                           std::set<std::string> &unbacked_images)
00159 {
00160   std::set<std::string> published_images;
00161   std::map<std::string, PublisherInfo>::iterator p;
00162   for (p = pubs_.begin(); p != pubs_.end(); ++p) {
00163     if (p->second.img->num_attached() > 1) {
00164       published_images.insert(p->first);
00165     }
00166   }
00167 
00168   std::set<std::string> image_buffers;
00169   SharedMemoryImageBufferHeader *h = new SharedMemoryImageBufferHeader();
00170   SharedMemory::SharedMemoryIterator i =
00171     SharedMemory::find(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h);
00172   SharedMemory::SharedMemoryIterator endi = SharedMemory::end();
00173                                                             
00174   while ( i != endi ) {
00175     const SharedMemoryImageBufferHeader *ih =
00176       dynamic_cast<const SharedMemoryImageBufferHeader *>(*i);
00177     if ( ih ) {
00178       image_buffers.insert(ih->image_id());
00179     }
00180     ++i;
00181   }
00182   delete h;
00183 
00184   missing_images.clear();
00185   unbacked_images.clear();
00186 
00187   std::set_difference(image_buffers.begin(), image_buffers.end(),
00188                       published_images.begin(), published_images.end(),
00189                       std::inserter(missing_images, missing_images.end()));
00190 
00191   std::set_difference(published_images.begin(), published_images.end(),
00192                       image_buffers.begin(), image_buffers.end(),
00193                       std::inserter(unbacked_images, unbacked_images.end()));
00194 }