22 #include "laserscan_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <utils/math/angle.h>
27 #include <ros/this_node.h>
28 #include <sensor_msgs/LaserScan.h>
32 using namespace fawkes;
41 :
Thread(
"RosLaserScanThread",
Thread::OPMODE_WAITFORWAKEUP),
45 __ls_msg_queue_mutex =
new Mutex();
46 __seq_num_mutex =
new Mutex();
52 delete __ls_msg_queue_mutex;
53 delete __seq_num_mutex;
58 RosLaserScanThread::topic_name(
const char *if_id,
const char *suffix)
60 std::string topic_name = std::string(
"fawkes_scans/") + if_id +
"_" + suffix;
61 std::string::size_type pos = 0;
62 while ((pos = topic_name.find(
"-", pos)) != std::string::npos) {
63 topic_name.replace(pos, 1,
"_");
66 while ((pos = topic_name.find(
" ", pos)) != std::string::npos) {
67 topic_name.replace(pos, 1,
"_");
81 __sub_ls =
rosnode->subscribe(
"scan", 100,
82 &RosLaserScanThread::laser_scan_message_cb,
this);
89 std::list<Laser360Interface *>::iterator i360;
90 for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
97 std::string topname = topic_name((*i360)->id(),
"360");
101 rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
104 (*i360)->uid(), topname.c_str());
106 pi.msg.header.frame_id = (*i360)->frame();
107 pi.msg.angle_min = 0;
108 pi.msg.angle_max = 2*M_PI;
109 pi.msg.angle_increment =
deg2rad(1);
110 pi.msg.ranges.resize(360);
112 __pubs[(*i360)->uid()] = pi;
114 std::list<Laser720Interface *>::iterator i720;
115 for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
121 std::string topname = topic_name((*i720)->id(),
"720");
125 rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
128 (*i720)->uid(), topname.c_str());
130 pi.msg.header.frame_id = (*i720)->frame();
131 pi.msg.angle_min = 0;
132 pi.msg.angle_max = 2*M_PI;
133 pi.msg.angle_increment =
deg2rad(0.5);
134 pi.msg.ranges.resize(720);
136 __pubs[(*i720)->uid()] = pi;
154 std::map<std::string, PublisherInfo>::iterator p;
155 for (p = __pubs.begin(); p != __pubs.end(); ++p) {
156 p->second.pub.shutdown();
159 std::list<Laser360Interface *>::iterator i360;
160 for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
164 std::list<Laser720Interface *>::iterator i720;
165 for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
175 __ls_msg_queue_mutex->
lock();
176 unsigned int queue = __active_queue;
177 __active_queue = 1 - __active_queue;
178 __ls_msg_queue_mutex->
unlock();
180 while (! __ls_msg_queues[queue].empty()) {
181 const sensor_msgs::LaserScan::ConstPtr &msg = __ls_msg_queues[queue].front();
184 std::map<std::string, std::string> *msg_header_map =
185 msg->__connection_header.get();
186 std::map<std::string, std::string>::iterator it =
187 msg_header_map->find(
"callerid");
188 const std::string &callerid = it->second;
191 if (it == msg_header_map->end()) {
195 bool have_interface =
true;
196 if (__ls360_wifs.find(callerid) == __ls360_wifs.end()) {
198 std::string
id = std::string(
"ROS Laser ") + callerid;
201 __ls360_wifs[callerid] = ls360if;
204 "message from node %s, exception follows",
207 have_interface =
false;
211 if (have_interface) {
214 ls360if->
set_frame(msg->header.frame_id.c_str());
215 float distances[360];
216 for (
unsigned int a = 0; a < 360; ++a) {
218 if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
223 (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
224 distances[a] = msg->ranges[idx];
232 __ls_msg_queues[queue].pop();
243 PublisherInfo &pi = __pubs[interface->uid()];
244 sensor_msgs::LaserScan &msg = pi.msg;
251 __seq_num_mutex->lock();
252 msg.header.seq = ++__seq_num;
253 __seq_num_mutex->unlock();
255 msg.header.frame_id = ls360if->
frame();
258 msg.angle_max = 2*M_PI;
259 msg.angle_increment =
deg2rad(1);
261 msg.range_max = 1000.;
262 msg.ranges.resize(360);
263 memcpy(&msg.ranges[0], ls360if->
distances(), 360*
sizeof(float));
265 pi.pub.publish(pi.msg);
267 }
else if (ls720if) {
272 sensor_msgs::LaserScan msg;
273 __seq_num_mutex->lock();
274 msg.header.seq = ++__seq_num;
275 __seq_num_mutex->unlock();
277 msg.header.frame_id = ls720if->
frame();
280 msg.angle_max = 2*M_PI;
281 msg.angle_increment =
deg2rad(1);
283 msg.range_max = 1000.;
284 msg.ranges.resize(720);
285 memcpy(&msg.ranges[0], ls720if->
distances(), 720*
sizeof(float));
287 pi.pub.publish(pi.msg);
297 if (fnmatch(
"ROS *",
id, FNM_NOESCAPE) == 0)
return;
299 if (strncmp(type,
"Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
303 logger->
log_info(name(),
"Opening %s:%s", type,
id);
307 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
312 bbil_add_data_interface(ls360if);
313 bbil_add_reader_interface(ls360if);
314 bbil_add_writer_interface(ls360if);
316 std::string topname = topic_name(ls360if->
id(),
"360");
319 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
321 logger->
log_info(name(),
"Publishing laser scan %s at %s",
322 ls360if->
uid(), topname.c_str());
324 pi.msg.header.frame_id = ls360if->
frame();
325 pi.msg.angle_min = 0;
326 pi.msg.angle_max = 2*M_PI;
327 pi.msg.angle_increment =
deg2rad(1);
328 pi.msg.ranges.resize(360);
330 __pubs[ls360if->
uid()] = pi;
333 __ls360_ifs.push_back(ls360if);
335 blackboard->
close(ls360if);
336 logger->
log_warn(name(),
"Failed to register for %s:%s: %s",
340 }
else if (strncmp(type,
"Laser720Interface", __INTERFACE_TYPE_SIZE) == 0) {
344 logger->
log_info(name(),
"Opening %s:%s", type,
id);
348 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
353 bbil_add_data_interface(ls720if);
354 bbil_add_reader_interface(ls720if);
355 bbil_add_writer_interface(ls720if);
357 std::string topname = topic_name(ls720if->
id(),
"720");
360 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
362 logger->
log_info(name(),
"Publishing laser scan %s at %s",
363 ls720if->
uid(), topname.c_str());
365 pi.msg.header.frame_id = ls720if->
frame();
366 pi.msg.angle_min = 0;
367 pi.msg.angle_max = 2*M_PI;
368 pi.msg.angle_increment =
deg2rad(0.5);
369 pi.msg.ranges.resize(720);
371 __pubs[ls720if->
uid()] = pi;
374 __ls720_ifs.push_back(ls720if);
376 blackboard->
close(ls720if);
377 logger->
log_warn(name(),
"Failed to register for %s:%s: %s",
386 unsigned int instance_serial)
throw()
388 conditional_close(interface);
393 unsigned int instance_serial)
throw()
395 conditional_close(interface);
399 RosLaserScanThread::conditional_close(
Interface *interface)
throw()
406 std::list<Laser360Interface *>::iterator i;
407 for (i = __ls360_ifs.begin(); i != __ls360_ifs.end(); ++i) {
408 if (*ls360if == **i) {
411 logger->
log_info(name(),
"Last on %s, closing", ls360if->
uid());
412 bbil_remove_data_interface(*i);
413 bbil_remove_reader_interface(*i);
414 bbil_remove_writer_interface(*i);
416 blackboard->
close(*i);
417 __ls360_ifs.erase(i);
422 }
else if (ls720if) {
423 std::list<Laser720Interface *>::iterator i;
424 for (i = __ls720_ifs.begin(); i != __ls720_ifs.end(); ++i) {
425 if (*ls720if == **i) {
428 logger->
log_info(name(),
"Last on %s, closing", ls720if->
uid());
429 bbil_remove_data_interface(*i);
430 bbil_remove_reader_interface(*i);
431 bbil_remove_writer_interface(*i);
433 blackboard->
close(*i);
434 __ls720_ifs.erase(i);
447 RosLaserScanThread::laser_scan_message_cb(
const sensor_msgs::LaserScan::ConstPtr &msg)
451 std::map<std::string, std::string> *msg_header_map =
452 msg->__connection_header.get();
453 std::map<std::string, std::string>::iterator it =
454 msg_header_map->find(
"callerid");
456 if (it == msg_header_map->end()) {
458 }
else if (it->second != ros::this_node::getName()) {
459 __ls_msg_queues[__active_queue].push(msg);
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
unsigned int num_readers() const
Get the number of readers.
void unlock()
Unlock the mutex.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
virtual void finalize()
Finalize the thread.
A class for handling time.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
void write()
Write from local copy into BlackBoard memory.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Base class for all Fawkes BlackBoard interfaces.
char * frame() const
Get frame value.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
const char * uid() const
Get unique identifier of interface.
virtual void loop()
Code to execute in the thread.
bool has_writer() const
Check if there is a writer for the interface.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
const char * id() const
Get identifier of interface.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
char * frame() const
Get frame value.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
virtual const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
long get_sec() const
Get seconds.
void read()
Read from BlackBoard into local copy.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
float * distances() const
Get distances value.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
long get_nsec() const
Get nanoseconds.
virtual ~RosLaserScanThread()
Destructor.
virtual void init()
Initialize the thread.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
RosLaserScanThread()
Constructor.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
float * distances() const
Get distances value.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Laser720Interface Fawkes BlackBoard Interface.
BlackBoard interface listener.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.