22 #include "image_thread.h"
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>
30 using namespace firevision;
39 :
Thread(
"RosImagesThread",
Thread::OPMODE_WAITFORWAKEUP),
52 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();
78 if (*now_ - last_update_ >= 5.0) {
83 std::map<std::string, PublisherInfo>::iterator p;
84 for (p = pubs_.begin(); p != pubs_.end(); ++p) {
85 PublisherInfo &pubinfo = p->second;
88 if ((pubinfo.last_sent != cap_time) && (pubinfo.pub.getNumSubscribers() > 0)) {
89 pubinfo.last_sent = cap_time;
92 pubinfo.msg.header.seq += 1;
94 convert(pubinfo.img->colorspace(),
96 pubinfo.img->buffer(),
101 pubinfo.pub.publish(pubinfo.msg);
107 RosImagesThread::update_images()
109 std::set<std::string> missing_images;
110 std::set<std::string> unbacked_images;
111 get_sets(missing_images, unbacked_images);
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",
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) {
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,
"_");
136 for (pos = 0; (pos = topic_name.find(
".", pos)) != std::string::npos;) {
137 topic_name.replace(pos, 1,
"_");
140 PublisherInfo pubinfo;
141 pubinfo.pub = it_->advertise(topic_name, 1);
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;
149 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;
177 image_buffers.insert(ih->
image_id());
183 missing_images.clear();
184 unbacked_images.clear();
186 std::set_difference(image_buffers.begin(),
188 published_images.begin(),
189 published_images.end(),
190 std::inserter(missing_images, missing_images.end()));
192 std::set_difference(published_images.begin(),
193 published_images.end(),
194 image_buffers.begin(),
196 std::inserter(unbacked_images, unbacked_images.end()));
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.
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.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
long get_usec() const
Get microseconds.
Time & stamp()
Set this time to the current time.
long get_sec() const
Get seconds.
Shared memory image buffer.
Fawkes library namespace.