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> 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);
91 std::list<Laser360Interface *>::iterator i360;
92 for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
99 std::string topname = topic_name((*i360)->id(),
"360");
103 rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
106 (*i360)->uid(), topname.c_str());
108 pi.msg.header.frame_id = (*i360)->frame();
109 pi.msg.angle_min = 0;
110 pi.msg.angle_max = 2*M_PI;
111 pi.msg.angle_increment =
deg2rad(1);
112 pi.msg.ranges.resize(360);
114 __pubs[(*i360)->uid()] = pi;
117 std::list<Laser720Interface *>::iterator i720;
118 for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
124 std::string topname = topic_name((*i720)->id(),
"720");
128 rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
131 (*i720)->uid(), topname.c_str());
133 pi.msg.header.frame_id = (*i720)->frame();
134 pi.msg.angle_min = 0;
135 pi.msg.angle_max = 2*M_PI;
136 pi.msg.angle_increment =
deg2rad(0.5);
137 pi.msg.ranges.resize(720);
139 __pubs[(*i720)->uid()] = pi;
142 std::list<Laser1080Interface *>::iterator i1080;
143 for (i1080 = __ls1080_ifs.begin(); i1080 != __ls1080_ifs.end(); ++i1080) {
149 std::string topname = topic_name((*i1080)->id(),
"1080");
153 rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
156 (*i1080)->uid(), topname.c_str(), (*i1080)->frame());
158 pi.msg.header.frame_id = (*i1080)->frame();
159 pi.msg.angle_min = 0;
160 pi.msg.angle_max = 2*M_PI;
161 pi.msg.angle_increment =
deg2rad(1. / 3.);
162 pi.msg.ranges.resize(1080);
164 __pubs[(*i1080)->uid()] = pi;
184 std::map<std::string, PublisherInfo>::iterator p;
185 for (p = __pubs.begin(); p != __pubs.end(); ++p) {
186 p->second.pub.shutdown();
189 std::list<Laser360Interface *>::iterator i360;
190 for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
194 std::list<Laser720Interface *>::iterator i720;
195 for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
199 std::list<Laser1080Interface *>::iterator i1080;
200 for (i1080 = __ls1080_ifs.begin(); i1080 != __ls1080_ifs.end(); ++i1080) {
203 __ls1080_ifs.clear();
210 __ls_msg_queue_mutex->
lock();
211 unsigned int queue = __active_queue;
212 __active_queue = 1 - __active_queue;
213 __ls_msg_queue_mutex->
unlock();
215 while (! __ls_msg_queues[queue].empty()) {
216 const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt =
217 __ls_msg_queues[queue].front();
219 sensor_msgs::LaserScan::ConstPtr msg = msg_evt.getConstMessage();
222 const std::string callerid = msg_evt.getPublisherName();
225 if (callerid.empty()) {
229 bool have_interface =
true;
230 if (__ls360_wifs.find(callerid) == __ls360_wifs.end()) {
232 std::string
id = std::string(
"ROS Laser ") + callerid;
235 __ls360_wifs[callerid] = ls360if;
238 "message from node %s, exception follows",
241 have_interface =
false;
245 if (have_interface) {
248 ls360if->
set_frame(msg->header.frame_id.c_str());
249 float distances[360];
250 for (
unsigned int a = 0; a < 360; ++a) {
252 if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
257 (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
258 distances[a] = msg->ranges[idx];
266 __ls_msg_queues[queue].pop();
278 PublisherInfo &pi = __pubs[interface->uid()];
279 sensor_msgs::LaserScan &msg = pi.msg;
286 __seq_num_mutex->
lock();
287 msg.header.seq = ++__seq_num;
288 __seq_num_mutex->
unlock();
290 msg.header.frame_id = ls360if->
frame();
293 msg.angle_max = 2*M_PI;
294 msg.angle_increment =
deg2rad(1);
296 msg.range_max = 1000.;
297 msg.ranges.resize(360);
298 memcpy(&msg.ranges[0], ls360if->
distances(), 360*
sizeof(float));
300 pi.pub.publish(pi.msg);
302 }
else if (ls720if) {
307 __seq_num_mutex->
lock();
308 msg.header.seq = ++__seq_num;
309 __seq_num_mutex->
unlock();
311 msg.header.frame_id = ls720if->
frame();
314 msg.angle_max = 2*M_PI;
315 msg.angle_increment =
deg2rad(1./2.);
317 msg.range_max = 1000.;
318 msg.ranges.resize(720);
319 memcpy(&msg.ranges[0], ls720if->
distances(), 720*
sizeof(float));
321 pi.pub.publish(pi.msg);
323 }
else if (ls1080if) {
328 __seq_num_mutex->
lock();
329 msg.header.seq = ++__seq_num;
330 __seq_num_mutex->
unlock();
332 msg.header.frame_id = ls1080if->
frame();
335 msg.angle_max = 2*M_PI;
336 msg.angle_increment =
deg2rad(1./3.);
338 msg.range_max = 1000.;
339 msg.ranges.resize(1080);
340 memcpy(&msg.ranges[0], ls1080if->
distances(), 1080*
sizeof(float));
342 pi.pub.publish(pi.msg);
352 if (fnmatch(
"ROS *",
id, FNM_NOESCAPE) == 0)
return;
354 if (strncmp(type,
"Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
371 std::string topname = topic_name(ls360if->
id(),
"360");
374 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
377 ls360if->
uid(), topname.c_str());
379 pi.msg.header.frame_id = ls360if->
frame();
380 pi.msg.angle_min = 0;
381 pi.msg.angle_max = 2*M_PI;
382 pi.msg.angle_increment =
deg2rad(1);
383 pi.msg.ranges.resize(360);
385 __pubs[ls360if->
uid()] = pi;
388 __ls360_ifs.push_back(ls360if);
396 }
else if (strncmp(type,
"Laser720Interface", __INTERFACE_TYPE_SIZE) == 0) {
412 std::string topname = topic_name(ls720if->
id(),
"720");
415 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
418 ls720if->
uid(), topname.c_str());
420 pi.msg.header.frame_id = ls720if->
frame();
421 pi.msg.angle_min = 0;
422 pi.msg.angle_max = 2*M_PI;
423 pi.msg.angle_increment =
deg2rad(0.5);
424 pi.msg.ranges.resize(720);
426 __pubs[ls720if->
uid()] = pi;
429 __ls720_ifs.push_back(ls720if);
437 }
else if (strncmp(type,
"Laser1080Interface", __INTERFACE_TYPE_SIZE) == 0) {
453 std::string topname = topic_name(ls1080if->
id(),
"1080");
456 pi.pub =
rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
459 ls1080if->
uid(), topname.c_str());
461 pi.msg.header.frame_id = ls1080if->
frame();
462 pi.msg.angle_min = 0;
463 pi.msg.angle_max = 2*M_PI;
464 pi.msg.angle_increment =
deg2rad(0.5);
465 pi.msg.ranges.resize(1080);
467 __pubs[ls1080if->
uid()] = pi;
470 __ls1080_ifs.push_back(ls1080if);
482 unsigned int instance_serial)
throw()
484 conditional_close(interface);
489 unsigned int instance_serial)
throw()
491 conditional_close(interface);
495 RosLaserScanThread::conditional_close(
Interface *interface)
throw()
503 std::list<Laser360Interface *>::iterator i;
504 for (i = __ls360_ifs.begin(); i != __ls360_ifs.end(); ++i) {
505 if (*ls360if == **i) {
514 __ls360_ifs.erase(i);
519 }
else if (ls720if) {
520 std::list<Laser720Interface *>::iterator i;
521 for (i = __ls720_ifs.begin(); i != __ls720_ifs.end(); ++i) {
522 if (*ls720if == **i) {
531 __ls720_ifs.erase(i);
537 }
else if (ls1080if) {
538 std::list<Laser1080Interface *>::iterator i;
539 for (i = __ls1080_ifs.begin(); i != __ls1080_ifs.end(); ++i) {
540 if (*ls1080if == **i) {
549 __ls1080_ifs.erase(i);
562 RosLaserScanThread::laser_scan_message_cb(
const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt)
565 __ls_msg_queues[__active_queue].push(msg_evt);
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.
Laser1080Interface Fawkes BlackBoard Interface.
Fawkes library namespace.
void unlock()
Unlock the mutex.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
const char * id() const
Get identifier of interface.
virtual void finalize()
Finalize the thread.
A class for handling time.
virtual const char * what() const
Get primary string.
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.
void bbil_remove_writer_interface(Interface *interface)
Remove an interface to the writer addition/removal watch list.
Logger * logger
This is the Logger member used to access the logger.
virtual void loop()
Code to execute in the thread.
char * frame() const
Get frame value.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
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.
Base class for exceptions in Fawkes.
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.
char * frame() const
Get frame value.
long get_nsec() const
Get nanoseconds.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
bool has_writer() const
Check if there is a writer for the interface.
const char * name() const
Get name of thread.
const char * uid() const
Get unique identifier of interface.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
float * distances() const
Get distances value.
const Time * timestamp() const
Get timestamp of last write.
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.
void bbil_remove_data_interface(Interface *interface)
Remove an interface to the data modification watch list.
long get_sec() const
Get seconds.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual ~RosLaserScanThread()
Destructor.
virtual void init()
Initialize the thread.
float * distances() const
Get distances value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
RosLaserScanThread()
Constructor.
void lock()
Lock this mutex.
unsigned int num_readers() const
Get the number of readers.
Mutex mutual exclusion lock.
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
char * frame() const
Get frame value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Laser720Interface Fawkes BlackBoard Interface.
void bbil_remove_reader_interface(Interface *interface)
Remove an interface to the reader addition/removal watch list.
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.