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 
48 using 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 
57 using namespace fawkes;
58 
59 /** Constructor. */
61 : Thread("LaserLinesThread", Thread::OPMODE_WAITFORWAKEUP),
62  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
63  ConfigurationChangeHandler(CFG_PREFIX),
64  TransformAspect(TransformAspect::BOTH_DEFER_PUBLISHER)
65 {
66 }
67 
68 /** Destructor. */
70 {
71 }
72 
73 void
75 {
76  read_config();
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 
153 void
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 
178 void
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. */
247 void
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 
273 void
274 LaserLinesThread::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 
341 void
342 LaserLinesThread::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 
423 void
424 LaserLinesThread::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) {
499  logger->log_warn(name(),
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 
511 void
512 LaserLinesThread::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
532 void
533 LaserLinesThread::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 
701 void
702 LaserLinesThread::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
LaserLinesThread::loop
virtual void loop()
Code to execute in the thread.
Definition: laser-lines-thread.cpp:179
LineInfo::line_direction
Eigen::Vector3f line_direction
line direction vector
Definition: line_info.h:48
fawkes::LaserLineInterface::set_point_on_line
void set_point_on_line(unsigned int index, const float new_point_on_line)
Set point_on_line value at given index.
Definition: LaserLineInterface.cpp:206
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::LaserLineInterface::set_end_point_1
void set_end_point_1(unsigned int index, const float new_end_point_1)
Set end_point_1 value at given index.
Definition: LaserLineInterface.cpp:372
fawkes::LaserLineInterface::set_length
void set_length(const float new_length)
Set length value.
Definition: LaserLineInterface.cpp:468
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::LaserLineInterface::set_frame_id
void set_frame_id(const char *new_frame_id)
Set frame_id value.
Definition: LaserLineInterface.cpp:100
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:303
LaserLinesThread::~LaserLinesThread
virtual ~LaserLinesThread()
Destructor.
Definition: laser-lines-thread.cpp:69
LineInfo::base_point
Eigen::Vector3f base_point
optimized closest point on line
Definition: line_info.h:50
fawkes::PointCloudManager::remove_pointcloud
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud_manager.cpp:66
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
TrackedLineInfo::visibility_history
int visibility_history
visibility history of this line, negative for "no sighting"
Definition: line_info.h:63
fawkes::LaserLineInterface::set_bearing
void set_bearing(const float new_bearing)
Set bearing value.
Definition: LaserLineInterface.cpp:304
fawkes::ConfigurationChangeHandler
Interface for configuration change handling.
Definition: change_handler.h:32
LaserLinesThread::init
virtual void init()
Initialize the thread.
Definition: laser-lines-thread.cpp:74
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
TrackedLineInfo::smooth
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Definition: line_info.h:65
fawkes::SwitchInterface::DisableSwitchMessage
DisableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:136
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
LineInfo::cloud
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Definition: line_info.h:55
LineInfo::end_point_1
Eigen::Vector3f end_point_1
line segment end point
Definition: line_info.h:52
LaserLinesThread::LaserLinesThread
LaserLinesThread()
Constructor.
Definition: laser-lines-thread.cpp:60
fawkes::SwitchInterface::EnableSwitchMessage
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:116
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes::SwitchInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: SwitchInterface.cpp:105
fawkes
Fawkes library namespace.
LineInfo
Line information container.
Definition: line_info.h:40
fawkes::PointCloudManager::get_pointcloud
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Definition: pointcloud_manager.h:91
fawkes::TransformAspect::tf_add_publisher
void tf_add_publisher(const char *frame_id_format,...)
Late add of publisher.
Definition: tf.cpp:186
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
pcl::PointCloud< ColorPointType >
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::LaserLineInterface::visibility_history
int32_t visibility_history() const
Get visibility_history value.
Definition: LaserLineInterface.cpp:116
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::LaserLineInterface::set_visibility_history
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Definition: LaserLineInterface.cpp:142
fawkes::LaserLineInterface::set_end_point_frame_1
void set_end_point_frame_1(const char *new_end_point_frame_1)
Set end_point_frame_1 value.
Definition: LaserLineInterface.cpp:502
LineInfo::bearing
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Definition: line_info.h:44
TrackedLineInfo
Container for a line with tracking and smoothing info.
Definition: line_info.h:60
TrackedLineInfo::interface_idx
int interface_idx
id of the interface, this line is written to, -1 when not yet assigned
Definition: line_info.h:62
LaserLinesThread::finalize
virtual void finalize()
Finalize the thread.
Definition: laser-lines-thread.cpp:154
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
fawkes::TimeTracker
Time tracking utility.
Definition: tracker.h:37
fawkes::tf::Transformer::get_cache_time
float get_cache_time() const
Get cache time.
Definition: transformer.cpp:128
LaserLinesThread::read_config
virtual void read_config()
Read all configuration values.
Definition: laser-lines-thread.cpp:248
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
LineInfo::length
float length
length of the detecte line segment
Definition: line_info.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::PointCloudManager::add_pointcloud
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
Definition: pointcloud_manager.h:78
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::SwitchInterface::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:83
LineInfo::end_point_2
Eigen::Vector3f end_point_2
line segment end point
Definition: line_info.h:53
fawkes::PointCloudAspect::pcl_manager
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
fawkes::LaserLineInterface
LaserLineInterface Fawkes BlackBoard Interface.
Definition: LaserLineInterface.h:34
fawkes::RefPtr::reset
void reset()
Reset pointer.
Definition: refptr.h:455
fawkes::LaserLineInterface::set_end_point_2
void set_end_point_2(unsigned int index, const float new_end_point_2)
Set end_point_2 value at given index.
Definition: LaserLineInterface.cpp:439
fawkes::Configuration::add_change_handler
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Definition: config.cpp:603
fawkes::LaserLineInterface::set_line_direction
void set_line_direction(unsigned int index, const float new_line_direction)
Set line_direction value at given index.
Definition: LaserLineInterface.cpp:269
fawkes::TransformAspect::tf_publishers
std::map< std::string, tf::TransformPublisher * > tf_publishers
Map of transform publishers created through the aspect.
Definition: tf.h:70
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
TrackedLineInfo::raw
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Definition: line_info.h:64
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::LaserLineInterface::set_end_point_frame_2
void set_end_point_frame_2(const char *new_end_point_frame_2)
Set end_point_frame_2 value.
Definition: LaserLineInterface.cpp:536
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36