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());
464 unsigned int instance_serial)
throw()
466 conditional_close(interface);
471 unsigned int instance_serial)
throw()
473 conditional_close(interface);
477 RosLaserScanThread::conditional_close(
Interface *interface)
throw()
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);