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/ipc/shm_image.h>
26 #include <fvutils/color/conversions.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 
50 
51 void
53 {
54  it_ = new image_transport::ImageTransport(**rosnode);
55  last_update_ = new Time(clock);
56  now_ = new Time(clock);
57  update_images();
58 }
59 
60 void
62 {
63  delete it_;
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 
75 void
77 {
78  now_->stamp();
79  if (*now_ - last_update_ >= 5.0) {
80  *last_update_ = now_;
81  update_images();
82  }
83 
84  std::map<std::string, PublisherInfo>::iterator p;
85  for (p = pubs_.begin(); p != pubs_.end(); ++p) {
86  PublisherInfo &pubinfo = p->second;
87 
88  fawkes::Time cap_time = pubinfo.img->capture_time();
89  if ((pubinfo.last_sent != cap_time) && (pubinfo.pub.getNumSubscribers() > 0)) {
90  pubinfo.last_sent = cap_time;
91 
92  //logger->log_debug(name(), "Need to send %s", p->first.c_str());
93  pubinfo.msg.header.seq += 1;
94  pubinfo.msg.header.stamp =
95  ros::Time(cap_time.get_sec(), cap_time.get_usec() * 1000);
96  convert(pubinfo.img->colorspace(), RGB,
97  pubinfo.img->buffer(), &pubinfo.msg.data[0],
98  pubinfo.msg.width, pubinfo.msg.height);
99 
100  pubinfo.pub.publish(pubinfo.msg);
101  }
102  }
103 
104 }
105 
106 
107 void
108 RosImagesThread::update_images()
109 {
110  std::set<std::string> missing_images;
111  std::set<std::string> unbacked_images;
112  get_sets(missing_images, unbacked_images);
113 
114  if (! unbacked_images.empty()) {
115  std::set<std::string>::iterator i;
116  for (i = unbacked_images.begin(); i != unbacked_images.end(); ++i) {
117  logger->log_info(name(), "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",
130  i->c_str());
131 
132  std::string topic_name = std::string("fawkes_imgs/") + *i;
133  std::string::size_type pos = 0;
134  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
135  topic_name.replace(pos, 1, "_");
136  }
137  for (pos = 0;(pos = topic_name.find(".", pos)) != std::string::npos;) {
138  topic_name.replace(pos, 1, "_");
139  }
140 
141  PublisherInfo &pubinfo = pubs_[*i];
142  pubinfo.pub = it_->advertise(topic_name, 1);
143  pubinfo.img = new SharedMemoryImageBuffer(i->c_str());
144 
145  pubinfo.msg.header.frame_id = pubinfo.img->frame_id();
146  pubinfo.msg.height = pubinfo.img->height();
147  pubinfo.msg.width = pubinfo.img->width();
148  pubinfo.msg.encoding = sensor_msgs::image_encodings::RGB8;
149  pubinfo.msg.step = pubinfo.msg.width * 3; // for RGB
150  pubinfo.msg.data.resize(colorspace_buffer_size(RGB, pubinfo.msg.width, pubinfo.msg.height));
151  }
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;
171  SharedMemory::find(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h);
172  SharedMemory::SharedMemoryIterator endi = SharedMemory::end();
173 
174  while ( i != endi ) {
176  dynamic_cast<const SharedMemoryImageBufferHeader *>(*i);
177  if ( ih ) {
178  image_buffers.insert(ih->image_id());
179  }
180  ++i;
181  }
182  delete h;
183 
184  missing_images.clear();
185  unbacked_images.clear();
186 
187  std::set_difference(image_buffers.begin(), image_buffers.end(),
188  published_images.begin(), published_images.end(),
189  std::inserter(missing_images, missing_images.end()));
190 
191  std::set_difference(published_images.begin(), published_images.end(),
192  image_buffers.begin(), image_buffers.end(),
193  std::inserter(unbacked_images, unbacked_images.end()));
194 }
Shared memory image buffer header.
Definition: shm_image.h:69
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:114
RosImagesThread()
Constructor.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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:45
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
const char * image_id() const
Get image number.
Definition: shm_image.cpp:881
virtual void finalize()
Finalize the thread.
Shared memory image buffer.
Definition: shm_image.h:181
const char * name() const
Get name of thread.
Definition: thread.h:95
long get_sec() const
Get seconds.
Definition: time.h:110
long get_usec() const
Get microseconds.
Definition: time.h:112
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
const char * frame_id() const
Get frame ID.
Definition: shm_image.cpp:174