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;
175 dynamic_cast<const SharedMemoryImageBufferHeader *>(*i);
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 log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
virtual ~RosImagesThread()
Destructor.
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.
virtual void finalize()
Finalize the thread.
Shared memory image buffer.
const char * name() const
Get name of thread.
long get_sec() const
Get seconds.
long get_usec() const
Get microseconds.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Time & stamp()
Set this time to the current time.
const char * frame_id() const
Get frame ID.