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