Fawkes API  Fawkes Development Version
image_thread.cpp
1 
2 /***************************************************************************
3  * image_thread.cpp - Thread to exchange point clouds
4  *
5  * Created: Tue Apr 10 22:12:38 2012
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "image_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <fvutils/color/conversions.h>
26 #include <fvutils/ipc/shm_image.h>
27 #include <sensor_msgs/image_encodings.h>
28 
29 using namespace fawkes;
30 using namespace firevision;
31 
32 /** @class RosImagesThread "image_thread.h"
33  * Thread to export Fawkes images to ROS.
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor. */
39 : Thread("RosImagesThread", Thread::OPMODE_WAITFORWAKEUP),
40  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
41 {
42 }
43 
44 /** Destructor. */
46 {
47 }
48 
49 void
51 {
52  it_ = new image_transport::ImageTransport(**rosnode);
53  last_update_ = new Time(clock);
54  now_ = new Time(clock);
55  update_images();
56 }
57 
58 void
60 {
61  delete it_;
62  delete last_update_;
63  delete now_;
64 
65  std::map<std::string, PublisherInfo>::iterator p;
66  for (p = pubs_.begin(); p != pubs_.end(); ++p) {
67  logger->log_info(name(), "Closing image %s", p->first.c_str());
68  p->second.pub.shutdown();
69  delete p->second.img;
70  }
71  pubs_.clear();
72 }
73 
74 void
76 {
77  now_->stamp();
78  if (*now_ - last_update_ >= 5.0) {
79  *last_update_ = now_;
80  update_images();
81  }
82 
83  std::map<std::string, PublisherInfo>::iterator p;
84  for (p = pubs_.begin(); p != pubs_.end(); ++p) {
85  PublisherInfo &pubinfo = p->second;
86 
87  fawkes::Time cap_time = pubinfo.img->capture_time();
88  if ((pubinfo.last_sent != cap_time) && (pubinfo.pub.getNumSubscribers() > 0)) {
89  pubinfo.last_sent = cap_time;
90 
91  //logger->log_debug(name(), "Need to send %s", p->first.c_str());
92  pubinfo.msg.header.seq += 1;
93  pubinfo.msg.header.stamp = ros::Time(cap_time.get_sec(), cap_time.get_usec() * 1000);
94  convert(pubinfo.img->colorspace(),
95  RGB,
96  pubinfo.img->buffer(),
97  &pubinfo.msg.data[0],
98  pubinfo.msg.width,
99  pubinfo.msg.height);
100 
101  pubinfo.pub.publish(pubinfo.msg);
102  }
103  }
104 }
105 
106 void
107 RosImagesThread::update_images()
108 {
109  std::set<std::string> missing_images;
110  std::set<std::string> unbacked_images;
111  get_sets(missing_images, unbacked_images);
112 
113  if (!unbacked_images.empty()) {
114  std::set<std::string>::iterator i;
115  for (i = unbacked_images.begin(); i != unbacked_images.end(); ++i) {
116  logger->log_info(name(),
117  "Shutting down publisher for no longer available image %s",
118  i->c_str());
119  PublisherInfo &pubinfo = pubs_[*i];
120  pubinfo.pub.shutdown();
121  delete pubinfo.img;
122  pubs_.erase(*i);
123  }
124  }
125 
126  if (!missing_images.empty()) {
127  std::set<std::string>::iterator i;
128  for (i = missing_images.begin(); i != missing_images.end(); ++i) {
129  logger->log_info(name(), "Creating publisher for new image %s", i->c_str());
130 
131  std::string topic_name = std::string("fawkes_imgs/") + *i;
132  std::string::size_type pos = 0;
133  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
134  topic_name.replace(pos, 1, "_");
135  }
136  for (pos = 0; (pos = topic_name.find(".", pos)) != std::string::npos;) {
137  topic_name.replace(pos, 1, "_");
138  }
139 
140  PublisherInfo pubinfo;
141  pubinfo.pub = it_->advertise(topic_name, 1);
142  pubinfo.img = new SharedMemoryImageBuffer(i->c_str());
143 
144  pubinfo.msg.header.frame_id = pubinfo.img->frame_id();
145  pubinfo.msg.height = pubinfo.img->height();
146  pubinfo.msg.width = pubinfo.img->width();
147  pubinfo.msg.encoding = sensor_msgs::image_encodings::RGB8;
148  pubinfo.msg.step = pubinfo.msg.width * 3; // for RGB
149  pubinfo.msg.data.resize(colorspace_buffer_size(RGB, pubinfo.msg.width, pubinfo.msg.height));
150 
151  pubs_[*i] = pubinfo;
152  }
153  }
154 }
155 
156 void
157 RosImagesThread::get_sets(std::set<std::string> &missing_images,
158  std::set<std::string> &unbacked_images)
159 {
160  std::set<std::string> published_images;
161  std::map<std::string, PublisherInfo>::iterator p;
162  for (p = pubs_.begin(); p != pubs_.end(); ++p) {
163  if (p->second.img->num_attached() > 1) {
164  published_images.insert(p->first);
165  }
166  }
167 
168  std::set<std::string> image_buffers;
170  SharedMemory::SharedMemoryIterator i = SharedMemory::find(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h);
171  SharedMemory::SharedMemoryIterator endi = SharedMemory::end();
172 
173  while (i != endi) {
175  dynamic_cast<const SharedMemoryImageBufferHeader *>(*i);
176  if (ih) {
177  image_buffers.insert(ih->image_id());
178  }
179  ++i;
180  }
181  delete h;
182 
183  missing_images.clear();
184  unbacked_images.clear();
185 
186  std::set_difference(image_buffers.begin(),
187  image_buffers.end(),
188  published_images.begin(),
189  published_images.end(),
190  std::inserter(missing_images, missing_images.end()));
191 
192  std::set_difference(published_images.begin(),
193  published_images.end(),
194  image_buffers.begin(),
195  image_buffers.end(),
196  std::inserter(unbacked_images, unbacked_images.end()));
197 }
Shared memory image buffer header.
Definition: shm_image.h:66
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
virtual ~RosImagesThread()
Destructor.
Shared Memory iterator.
Definition: shm.h:118
RosImagesThread()
Constructor.
A class for handling time.
Definition: time.h:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void loop()
Code to execute in the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
const char * image_id() const
Get image number.
Definition: shm_image.cpp:838
virtual void finalize()
Finalize the thread.
Shared memory image buffer.
Definition: shm_image.h:183
const char * name() const
Get name of thread.
Definition: thread.h:100
long get_sec() const
Get seconds.
Definition: time.h:117
long get_usec() const
Get microseconds.
Definition: time.h:127
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
const char * frame_id() const
Get frame ID.
Definition: shm_image.cpp:169