22 #include "image_thread.h"
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>
29 using namespace fawkes;
30 using namespace firevision;
39 :
Thread(
"RosImagesThread",
Thread::OPMODE_WAITFORWAKEUP),
54 it_ =
new image_transport::ImageTransport(**
rosnode);
65 std::map<std::string, PublisherInfo>::iterator p;
66 for (p = pubs_.begin(); p != pubs_.end(); ++p) {
68 p->second.pub.shutdown();
79 if (*now_ - last_update_ >= 5.0) {
84 std::map<std::string, PublisherInfo>::iterator p;
85 for (p = pubs_.begin(); p != pubs_.end(); ++p) {
86 PublisherInfo &pubinfo = p->second;
89 if ((pubinfo.last_sent != cap_time) && (pubinfo.pub.getNumSubscribers() > 0)) {
90 pubinfo.last_sent = cap_time;
93 pubinfo.msg.header.seq += 1;
94 pubinfo.msg.header.
stamp =
96 convert(pubinfo.img->colorspace(), RGB,
97 pubinfo.img->buffer(), &pubinfo.msg.data[0],
98 pubinfo.msg.width, pubinfo.msg.height);
100 pubinfo.pub.publish(pubinfo.msg);
108 RosImagesThread::update_images()
110 std::set<std::string> missing_images;
111 std::set<std::string> unbacked_images;
112 get_sets(missing_images, unbacked_images);
114 if (! unbacked_images.empty()) {
115 std::set<std::string>::iterator i;
116 for (i = unbacked_images.begin(); i != unbacked_images.end(); ++i) {
119 PublisherInfo &pubinfo = pubs_[*i];
120 pubinfo.pub.shutdown();
126 if (! missing_images.empty()) {
127 std::set<std::string>::iterator i;
128 for (i = missing_images.begin(); i != missing_images.end(); ++i) {
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,
"_");
137 for (pos = 0;(pos = topic_name.find(
".", pos)) != std::string::npos;) {
138 topic_name.replace(pos, 1,
"_");
141 PublisherInfo &pubinfo = pubs_[*i];
142 pubinfo.pub = it_->advertise(topic_name, 1);
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;
150 pubinfo.msg.data.resize(colorspace_buffer_size(RGB, pubinfo.msg.width, pubinfo.msg.height));
157 RosImagesThread::get_sets(std::set<std::string> &missing_images,
158 std::set<std::string> &unbacked_images)
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);
168 std::set<std::string> image_buffers;
171 SharedMemory::find(FIREVISION_SHM_IMAGE_MAGIC_TOKEN, h);
174 while ( i != endi ) {
178 image_buffers.insert(ih->
image_id());
184 missing_images.clear();
185 unbacked_images.clear();
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()));
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()));
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
const char * frame_id() const
Get frame ID.
virtual ~RosImagesThread()
Destructor.
long get_usec() const
Get microseconds.
RosImagesThread()
Constructor.
A class for handling time.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void loop()
Code to execute in the thread.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
long get_sec() const
Get seconds.
virtual void finalize()
Finalize the thread.
Shared memory image buffer.
const char * name() const
Get name of thread.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Time & stamp()
Set this time to the current time.