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
29using namespace fawkes;
30using 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
49void
51{
52 it_ = new image_transport::ImageTransport(**rosnode);
53 last_update_ = new Time(clock);
54 now_ = new Time(clock);
55 update_images();
56}
57
58void
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
74void
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
106void
107RosImagesThread::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) {
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
156void
157RosImagesThread::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}
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual ~RosImagesThread()
Destructor.
RosImagesThread()
Constructor.
virtual void finalize()
Finalize the thread.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Shared Memory iterator.
Definition: shm.h:119
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
long get_usec() const
Get microseconds.
Definition: time.h:127
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
long get_sec() const
Get seconds.
Definition: time.h:117
Shared memory image buffer header.
Definition: shm_image.h:67
const char * image_id() const
Get image number.
Definition: shm_image.cpp:838
Shared memory image buffer.
Definition: shm_image.h:184
Fawkes library namespace.