Fawkes API Fawkes Development Version
laser-lines-thread.cpp
1
2/***************************************************************************
3 * laser-lines-thread.cpp - Thread to detect lines in 2D laser data
4 *
5 * Created: Fri May 23 18:12:14 2014
6 * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "laser-lines-thread.h"
23
24#include "line_colors.h"
25#include "line_func.h"
26
27#include <pcl_utils/comparisons.h>
28#include <pcl_utils/utils.h>
29#include <utils/math/angle.h>
30#include <utils/time/wait.h>
31#ifdef USE_TIMETRACKER
32# include <utils/time/tracker.h>
33#endif
34#include <baseapp/run.h>
35#include <interfaces/LaserLineInterface.h>
36#include <interfaces/Position3DInterface.h>
37#include <interfaces/SwitchInterface.h>
38#include <utils/time/tracker_macros.h>
39
40#ifdef HAVE_VISUAL_DEBUGGING
41# include <ros/ros.h>
42#endif
43
44#include <cmath>
45#include <iostream>
46#include <limits>
47
48using namespace std;
49
50#define CFG_PREFIX "/laser-lines/"
51
52/** @class LaserLinesThread "laser-lines-thread.h"
53 * Main thread of laser-lines plugin.
54 * @author Tim Niemueller
55 */
56
57using namespace fawkes;
58
59/** Constructor. */
61: Thread("LaserLinesThread", Thread::OPMODE_WAITFORWAKEUP),
62 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
64 TransformAspect(TransformAspect::BOTH_DEFER_PUBLISHER)
65{
66}
67
68/** Destructor. */
70{
71}
72
73void
75{
78
79 finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pcl_.c_str());
80 input_ = pcl_utils::cloudptr_from_refptr(finput_);
81
82 //step 2: configure the interfaces
83 try {
84 //2.1:format the interface-arrays
85 line_ifs_.resize(cfg_max_num_lines_, NULL);
86 if (cfg_moving_avg_enabled_) {
87 line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
88 }
89 //2.2:open interfaces for writing
90 for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
91 //2.2.1:create id name /laser-lines/(i+1)
92 char *tmp;
93 if (asprintf(&tmp, "/laser-lines/%u", i + 1) != -1) {
94 // Copy to get memory freed on exception
95 std::string id = tmp;
96 free(tmp);
97
98 //2.2.2: actually opening the interfaces
99 line_ifs_[i] = blackboard->open_for_writing<LaserLineInterface>(id.c_str());
100 if (cfg_moving_avg_enabled_) {
101 line_avg_ifs_[i] =
102 blackboard->open_for_writing<LaserLineInterface>((id + "/moving_avg").c_str());
103 }
104 }
105 }
106
107 //step 3:configure switch interface
108 switch_if_ = NULL;
109 switch_if_ = blackboard->open_for_writing<SwitchInterface>("laser-lines");
110
111 bool autostart = true;
112 try {
113 autostart = config->get_bool(CFG_PREFIX "auto-start");
114 } catch (Exception &e) {
115 } // ignored, use default
116 switch_if_->set_enabled(autostart);
117 switch_if_->write();
118 } catch (Exception &e) {
119 //step 4:close all interfaces if something went wrong
120 for (size_t i = 0; i < line_ifs_.size(); ++i) {
121 blackboard->close(line_ifs_[i]);
122 if (cfg_moving_avg_enabled_) {
123 blackboard->close(line_avg_ifs_[i]);
124 }
125 }
126 blackboard->close(switch_if_);
127 throw;
128 }
129
130 flines_ = new pcl::PointCloud<ColorPointType>();
131 flines_->header.frame_id = finput_->header.frame_id;
132 flines_->is_dense = false;
133 pcl_manager->add_pointcloud<ColorPointType>("laser-lines", flines_);
134 lines_ = pcl_utils::cloudptr_from_refptr(flines_);
135
136 loop_count_ = 0;
137
138#ifdef USE_TIMETRACKER
139 tt_ = new TimeTracker();
140 tt_loopcount_ = 0;
141 ttc_full_loop_ = tt_->add_class("Full Loop");
142 ttc_msgproc_ = tt_->add_class("Message Processing");
143 ttc_extract_lines_ = tt_->add_class("Line Extraction");
144 ttc_clustering_ = tt_->add_class("Clustering");
145#endif
146#ifdef HAVE_VISUAL_DEBUGGING
147 vispub_ = new ros::Publisher();
148 *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
149 last_id_num_ = 0;
150#endif
151}
152
153void
155{
156#ifdef HAVE_VISUAL_DEBUGGING
157 vispub_->shutdown();
158 delete vispub_;
159#endif
160
161 input_.reset();
162 lines_.reset();
163
164 pcl_manager->remove_pointcloud("laser-lines");
165
166 for (size_t i = 0; i < line_ifs_.size(); ++i) {
167 blackboard->close(line_ifs_[i]);
168 if (cfg_moving_avg_enabled_) {
169 blackboard->close(line_avg_ifs_[i]);
170 }
171 }
172 blackboard->close(switch_if_);
173
174 finput_.reset();
175 flines_.reset();
176}
177
178void
180{
181 TIMETRACK_START(ttc_full_loop_);
182
183 ++loop_count_;
184
185 TIMETRACK_START(ttc_msgproc_);
186
187 //step 1:deal with switch on/off-messages
188 while (!switch_if_->msgq_empty()) {
189 if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
190 switch_if_->set_enabled(true);
191 switch_if_->write();
192 } else if (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
193 // line data is now invalid
194 for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
195 line_ifs_[i]->set_visibility_history(0);
196 line_ifs_[i]->write();
197 }
198
199 switch_if_->set_enabled(false);
200 switch_if_->write();
201 }
202
203 switch_if_->msgq_pop();
204 }
205
206 //step 2:if switch is off, don't even try to do something
207 if (!switch_if_->is_enabled()) {
208 //TimeWait::wait(250000);
209 return;
210 }
211
212 TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
213
214 if (input_->points.size() <= 10) {
215 // this can happen if run at startup. Since thread runs continuous
216 // and not synchronized with main loop, but point cloud acquisition thread is
217 // synchronized, we might start before any data has been read
218 //logger->log_warn(name(), "Empty voxelized point cloud, omitting loop");
219 //TimeWait::wait(50000);
220
221 for (unsigned int i = 0; i < this->known_lines_.size(); ++i) {
222 known_lines_[i].not_visible_update();
223 }
224 } else {
225 //logger->log_info(name(), "[L %u] total: %zu finite: %zu",
226 // loop_count_, input_->points.size(), in_cloud->points.size());
227 std::vector<LineInfo> linfos = calc_lines<PointType>(input_,
228 cfg_segm_min_inliers_,
229 cfg_segm_max_iterations_,
230 cfg_segm_distance_threshold_,
231 cfg_segm_sample_max_dist_,
232 cfg_cluster_tolerance_,
233 cfg_cluster_quota_,
234 cfg_min_length_,
235 cfg_max_length_,
236 cfg_min_dist_,
237 cfg_max_dist_);
238
239 TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
240 update_lines(linfos);
241 }
242
243 publish_known_lines();
244}
245
246/** Read all configuration values. */
247void
249{
250 cfg_segm_max_iterations_ = config->get_uint(CFG_PREFIX "line_segmentation_max_iterations");
251 cfg_segm_distance_threshold_ =
252 config->get_float(CFG_PREFIX "line_segmentation_distance_threshold");
253 cfg_segm_sample_max_dist_ = config->get_float(CFG_PREFIX "line_segmentation_sample_max_dist");
254 cfg_segm_min_inliers_ = config->get_uint(CFG_PREFIX "line_segmentation_min_inliers");
255 cfg_min_length_ = config->get_float(CFG_PREFIX "line_min_length");
256 cfg_max_length_ = config->get_float(CFG_PREFIX "line_max_length");
257 cfg_min_dist_ = config->get_float(CFG_PREFIX "line_min_distance");
258 cfg_max_dist_ = config->get_float(CFG_PREFIX "line_max_distance");
259 cfg_cluster_tolerance_ = config->get_float(CFG_PREFIX "line_cluster_tolerance");
260 cfg_cluster_quota_ = config->get_float(CFG_PREFIX "line_cluster_quota");
261 cfg_moving_avg_enabled_ = config->get_bool(CFG_PREFIX "moving_avg_enabled");
262 cfg_moving_avg_window_size_ = config->get_uint(CFG_PREFIX "moving_avg_window_size");
263
264 cfg_switch_tolerance_ = config->get_float(CFG_PREFIX "switch_tolerance");
265
266 cfg_input_pcl_ = config->get_string(CFG_PREFIX "input_cloud");
267 //max_num_lines_ resulting in the specified number of interfaces
268 cfg_max_num_lines_ = config->get_uint(CFG_PREFIX "max_num_lines");
269
270 cfg_tracking_frame_id_ = config->get_string("/frames/odom");
271}
272
273void
274LaserLinesThread::update_lines(std::vector<LineInfo> &linfos)
275{
276 size_t num_points = 0;
277 for (size_t i = 0; i < linfos.size(); ++i) {
278 num_points += linfos[i].cloud->points.size();
279 }
280
281 lines_->points.resize(num_points);
282 lines_->height = 1;
283 lines_->width = num_points;
284
285 vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
286 while (known_it != known_lines_.end()) {
287 btScalar min_dist = numeric_limits<btScalar>::max();
288 auto best_match = linfos.end();
289 for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != linfos.end(); ++it_new) {
290 btScalar d = known_it->distance(*it_new);
291 if (d < min_dist) {
292 min_dist = d;
293 best_match = it_new;
294 }
295 }
296 if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
297 known_it->update(*best_match);
298
299 // Important: erase line because all lines remaining after this are considered "new" (see below)
300 linfos.erase(best_match);
301 ++known_it;
302 } else { // No match for this line
303 known_it->not_visible_update();
304 ++known_it;
305 }
306 }
307
308 for (LineInfo &l : linfos) {
309 // Only unmatched lines remaining, so these are the "new" lines
311 finput_->header.frame_id,
312 cfg_tracking_frame_id_,
313 cfg_switch_tolerance_,
314 cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 0,
315 logger,
316 name());
317 tl.update(l);
318 known_lines_.push_back(tl);
319 }
320
321 // When there are too many lines, delete the ones with negative and lowest
322 // visibility history
323 std::sort(known_lines_.begin(),
324 known_lines_.end(),
325 [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool {
326 return l1.visibility_history < l2.visibility_history;
327 });
328 while (known_lines_.size() > cfg_max_num_lines_ && known_lines_[0].visibility_history < 0)
329 known_lines_.erase(known_lines_.begin());
330
331 // When there are still too many lines, delete the ones farthest away
332 std::sort(known_lines_.begin(),
333 known_lines_.end(),
334 [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool {
335 return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
336 });
337 while (known_lines_.size() > cfg_max_num_lines_)
338 known_lines_.erase(known_lines_.end() - 1);
339}
340
341void
342LaserLinesThread::publish_known_lines()
343{
344 // set line parameters
345 size_t oi = 0;
346 for (size_t i = 0; i < known_lines_.size(); ++i) {
347 const TrackedLineInfo &info = known_lines_[i];
348
349 if (info.raw.cloud) {
350 for (size_t p = 0; p < info.raw.cloud->points.size(); ++p) {
351 ColorPointType &out_point = lines_->points[oi++];
352 PointType & in_point = info.raw.cloud->points[p];
353 out_point.x = in_point.x;
354 out_point.y = in_point.y;
355 out_point.z = in_point.z;
356
357 if (i < MAX_LINES) {
358 out_point.r = line_colors[i][0];
359 out_point.g = line_colors[i][1];
360 out_point.b = line_colors[i][2];
361 } else {
362 out_point.r = out_point.g = out_point.b = 1.0;
363 }
364 }
365 }
366 }
367
368 //set interfaces
369 for (unsigned int line_if_idx = 0; line_if_idx < cfg_max_num_lines_; ++line_if_idx) {
370 int known_line_idx = -1;
371 //find the associated known line to the interface
372 //if there is none, make a new association with the first, newfound line
373 //otherwise clear the interface (indicated by known_line_idx = -1)
374 for (unsigned int test_idx = 0; test_idx < known_lines_.size(); ++test_idx) {
375 const TrackedLineInfo &info = known_lines_[test_idx];
376 if (info.interface_idx == (int)line_if_idx) {
377 known_line_idx = test_idx;
378 break;
379 }
380 if (info.interface_idx == -1 && known_line_idx == -1) {
381 known_line_idx = test_idx;
382 }
383 }
384
385 if (known_line_idx == -1) {
386 set_empty_interface(line_ifs_[line_if_idx]);
387 if (cfg_moving_avg_enabled_) {
388 set_empty_interface(line_avg_ifs_[line_if_idx]);
389 }
390 } else {
391 known_lines_[known_line_idx].interface_idx = line_if_idx;
392 const TrackedLineInfo &info = known_lines_[known_line_idx];
393 set_interface(line_if_idx, line_ifs_[line_if_idx], false, info, finput_->header.frame_id);
394 if (cfg_moving_avg_enabled_) {
395 set_interface(
396 line_if_idx, line_avg_ifs_[line_if_idx], true, info, finput_->header.frame_id);
397 }
398 }
399 }
400
401#ifdef HAVE_VISUAL_DEBUGGING
402 publish_visualization(known_lines_, "laser_lines", "laser_lines_moving_average");
403#endif
404
405 if (finput_->header.frame_id == ""
406 && fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
407 logger->log_error(name(), "Empty frame ID");
408 }
409 flines_->header.frame_id = finput_->header.frame_id;
410 pcl_utils::copy_time(finput_, flines_);
411
412 TIMETRACK_END(ttc_clustering_);
413 TIMETRACK_END(ttc_full_loop_);
414
415#ifdef USE_TIMETRACKER
416 if (++tt_loopcount_ >= 5) {
417 tt_loopcount_ = 0;
418 tt_->print_to_stdout();
419 }
420#endif
421}
422
423void
424LaserLinesThread::set_interface(unsigned int idx,
426 bool moving_average,
427 const TrackedLineInfo & tinfo,
428 const std::string & frame_id)
429{
430 const LineInfo &info = moving_average ? tinfo.smooth : tinfo.raw;
431
433
434 //add the offset and publish
435 float if_point_on_line[3] = {info.base_point[0], info.base_point[1], info.base_point[2]};
436 float if_line_direction[3] = {info.line_direction[0],
437 info.line_direction[1],
438 info.line_direction[2]};
439 float if_end_point_1[3] = {info.end_point_1[0], info.end_point_1[1], info.end_point_1[2]};
440 float if_end_point_2[3] = {info.end_point_2[0], info.end_point_2[1], info.end_point_2[2]};
441
442 iface->set_point_on_line(if_point_on_line);
443 iface->set_line_direction(if_line_direction);
444 iface->set_frame_id(frame_id.c_str());
445 iface->set_bearing(info.bearing);
446 iface->set_length(info.length);
447 iface->set_end_point_1(if_end_point_1);
448 iface->set_end_point_2(if_end_point_2);
449
450 // this makes the usual assumption that the laser data is in the X-Y plane
451 fawkes::Time now(clock);
452 std::string frame_name_1, frame_name_2;
453 char * tmp;
454 std::string avg = moving_average ? "avg_" : "";
455 if (asprintf(&tmp, "laser_line_%s%u_e1", avg.c_str(), idx + 1) != -1) {
456 frame_name_1 = tmp;
457 free(tmp);
458 }
459 if (asprintf(&tmp, "laser_line_%s%u_e2", avg.c_str(), idx + 1) != -1) {
460 frame_name_2 = tmp;
461 free(tmp);
462 }
463
464 iface->set_end_point_frame_1(frame_name_1.c_str());
465 iface->set_end_point_frame_2(frame_name_2.c_str());
466
467 if (tinfo.visibility_history <= 0) {
468 iface->write();
469 return;
470 }
471
472 if (frame_name_1 != "" && frame_name_2 != "") {
473 Eigen::Vector3f bp_unit = info.base_point / info.base_point.norm();
474 double dotprod = Eigen::Vector3f::UnitX().dot(bp_unit);
475 double angle = acos(dotprod) + M_PI;
476
477 if (info.base_point[1] < 0.)
478 angle = fabs(angle) * -1.;
479
480 tf::Transform t1(tf::Quaternion(tf::Vector3(0, 0, 1), angle),
481 tf::Vector3(info.end_point_1[0], info.end_point_1[1], info.end_point_1[2]));
482 tf::Transform t2(tf::Quaternion(tf::Vector3(0, 0, 1), angle),
483 tf::Vector3(info.end_point_2[0], info.end_point_2[1], info.end_point_2[2]));
484
485 try {
486 auto tf_it_1 = tf_publishers.find(frame_name_1);
487 if (tf_it_1 == tf_publishers.end()) {
488 tf_add_publisher(frame_name_1.c_str());
489 tf_it_1 = tf_publishers.find(frame_name_1);
490 }
491 auto tf_it_2 = tf_publishers.find(frame_name_2);
492 if (tf_it_2 == tf_publishers.end()) {
493 tf_add_publisher(frame_name_2.c_str());
494 tf_it_2 = tf_publishers.find(frame_name_2);
495 }
496 tf_it_1->second->send_transform(t1, now, frame_id, frame_name_1);
497 tf_it_2->second->send_transform(t2, now, frame_id, frame_name_2);
498 } catch (Exception &e) {
500 "Failed to publish laser_line_%u_* transforms, exception follows",
501 idx + 1);
502 logger->log_warn(name(), e);
503 }
504 } else {
505 logger->log_warn(name(), "Failed to determine frame names");
506 }
507
508 iface->write();
509}
510
511void
512LaserLinesThread::set_empty_interface(fawkes::LaserLineInterface *iface) const
513{
514 int visibility_history = iface->visibility_history();
515 if (visibility_history <= 0) {
516 iface->set_visibility_history(visibility_history - 1);
517 } else {
518 iface->set_visibility_history(-1);
519 float zero_vector[3] = {0, 0, 0};
520 iface->set_point_on_line(zero_vector);
521 iface->set_line_direction(zero_vector);
522 iface->set_end_point_1(zero_vector);
523 iface->set_end_point_2(zero_vector);
524 iface->set_bearing(0);
525 iface->set_length(0);
526 iface->set_frame_id("");
527 }
528 iface->write();
529}
530
531#ifdef HAVE_VISUAL_DEBUGGING
532void
533LaserLinesThread::publish_visualization_add_line(visualization_msgs::MarkerArray &m,
534 unsigned int & idnum,
535 const LineInfo & info,
536 const size_t i,
537 const std::string & marker_namespace,
538 const std::string & name_suffix)
539{
540 /*
541 visualization_msgs::Marker basevec;
542 basevec.header.frame_id = finput_->header.frame_id;
543 basevec.header.stamp = ros::Time::now();
544 basevec.ns = marker_namespace;
545 basevec.id = idnum++;
546 basevec.type = visualization_msgs::Marker::ARROW;
547 basevec.action = visualization_msgs::Marker::ADD;
548 basevec.points.resize(2);
549 basevec.points[0].x = basevec.points[0].y = basevec.points[0].z = 0.;
550 basevec.points[1].x = info.point_on_line[0];
551 basevec.points[1].y = info.point_on_line[1];
552 basevec.points[1].z = info.point_on_line[2];
553 basevec.scale.x = 0.02;
554 basevec.scale.y = 0.04;
555 basevec.color.r = 1.0;
556 basevec.color.g = basevec.color.b = 0.;
557 basevec.color.a = 1.0;
558 basevec.lifetime = ros::Duration(2, 0);
559 m.markers.push_back(basevec);
560 */
561
562 visualization_msgs::Marker dirvec;
563 dirvec.header.frame_id = finput_->header.frame_id;
564 dirvec.header.stamp = ros::Time::now();
565 dirvec.ns = marker_namespace;
566 dirvec.id = idnum++;
567 dirvec.type = visualization_msgs::Marker::ARROW;
568 dirvec.action = visualization_msgs::Marker::ADD;
569 dirvec.points.resize(2);
570 dirvec.points[0].x = info.base_point[0];
571 dirvec.points[0].y = info.base_point[1];
572 dirvec.points[0].z = info.base_point[2];
573 dirvec.points[1].x = info.base_point[0] + info.line_direction[0];
574 dirvec.points[1].y = info.base_point[1] + info.line_direction[1];
575 dirvec.points[1].z = info.base_point[2] + info.line_direction[2];
576 dirvec.scale.x = 0.02;
577 dirvec.scale.y = 0.04;
578 dirvec.color.r = 0.0;
579 dirvec.color.g = 1.0;
580 dirvec.color.b = 0.f;
581 dirvec.color.a = 1.0;
582 dirvec.lifetime = ros::Duration(2, 0);
583 m.markers.push_back(dirvec);
584
585 visualization_msgs::Marker testvec;
586 testvec.header.frame_id = finput_->header.frame_id;
587 testvec.header.stamp = ros::Time::now();
588 testvec.ns = marker_namespace;
589 testvec.id = idnum++;
590 testvec.type = visualization_msgs::Marker::ARROW;
591 testvec.action = visualization_msgs::Marker::ADD;
592 testvec.points.resize(2);
593 testvec.points[0].x = 0; //info.point_on_line[0];
594 testvec.points[0].y = 0; //info.point_on_line[1];
595 testvec.points[0].z = 0; //info.point_on_line[2];
596 testvec.points[1].x = info.base_point[0];
597 testvec.points[1].y = info.base_point[1];
598 testvec.points[1].z = info.base_point[2];
599 testvec.scale.x = 0.02;
600 testvec.scale.y = 0.04;
601 testvec.color.r = line_colors[i][0] / 255.;
602 testvec.color.g = line_colors[i][1] / 255.;
603 testvec.color.b = line_colors[i][2] / 255.;
604 testvec.color.a = 1.0;
605 testvec.lifetime = ros::Duration(2, 0);
606 m.markers.push_back(testvec);
607
608 char *tmp;
609 if (asprintf(&tmp, "L_%zu%s", i + 1, name_suffix.c_str()) != -1) {
610 // Copy to get memory freed on exception
611 std::string id = tmp;
612 free(tmp);
613
614 visualization_msgs::Marker text;
615 text.header.frame_id = finput_->header.frame_id;
616 text.header.stamp = ros::Time::now();
617 text.ns = marker_namespace;
618 text.id = idnum++;
619 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
620 text.action = visualization_msgs::Marker::ADD;
621 text.pose.position.x = info.base_point[0];
622 text.pose.position.y = info.base_point[1];
623 text.pose.position.z = info.base_point[2] + .15;
624 text.pose.orientation.w = 1.;
625 text.scale.z = 0.15;
626 text.color.r = text.color.g = text.color.b = 1.0f;
627 text.color.a = 1.0;
628 text.lifetime = ros::Duration(2, 0);
629 text.text = id;
630 m.markers.push_back(text);
631 }
632
633 if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
634 visualization_msgs::Marker sphere_ep_1;
635 sphere_ep_1.header.frame_id = finput_->header.frame_id;
636 sphere_ep_1.header.stamp = ros::Time::now();
637 sphere_ep_1.ns = marker_namespace;
638 sphere_ep_1.id = idnum++;
639 sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
640 sphere_ep_1.action = visualization_msgs::Marker::ADD;
641 sphere_ep_1.pose.position.x = info.end_point_1[0];
642 sphere_ep_1.pose.position.y = info.end_point_1[1];
643 sphere_ep_1.pose.position.z = info.end_point_1[2];
644 sphere_ep_1.pose.orientation.w = 1.;
645 sphere_ep_1.scale.x = 0.05;
646 sphere_ep_1.scale.y = 0.05;
647 sphere_ep_1.scale.z = 0.05;
648 sphere_ep_1.color.r = line_colors[i][0] / 255.;
649 sphere_ep_1.color.g = line_colors[i][1] / 255.;
650 sphere_ep_1.color.b = line_colors[i][2] / 255.;
651 sphere_ep_1.color.a = 1.0;
652 sphere_ep_1.lifetime = ros::Duration(2, 0);
653 m.markers.push_back(sphere_ep_1);
654
655 visualization_msgs::Marker sphere_ep_2;
656 sphere_ep_2.header.frame_id = finput_->header.frame_id;
657 sphere_ep_2.header.stamp = ros::Time::now();
658 sphere_ep_2.ns = marker_namespace;
659 sphere_ep_2.id = idnum++;
660 sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
661 sphere_ep_2.action = visualization_msgs::Marker::ADD;
662 sphere_ep_2.pose.position.x = info.end_point_2[0];
663 sphere_ep_2.pose.position.y = info.end_point_2[1];
664 sphere_ep_2.pose.position.z = info.end_point_2[2];
665 sphere_ep_2.pose.orientation.w = 1.;
666 sphere_ep_2.scale.x = 0.05;
667 sphere_ep_2.scale.y = 0.05;
668 sphere_ep_2.scale.z = 0.05;
669 sphere_ep_2.color.r = line_colors[i][0] / 255.;
670 sphere_ep_2.color.g = line_colors[i][1] / 255.;
671 sphere_ep_2.color.b = line_colors[i][2] / 255.;
672 sphere_ep_2.color.a = 1.0;
673 sphere_ep_2.lifetime = ros::Duration(2, 0);
674 m.markers.push_back(sphere_ep_2);
675
676 visualization_msgs::Marker lineseg;
677 lineseg.header.frame_id = finput_->header.frame_id;
678 lineseg.header.stamp = ros::Time::now();
679 lineseg.ns = marker_namespace;
680 lineseg.id = idnum++;
681 lineseg.type = visualization_msgs::Marker::LINE_LIST;
682 lineseg.action = visualization_msgs::Marker::ADD;
683 lineseg.points.resize(2);
684 lineseg.points[0].x = info.end_point_1[0];
685 lineseg.points[0].y = info.end_point_1[1];
686 lineseg.points[0].z = info.end_point_1[2];
687 lineseg.points[1].x = info.end_point_2[0];
688 lineseg.points[1].y = info.end_point_2[1];
689 lineseg.points[1].z = info.end_point_2[2];
690 lineseg.scale.x = 0.02;
691 lineseg.scale.y = 0.04;
692 lineseg.color.r = line_colors[i][0] / 255.;
693 lineseg.color.g = line_colors[i][1] / 255.;
694 lineseg.color.b = line_colors[i][2] / 255.;
695 lineseg.color.a = 1.0;
696 lineseg.lifetime = ros::Duration(2, 0);
697 m.markers.push_back(lineseg);
698 }
699}
700
701void
702LaserLinesThread::publish_visualization(const std::vector<TrackedLineInfo> &linfos,
703 const std::string & marker_namespace,
704 const std::string & avg_marker_namespace)
705{
706 visualization_msgs::MarkerArray m;
707
708 unsigned int idnum = 0;
709
710 for (size_t i = 0; i < linfos.size(); ++i) {
711 const TrackedLineInfo &info = linfos[i];
712 if (info.visibility_history > 0) {
713 publish_visualization_add_line(m, idnum, info.raw, info.interface_idx, marker_namespace);
714 publish_visualization_add_line(
715 m, idnum, info.smooth, info.interface_idx, avg_marker_namespace, "_avg");
716 }
717 }
718
719 for (size_t i = idnum; i < last_id_num_; ++i) {
720 visualization_msgs::Marker delop;
721 delop.header.frame_id = finput_->header.frame_id;
722 delop.header.stamp = ros::Time::now();
723 delop.ns = marker_namespace;
724 delop.id = i;
725 delop.action = visualization_msgs::Marker::DELETE;
726 m.markers.push_back(delop);
727 }
728 last_id_num_ = idnum;
729
730 vispub_->publish(m);
731}
732#endif
virtual ~LaserLinesThread()
Destructor.
virtual void finalize()
Finalize the thread.
LaserLinesThread()
Constructor.
virtual void read_config()
Read all configuration values.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Line information container.
Definition: line_info.h:40
float length
length of the detecte line segment
Definition: line_info.h:45
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:55
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:48
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:52
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:44
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:50
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:53
Container for a line with tracking and smoothing info.
Definition: line_info.h:60
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Definition: line_info.h:64
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Definition: line_info.h:65
int interface_idx
id of the interface, this line is written to, -1 when not yet assigned
Definition: line_info.h:62
int visibility_history
visibility history of this line, negative for "no sighting"
Definition: line_info.h:63
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration change handling.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:603
Base class for exceptions in Fawkes.
Definition: exception.h:36
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
LaserLineInterface Fawkes BlackBoard Interface.
void set_end_point_1(unsigned int index, const float new_end_point_1)
Set end_point_1 value at given index.
void set_end_point_frame_1(const char *new_end_point_frame_1)
Set end_point_frame_1 value.
int32_t visibility_history() const
Get visibility_history value.
void set_line_direction(unsigned int index, const float new_line_direction)
Set line_direction value at given index.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_point_on_line(unsigned int index, const float new_point_on_line)
Set point_on_line value at given index.
void set_length(const float new_length)
Set length value.
void set_frame_id(const char *new_frame_id)
Set frame_id value.
void set_bearing(const float new_bearing)
Set bearing value.
void set_end_point_frame_2(const char *new_end_point_frame_2)
Set end_point_frame_2 value.
void set_end_point_2(unsigned int index, const float new_end_point_2)
Set end_point_2 value at given index.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void remove_pointcloud(const char *id)
Remove the point cloud.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
void reset()
Reset pointer.
Definition: refptr.h:455
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:186
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
float get_cache_time() const
Get cache time.
Fawkes library namespace.