22 #include "laserscan_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <ros/this_node.h>
26 #include <sensor_msgs/LaserScan.h>
27 #include <utils/math/angle.h>
40 :
Thread(
"RosLaserScanThread",
Thread::OPMODE_WAITFORWAKEUP),
44 ls_msg_queue_mutex_ =
new Mutex();
45 seq_num_mutex_ =
new Mutex();
51 delete ls_msg_queue_mutex_;
52 delete seq_num_mutex_;
56 RosLaserScanThread::topic_name(
const char *if_id,
const char *suffix)
58 std::string topic_name = std::string(
"fawkes_scans/") + if_id +
"_" + suffix;
59 std::string::size_type pos = 0;
60 while ((pos = topic_name.find(
"-", pos)) != std::string::npos) {
61 topic_name.replace(pos, 1,
"_");
64 while ((pos = topic_name.find(
" ", pos)) != std::string::npos) {
65 topic_name.replace(pos, 1,
"_");
78 sub_ls_ =
rosnode->subscribe(
"scan", 100, &RosLaserScanThread::laser_scan_message_cb,
this);
84 std::list<Laser360Interface *>::iterator i360;
85 for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
92 std::string topname = topic_name((*i360)->id(),
"360");
95 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
97 logger->
log_info(
name(),
"Publishing laser scan %s at %s", (*i360)->uid(), topname.c_str());
99 pi.msg.header.frame_id = (*i360)->frame();
100 pi.msg.angle_min = 0;
101 pi.msg.angle_max = 2 * M_PI;
102 pi.msg.angle_increment =
deg2rad(1);
103 pi.msg.ranges.resize(360);
105 pubs_[(*i360)->uid()] = pi;
108 std::list<Laser720Interface *>::iterator i720;
109 for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
115 std::string topname = topic_name((*i720)->id(),
"720");
118 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
120 logger->
log_info(
name(),
"Publishing laser scan %s at %s", (*i720)->uid(), topname.c_str());
122 pi.msg.header.frame_id = (*i720)->frame();
123 pi.msg.angle_min = 0;
124 pi.msg.angle_max = 2 * M_PI;
125 pi.msg.angle_increment =
deg2rad(0.5);
126 pi.msg.ranges.resize(720);
128 pubs_[(*i720)->uid()] = pi;
131 std::list<Laser1080Interface *>::iterator i1080;
132 for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
138 std::string topname = topic_name((*i1080)->id(),
"1080");
141 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
144 "Publishing laser scan %s at %s, frame %s",
149 pi.msg.header.frame_id = (*i1080)->frame();
150 pi.msg.angle_min = 0;
151 pi.msg.angle_max = 2 * M_PI;
152 pi.msg.angle_increment =
deg2rad(1. / 3.);
153 pi.msg.ranges.resize(1080);
155 pubs_[(*i1080)->uid()] = pi;
174 std::map<std::string, PublisherInfo>::iterator p;
175 for (p = pubs_.begin(); p != pubs_.end(); ++p) {
176 p->second.pub.shutdown();
179 std::list<Laser360Interface *>::iterator i360;
180 for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
184 std::list<Laser720Interface *>::iterator i720;
185 for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
189 std::list<Laser1080Interface *>::iterator i1080;
190 for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
199 ls_msg_queue_mutex_->
lock();
200 unsigned int queue = active_queue_;
201 active_queue_ = 1 - active_queue_;
202 ls_msg_queue_mutex_->
unlock();
204 while (!ls_msg_queues_[queue].empty()) {
205 const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt = ls_msg_queues_[queue].front();
207 sensor_msgs::LaserScan::ConstPtr msg = msg_evt.getConstMessage();
210 const std::string callerid = msg_evt.getPublisherName();
213 if (callerid.empty()) {
215 "Received laser scan from ROS without caller ID,"
218 bool have_interface =
true;
219 if (ls360_wifs_.find(callerid) == ls360_wifs_.end()) {
221 std::string
id = std::string(
"ROS Laser ") + callerid;
223 ls360_wifs_[callerid] = ls360if;
226 "Failed to open ROS laser interface for "
227 "message from node %s, exception follows",
230 have_interface =
false;
234 if (have_interface) {
237 ls360if->
set_frame(msg->header.frame_id.c_str());
238 float distances[360];
239 for (
unsigned int a = 0; a < 360; ++a) {
241 if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
245 int idx = (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
246 distances[a] = msg->ranges[idx];
254 ls_msg_queues_[queue].pop();
265 PublisherInfo & pi = pubs_[interface->uid()];
266 sensor_msgs::LaserScan &msg = pi.msg;
273 seq_num_mutex_->lock();
274 msg.header.seq = ++seq_num_;
275 seq_num_mutex_->unlock();
277 msg.header.frame_id = ls360if->
frame();
280 msg.angle_max = 2 * M_PI;
281 msg.angle_increment =
deg2rad(1);
283 msg.range_max = 1000.;
284 msg.ranges.resize(360);
285 memcpy(&msg.ranges[0], ls360if->
distances(), 360 *
sizeof(
float));
287 pi.pub.publish(pi.msg);
289 }
else if (ls720if) {
294 seq_num_mutex_->lock();
295 msg.header.seq = ++seq_num_;
296 seq_num_mutex_->unlock();
298 msg.header.frame_id = ls720if->
frame();
301 msg.angle_max = 2 * M_PI;
302 msg.angle_increment =
deg2rad(1. / 2.);
304 msg.range_max = 1000.;
305 msg.ranges.resize(720);
306 memcpy(&msg.ranges[0], ls720if->
distances(), 720 *
sizeof(
float));
308 pi.pub.publish(pi.msg);
310 }
else if (ls1080if) {
315 seq_num_mutex_->lock();
316 msg.header.seq = ++seq_num_;
317 seq_num_mutex_->unlock();
319 msg.header.frame_id = ls1080if->
frame();
322 msg.angle_max = 2 * M_PI;
323 msg.angle_increment =
deg2rad(1. / 3.);
325 msg.range_max = 1000.;
326 msg.ranges.resize(1080);
327 memcpy(&msg.ranges[0], ls1080if->
distances(), 1080 *
sizeof(
float));
329 pi.pub.publish(pi.msg);
337 if (fnmatch(
"ROS *",
id, FNM_NOESCAPE) == 0)
340 if (strncmp(type,
"Laser360Interface", INTERFACE_TYPE_SIZE_) == 0) {
343 logger->
log_info(name(),
"Opening %s:%s", type,
id);
347 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
352 bbil_add_data_interface(ls360if);
353 bbil_add_reader_interface(ls360if);
354 bbil_add_writer_interface(ls360if);
356 std::string topname = topic_name(ls360if->
id(),
"360");
359 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
361 logger->
log_info(name(),
"Publishing laser scan %s at %s", ls360if->
uid(), topname.c_str());
363 pi.msg.header.frame_id = ls360if->
frame();
364 pi.msg.angle_min = 0;
365 pi.msg.angle_max = 2 * M_PI;
366 pi.msg.angle_increment =
deg2rad(1);
367 pi.msg.ranges.resize(360);
369 pubs_[ls360if->
uid()] = pi;
372 ls360_ifs_.push_back(ls360if);
374 blackboard->
close(ls360if);
375 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
379 }
else if (strncmp(type,
"Laser720Interface", INTERFACE_TYPE_SIZE_) == 0) {
382 logger->
log_info(name(),
"Opening %s:%s", type,
id);
386 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
391 bbil_add_data_interface(ls720if);
392 bbil_add_reader_interface(ls720if);
393 bbil_add_writer_interface(ls720if);
395 std::string topname = topic_name(ls720if->
id(),
"720");
398 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
400 logger->
log_info(name(),
"Publishing laser scan %s at %s", ls720if->
uid(), topname.c_str());
402 pi.msg.header.frame_id = ls720if->
frame();
403 pi.msg.angle_min = 0;
404 pi.msg.angle_max = 2 * M_PI;
405 pi.msg.angle_increment =
deg2rad(0.5);
406 pi.msg.ranges.resize(720);
408 pubs_[ls720if->
uid()] = pi;
411 ls720_ifs_.push_back(ls720if);
413 blackboard->
close(ls720if);
414 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
418 }
else if (strncmp(type,
"Laser1080Interface", INTERFACE_TYPE_SIZE_) == 0) {
421 logger->
log_info(name(),
"Opening %s:%s", type,
id);
425 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
430 bbil_add_data_interface(ls1080if);
431 bbil_add_reader_interface(ls1080if);
432 bbil_add_writer_interface(ls1080if);
434 std::string topname = topic_name(ls1080if->
id(),
"1080");
437 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
440 "Publishing 1080 laser scan %s at %s",
444 pi.msg.header.frame_id = ls1080if->
frame();
445 pi.msg.angle_min = 0;
446 pi.msg.angle_max = 2 * M_PI;
447 pi.msg.angle_increment =
deg2rad(0.5);
448 pi.msg.ranges.resize(1080);
450 pubs_[ls1080if->
uid()] = pi;
453 ls1080_ifs_.push_back(ls1080if);
455 blackboard->
close(ls1080if);
456 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
466 conditional_close(interface);
473 conditional_close(interface);
477 RosLaserScanThread::conditional_close(
Interface *interface) noexcept
485 std::list<Laser360Interface *>::iterator i;
486 for (i = ls360_ifs_.begin(); i != ls360_ifs_.end(); ++i) {
487 if (*ls360if == **i) {
490 logger->
log_info(name(),
"Last on %s, closing", ls360if->
uid());
491 bbil_remove_data_interface(*i);
492 bbil_remove_reader_interface(*i);
493 bbil_remove_writer_interface(*i);
495 blackboard->
close(*i);
501 }
else if (ls720if) {
502 std::list<Laser720Interface *>::iterator i;
503 for (i = ls720_ifs_.begin(); i != ls720_ifs_.end(); ++i) {
504 if (*ls720if == **i) {
507 logger->
log_info(name(),
"Last on %s, closing", ls720if->
uid());
508 bbil_remove_data_interface(*i);
509 bbil_remove_reader_interface(*i);
510 bbil_remove_writer_interface(*i);
512 blackboard->
close(*i);
519 }
else if (ls1080if) {
520 std::list<Laser1080Interface *>::iterator i;
521 for (i = ls1080_ifs_.begin(); i != ls1080_ifs_.end(); ++i) {
522 if (*ls1080if == **i) {
525 logger->
log_info(name(),
"Last on %s, closing", ls1080if->
uid());
526 bbil_remove_data_interface(*i);
527 bbil_remove_reader_interface(*i);
528 bbil_remove_writer_interface(*i);
530 blackboard->
close(*i);
531 ls1080_ifs_.erase(i);
543 RosLaserScanThread::laser_scan_message_cb(
544 const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt)
547 ls_msg_queues_[active_queue_].push(msg_evt);
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual void finalize()
Finalize the thread.
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual ~RosLaserScanThread()
Destructor.
virtual void init()
Initialize the thread.
RosLaserScanThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
Base class for all Fawkes BlackBoard interfaces.
const Time * timestamp() const
Get timestamp of last write.
void write()
Write from local copy into BlackBoard memory.
const char * id() const
Get identifier of interface.
const char * uid() const
Get unique identifier of interface.
unsigned int num_readers() const
Get the number of readers.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
Laser1080Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
void set_frame(const char *new_frame)
Set frame value.
char * frame() const
Get frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Laser720Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Mutex mutual exclusion lock.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
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.
Time & stamp()
Set this time to the current time.
long get_nsec() const
Get nanoseconds.
long get_sec() const
Get seconds.
A convenience class for universally unique identifiers (UUIDs).
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.