Fawkes API
Fawkes Development Version
|
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 }