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> 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.
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.