Fawkes API  Fawkes Development Version
laser-cluster-thread.cpp
1 
2 /***************************************************************************
3  * laser-cluster-thread.cpp - Thread to detect a cluster in 2D laser data
4  *
5  * Created: Sun Apr 21 01:27:10 2013
6  * Copyright 2011-2015 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-cluster-thread.h"
23 #include "cluster_colors.h"
24 
25 #include <pcl_utils/utils.h>
26 #include <pcl_utils/comparisons.h>
27 #include <utils/time/wait.h>
28 #include <utils/math/angle.h>
29 #include <baseapp/run.h>
30 #ifdef USE_TIMETRACKER
31 # include <utils/time/tracker.h>
32 #endif
33 #include <utils/time/tracker_macros.h>
34 
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 #include <pcl/sample_consensus/method_types.h>
38 #include <pcl/sample_consensus/model_types.h>
39 #include <pcl/segmentation/extract_clusters.h>
40 #include <pcl/filters/extract_indices.h>
41 #include <pcl/surface/convex_hull.h>
42 #include <pcl/search/kdtree.h>
43 #include <pcl/filters/passthrough.h>
44 #include <pcl/filters/project_inliers.h>
45 #include <pcl/filters/conditional_removal.h>
46 #include <pcl/common/centroid.h>
47 #include <pcl/common/transforms.h>
48 #include <pcl/common/distances.h>
49 
50 #include <interfaces/Position3DInterface.h>
51 #include <interfaces/SwitchInterface.h>
52 #include <interfaces/LaserClusterInterface.h>
53 
54 #include <iostream>
55 #include <limits>
56 #include <cmath>
57 
58 using namespace std;
59 
60 /** @class LaserClusterThread "laser-cluster-thread.h"
61  * Main thread of laser-cluster plugin.
62  * @author Tim Niemueller
63  */
64 
65 using namespace fawkes;
66 
67 
68 /** Constructor.
69  * @param cfg_name configuration name
70  * @param cfg_prefix configuration path prefix
71  */
72 LaserClusterThread::LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
73  : Thread("LaserClusterThread", Thread::OPMODE_WAITFORWAKEUP),
74  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
75  TransformAspect(TransformAspect::ONLY_LISTENER)
76 {
77  set_name("LaserClusterThread(%s)", cfg_name.c_str());
78  cfg_name_ = cfg_name;
79  cfg_prefix_ = cfg_prefix;
80 }
81 
82 
83 /** Destructor. */
85 {
86 }
87 
88 
89 void
91 {
92  cfg_line_removal_ = config->get_bool(cfg_prefix_+"line_removal/enable");
93  if (cfg_line_removal_) {
94  cfg_segm_max_iterations_ =
95  config->get_uint(cfg_prefix_+"line_removal/segmentation_max_iterations");
96  cfg_segm_distance_threshold_ =
97  config->get_float(cfg_prefix_+"line_removal/segmentation_distance_threshold");
98  cfg_segm_min_inliers_ =
99  config->get_uint(cfg_prefix_+"line_removal/segmentation_min_inliers");
100  cfg_segm_sample_max_dist_ =
101  config->get_float(cfg_prefix_+"line_removal/segmentation_sample_max_dist");
102  cfg_line_min_length_ =
103  config->get_float(cfg_prefix_+"line_removal/min_length");
104  }
105  cfg_switch_tolerance_ = config->get_float(cfg_prefix_+"switch_tolerance");
106  cfg_cluster_tolerance_ = config->get_float(cfg_prefix_+"clustering/tolerance");
107  cfg_cluster_min_size_ = config->get_uint(cfg_prefix_+"clustering/min_size");
108  cfg_cluster_max_size_ = config->get_uint(cfg_prefix_+"clustering/max_size");
109  cfg_input_pcl_ = config->get_string(cfg_prefix_+"input_cloud");
110  cfg_result_frame_ = config->get_string(cfg_prefix_+"result_frame");
111 
112  cfg_use_bbox_ = false;
113  cfg_bbox_min_x_ = 0.0;
114  cfg_bbox_max_x_ = 0.0;
115  try {
116  cfg_bbox_min_x_ = config->get_float(cfg_prefix_+"bounding-box/min_x");
117  cfg_bbox_max_x_ = config->get_float(cfg_prefix_+"bounding-box/max_x");
118  cfg_bbox_min_y_ = config->get_float(cfg_prefix_+"bounding-box/min_y");
119  cfg_bbox_max_y_ = config->get_float(cfg_prefix_+"bounding-box/max_y");
120  cfg_use_bbox_ = true;
121  } catch (Exception &e) {} // ignored, use default
122  cfg_offset_x_ = config->get_float(cfg_prefix_+"offsets/x");
123  cfg_offset_y_ = config->get_float(cfg_prefix_+"offsets/y");
124  cfg_offset_z_ = config->get_float(cfg_prefix_+"offsets/z");
125  cfg_max_num_clusters_ = config->get_uint(cfg_prefix_+"max_num_clusters");
126 
127  cfg_selection_mode_ = SELECT_MIN_ANGLE;
128  try {
129  std::string selmode = config->get_string(cfg_prefix_+"cluster_selection_mode");
130  if (selmode == "min-angle") {
131  cfg_selection_mode_ = SELECT_MIN_ANGLE;
132  } else if (selmode == "min-dist") {
133  cfg_selection_mode_ = SELECT_MIN_DIST;
134  } else {
135  logger->log_warn(name(), "Invalid selection mode, using min angle");
136  }
137  } catch (Exception &e) {} // ignored, use default
138 
139  current_max_x_ = cfg_bbox_max_x_;
140 
141  finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pcl_.c_str());
142  input_ = pcl_utils::cloudptr_from_refptr(finput_);
143 
144  try {
145  double rotation[4] = {0., 0., 0., 1.};
146  cluster_pos_ifs_.resize(cfg_max_num_clusters_, NULL);
147  for (unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
148  cluster_pos_ifs_[i] =
149  blackboard->open_for_writing_f<Position3DInterface>("/laser-cluster/%s/%u",
150  cfg_name_.c_str(), i + 1);
151  cluster_pos_ifs_[i]->set_rotation(rotation);
152  cluster_pos_ifs_[i]->write();
153  }
154 
155  switch_if_ = NULL;
156  switch_if_ =
157  blackboard->open_for_writing_f<SwitchInterface>("/laser-cluster/%s", cfg_name_.c_str());
158 
159  config_if_ = NULL;
160  config_if_ = blackboard->open_for_writing_f<LaserClusterInterface>("/laser-cluster/%s",
161  cfg_name_.c_str());
162 
163  config_if_->set_max_x(current_max_x_);
164  if (cfg_selection_mode_ == SELECT_MIN_DIST) {
165  config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_DIST);
166  } else {
167  config_if_->set_selection_mode(LaserClusterInterface::SELMODE_MIN_ANGLE);
168  }
169  config_if_->write();
170 
171  bool autostart = true;
172  try {
173  autostart = config->get_bool(cfg_prefix_+"auto-start");
174  } catch (Exception &e) {} // ignored, use default
175  switch_if_->set_enabled(autostart);
176  switch_if_->write();
177  } catch (Exception &e) {
178  for (size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
179  blackboard->close(cluster_pos_ifs_[i]);
180  }
181  blackboard->close(switch_if_);
182  blackboard->close(config_if_);
183  throw;
184  }
185 
186  fclusters_ = new pcl::PointCloud<ColorPointType>();
187  fclusters_->header.frame_id = finput_->header.frame_id;
188  fclusters_->is_dense = false;
189  char *output_cluster_name;
190  if (asprintf(&output_cluster_name, "/laser-cluster/%s", cfg_name_.c_str()) != -1) {
191  output_cluster_name_ = output_cluster_name;
192  free(output_cluster_name);
193  pcl_manager->add_pointcloud<ColorPointType>(output_cluster_name_.c_str(), fclusters_);
194  }
195  clusters_ = pcl_utils::cloudptr_from_refptr(fclusters_);
196 
197  fclusters_labeled_ = new pcl::PointCloud<LabelPointType>();
198  fclusters_labeled_->header.frame_id = finput_->header.frame_id;
199  fclusters_labeled_->is_dense = false;
200  char *output_cluster_labeled_name;
201  if (asprintf(&output_cluster_labeled_name,
202  "/laser-cluster/%s-labeled", cfg_name_.c_str()) != -1) {
203  output_cluster_labeled_name_ = output_cluster_labeled_name;
204  free(output_cluster_labeled_name);
205  pcl_manager->add_pointcloud<LabelPointType>(output_cluster_labeled_name_.c_str(),
206  fclusters_labeled_);
207  }
208  clusters_labeled_ = pcl_utils::cloudptr_from_refptr(fclusters_labeled_);
209 
210  seg_.setOptimizeCoefficients(true);
211  seg_.setModelType(pcl::SACMODEL_LINE);
212  seg_.setMethodType(pcl::SAC_RANSAC);
213  seg_.setMaxIterations(cfg_segm_max_iterations_);
214  seg_.setDistanceThreshold(cfg_segm_distance_threshold_);
215 
216  loop_count_ = 0;
217 
218 #ifdef USE_TIMETRACKER
219  tt_ = new TimeTracker();
220  tt_loopcount_ = 0;
221  ttc_full_loop_ = tt_->add_class("Full Loop");
222  ttc_msgproc_ = tt_->add_class("Message Processing");
223  ttc_extract_lines_ = tt_->add_class("Line Extraction");
224  ttc_clustering_ = tt_->add_class("Clustering");
225 #endif
226 }
227 
228 
229 void
231 {
232  input_.reset();
233  clusters_.reset();
234  clusters_labeled_.reset();
235 
236  pcl_manager->remove_pointcloud(output_cluster_name_.c_str());
237 
238  for (size_t i = 0; i < cluster_pos_ifs_.size(); ++i) {
239  blackboard->close(cluster_pos_ifs_[i]);
240  }
241  blackboard->close(switch_if_);
242  blackboard->close(config_if_);
243 
244  finput_.reset();
245  fclusters_.reset();
246  fclusters_labeled_.reset();
247 }
248 
249 void
251 {
252  TIMETRACK_START(ttc_full_loop_);
253 
254  ++loop_count_;
255 
256  TIMETRACK_START(ttc_msgproc_);
257 
258  while (! switch_if_->msgq_empty()) {
260  switch_if_->msgq_first_safe(msg))
261  {
262  switch_if_->set_enabled(true);
263  switch_if_->write();
264  } else if (SwitchInterface::DisableSwitchMessage *msg =
265  switch_if_->msgq_first_safe(msg))
266  {
267  switch_if_->set_enabled(false);
268  switch_if_->write();
269  }
270 
271  switch_if_->msgq_pop();
272  }
273 
274  while (! config_if_->msgq_empty()) {
275  if (LaserClusterInterface::SetMaxXMessage *msg = config_if_->msgq_first_safe(msg))
276  {
277  if (msg->max_x() < 0.0) {
278  logger->log_info(name(), "Got cluster max x less than zero, setting config default %f",
279  cfg_bbox_max_x_);
280  current_max_x_ = cfg_bbox_max_x_;
281  } else {
282  current_max_x_ = msg->max_x();
283  }
284  }
285 
287  config_if_->msgq_first_safe(msg))
288  {
289  if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_DIST) {
290  cfg_selection_mode_ = SELECT_MIN_DIST;
291  } else if (msg->selection_mode() == LaserClusterInterface::SELMODE_MIN_ANGLE) {
292  cfg_selection_mode_ = SELECT_MIN_ANGLE;
293  } else {
294  logger->log_error(name(), "Unknown cluster selection mode received, ignoring");
295  }
296  config_if_->set_selection_mode(msg->selection_mode());
297  config_if_->write();
298  }
299 
300  config_if_->msgq_pop();
301  }
302 
303  if (! switch_if_->is_enabled()) {
304  //TimeWait::wait(250000);
305  for (unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
306  set_position(cluster_pos_ifs_[i], false);
307  }
308  return;
309  }
310 
311  TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
312 
313 
314  if (input_->points.size() <= 10) {
315  // this can happen if run at startup. Since tabletop threads runs continuous
316  // and not synchronized with main loop, but point cloud acquisition thread is
317  // synchronized, we might start before any data has been read
318  //logger->log_warn(name(), "Empty voxelized point cloud, omitting loop");
319  //TimeWait::wait(50000);
320  return;
321  }
322 
323  CloudPtr noline_cloud(new Cloud());
324 
325  // Erase non-finite points
326  pcl::PassThrough<PointType> passthrough;
327  if (current_max_x_ > 0.) {
328  passthrough.setFilterFieldName("x");
329  passthrough.setFilterLimits(cfg_bbox_min_x_, current_max_x_);
330  }
331  passthrough.setInputCloud(input_);
332  passthrough.filter(*noline_cloud);
333 
334  //logger->log_info(name(), "[L %u] total: %zu finite: %zu",
335  // loop_count_, input_->points.size(), noline_cloud->points.size());
336 
337  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients());
338  pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
339 
340  if (cfg_line_removal_) {
341  std::list<CloudPtr> restore_pcls;
342 
343  while (noline_cloud->points.size () > cfg_cluster_min_size_) {
344  // Segment the largest planar component from the remaining cloud
345  //logger->log_info(name(), "[L %u] %zu points left",
346  // loop_count_, noline_cloud->points.size());
347 
348  pcl::search::KdTree<PointType>::Ptr
349  search(new pcl::search::KdTree<PointType>);
350  search->setInputCloud(noline_cloud);
351  seg_.setSamplesMaxDist(cfg_segm_sample_max_dist_, search);
352  seg_.setInputCloud(noline_cloud);
353  seg_.segment(*inliers, *coeff);
354  if (inliers->indices.size () == 0) {
355  // no line found
356  break;
357  }
358 
359  // check for a minimum number of expected inliers
360  if ((double)inliers->indices.size() < cfg_segm_min_inliers_) {
361  //logger->log_warn(name(), "[L %u] no more lines (%zu inliers, required %u)",
362  // loop_count_, inliers->indices.size(), cfg_segm_min_inliers_);
363  break;
364  }
365 
366  float length = calc_line_length(noline_cloud, inliers, coeff);
367 
368  if (length < cfg_line_min_length_) {
369  // we must remove the points for now to continue filtering,
370  // but must restore them later
371  // Remove the linear inliers, extract the rest
372  CloudPtr cloud_line(new Cloud());
373  pcl::ExtractIndices<PointType> extract;
374  extract.setInputCloud(noline_cloud);
375  extract.setIndices(inliers);
376  extract.setNegative(false);
377  extract.filter(*cloud_line);
378  restore_pcls.push_back(cloud_line);
379 
380  }
381 
382  // Remove the linear inliers, extract the rest
383  CloudPtr cloud_f(new Cloud());
384  pcl::ExtractIndices<PointType> extract;
385  extract.setInputCloud(noline_cloud);
386  extract.setIndices(inliers);
387  extract.setNegative(true);
388  extract.filter(*cloud_f);
389  *noline_cloud = *cloud_f;
390  }
391 
392  for (CloudPtr cloud : restore_pcls) {
393  *noline_cloud += *cloud;
394  }
395  }
396 
397  {
398  CloudPtr tmp_cloud(new Cloud());
399  // Erase non-finite points
400  pcl::PassThrough<PointType> passthrough;
401  passthrough.setInputCloud(noline_cloud);
402  passthrough.filter(*tmp_cloud);
403 
404  if (noline_cloud->points.size() != tmp_cloud->points.size()) {
405  //logger->log_error(name(), "[L %u] new non-finite points total: %zu finite: %zu",
406  // loop_count_, noline_cloud->points.size(), tmp_cloud->points.size());
407  *noline_cloud = *tmp_cloud;
408  }
409  }
410 
411  // What remains in the cloud are now potential clusters
412 
413  TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
414 
415  clusters_->points.resize(noline_cloud->points.size());
416  clusters_->height = 1;
417  clusters_->width = noline_cloud->points.size();
418 
419  clusters_labeled_->points.resize(noline_cloud->points.size());
420  clusters_labeled_->height = 1;
421  clusters_labeled_->width = noline_cloud->points.size();
422 
423  // copy points and set to white
424  for (size_t p = 0; p < clusters_->points.size(); ++p) {
425  ColorPointType &out_point = clusters_->points[p];
426  LabelPointType &out_lab_point = clusters_labeled_->points[p];
427  PointType &in_point = noline_cloud->points[p];
428  out_point.x = out_lab_point.x = in_point.x;
429  out_point.y = out_lab_point.y = in_point.y;
430  out_point.z = out_lab_point.z = in_point.z;
431  out_point.r = out_point.g = out_point.b = 1.0;
432  out_lab_point.label = 0;
433  }
434 
435  //logger->log_info(name(), "[L %u] remaining: %zu",
436  // loop_count_, noline_cloud->points.size());
437 
438  std::vector<pcl::PointIndices> cluster_indices;
439  if (noline_cloud->points.size() > 0) {
440  // Creating the KdTree object for the search method of the extraction
441  pcl::search::KdTree<PointType>::Ptr
442  kdtree_cl(new pcl::search::KdTree<PointType>());
443  kdtree_cl->setInputCloud(noline_cloud);
444 
445  pcl::EuclideanClusterExtraction<PointType> ec;
446  ec.setClusterTolerance(cfg_cluster_tolerance_);
447  ec.setMinClusterSize(cfg_cluster_min_size_);
448  ec.setMaxClusterSize(cfg_cluster_max_size_);
449  ec.setSearchMethod(kdtree_cl);
450  ec.setInputCloud(noline_cloud);
451  ec.extract(cluster_indices);
452 
453  //logger->log_info(name(), "Found %zu clusters", cluster_indices.size());
454 
455  unsigned int i = 0;
456  for (auto cluster : cluster_indices) {
457  //Eigen::Vector4f centroid;
458  //pcl::compute3DCentroid(*noline_cloud, cluster.indices, centroid);
459 
460  //logger->log_info(name(), " Cluster %u with %zu points at (%f, %f, %f)",
461  // i, cluster.indices.size(), centroid.x(), centroid.y(), centroid.z());
462 
463  // color points of cluster
464  for (auto ci : cluster.indices) {
465  ColorPointType &out_point = clusters_->points[ci];
466  out_point.r = ignored_cluster_color[0];
467  out_point.g = ignored_cluster_color[1];;
468  out_point.b = ignored_cluster_color[2];;
469  }
470 
471  ++i;
472  }
473 
474  } else {
475  //logger->log_info(name(), "Filter left no points for clustering");
476  }
477 
478 
479  if (! cluster_indices.empty()) {
480  std::vector<ClusterInfo> cinfos;
481 
482  for (unsigned int i = 0; i < cluster_indices.size(); ++i) {
483  Eigen::Vector4f centroid;
484  pcl::compute3DCentroid(*noline_cloud, cluster_indices[i].indices, centroid);
485  if ( !cfg_use_bbox_ ||
486  ((centroid.x() >= cfg_bbox_min_x_) && (centroid.x() <= cfg_bbox_max_x_) &&
487  (centroid.y() >= cfg_bbox_min_y_) && (centroid.y() <= cfg_bbox_max_y_)))
488  {
489  ClusterInfo info;
490  info.angle = std::atan2(centroid.y(), centroid.x());
491  info.dist = centroid.norm();
492  info.index = i;
493  info.centroid = centroid;
494  cinfos.push_back(info);
495  } else {
496  /*
497  logger->log_info(name(), "[L %u] Cluster %u out of bounds (%f,%f) "
498  "not in ((%f,%f),(%f,%f))",
499  loop_count_, centroid.x(), centroid.y(),
500  cfg_bbox_min_x_, cfg_bbox_max_x_,
501  cfg_bbox_min_y_, cfg_bbox_max_y_);
502  */
503  }
504  }
505 
506  if (! cinfos.empty()) {
507  if (cfg_selection_mode_ == SELECT_MIN_ANGLE) {
508  std::sort(cinfos.begin(), cinfos.end(),
509  [](const ClusterInfo & a, const ClusterInfo & b) -> bool
510  {
511  return a.angle < b.angle;
512  });
513  } else if (cfg_selection_mode_ == SELECT_MIN_DIST) {
514  std::sort(cinfos.begin(), cinfos.end(),
515  [](const ClusterInfo & a, const ClusterInfo & b) -> bool
516  {
517  return a.dist < b.dist;
518  });
519  } else {
520  logger->log_error(name(), "Invalid selection mode, cannot select cluster");
521  }
522 
523 
524  unsigned int i;
525  for (i = 0; i < std::min(cinfos.size(), (size_t)cfg_max_num_clusters_); ++i) {
526  // color points of cluster
527  for (auto ci : cluster_indices[cinfos[i].index].indices) {
528  ColorPointType &out_point = clusters_->points[ci];
529  LabelPointType &out_lab_point = clusters_labeled_->points[ci];
530  out_point.r = cluster_colors[i][0];
531  out_point.g = cluster_colors[i][1];;
532  out_point.b = cluster_colors[i][2];;
533  out_lab_point.label = i;
534  }
535 
536  set_position(cluster_pos_ifs_[i], true, cinfos[i].centroid);
537  }
538  for (unsigned int j = i; j < cfg_max_num_clusters_; ++j) {
539  set_position(cluster_pos_ifs_[j], false);
540  }
541  } else {
542  //logger->log_warn(name(), "No acceptable cluster found, %zu clusters",
543  // cluster_indices.size());
544  for (unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
545  set_position(cluster_pos_ifs_[i], false);
546  }
547  }
548  } else {
549  //logger->log_warn(name(), "No clusters found, %zu remaining points",
550  // noline_cloud->points.size());
551  for (unsigned int i = 0; i < cfg_max_num_clusters_; ++i) {
552  set_position(cluster_pos_ifs_[i], false);
553  }
554  }
555 
556  //*clusters_ = *tmp_clusters;
557  if (finput_->header.frame_id == "") {
558  logger->log_debug(name(), "Empty frame ID");
559  }
560  fclusters_->header.frame_id = finput_->header.frame_id;
561  pcl_utils::copy_time(finput_, fclusters_);
562  fclusters_labeled_->header.frame_id = finput_->header.frame_id;
563  pcl_utils::copy_time(finput_, fclusters_labeled_);
564 
565  TIMETRACK_END(ttc_clustering_);
566  TIMETRACK_END(ttc_full_loop_);
567 
568 #ifdef USE_TIMETRACKER
569  if (++tt_loopcount_ >= 5) {
570  tt_loopcount_ = 0;
571  tt_->print_to_stdout();
572  }
573 #endif
574 }
575 
576 
577 void
578 LaserClusterThread::set_position(fawkes::Position3DInterface *iface,
579  bool is_visible, const Eigen::Vector4f &centroid,
580  const Eigen::Quaternionf &attitude)
581 {
582  tf::Stamped<tf::Pose> baserel_pose;
583 
584  if (input_->header.frame_id.empty()) {
585  is_visible = false;
586  } else {
587  try{
588  // Note that we add a correction offset to the centroid X value.
589  // This offset is determined empirically and just turns out to be helpful
590  // in certain situations.
591 
593  spose(tf::Pose(tf::Quaternion(attitude.x(), attitude.y(), attitude.z(), attitude.w()),
594  tf::Vector3(centroid[0], centroid[1], centroid[2])),
595  fawkes::Time(0, 0), input_->header.frame_id);
596  tf_listener->transform_pose(cfg_result_frame_, spose, baserel_pose);
597  iface->set_frame(cfg_result_frame_.c_str());
598  } catch (tf::TransformException &e) {
599  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
600  logger->log_warn(name(),"Transform exception:");
601  logger->log_warn(name(),e);
602  }
603  is_visible = false;
604  }
605  }
606 
607  int visibility_history = iface->visibility_history();
608  if (is_visible) {
609 
610  tf::Vector3 &origin = baserel_pose.getOrigin();
611  tf::Quaternion quat = baserel_pose.getRotation();
612 
613  Eigen::Vector4f baserel_centroid;
614  baserel_centroid[0] = origin.x();
615  baserel_centroid[1] = origin.y();
616  baserel_centroid[2] = origin.z();
617  baserel_centroid[3] = 0.;
618 
619  //we have to subtract the previously added offset to be
620  //able to compare against the current centroid
621  Eigen::Vector4f last_centroid(iface->translation(0) - cfg_offset_x_,
622  iface->translation(1) - cfg_offset_y_,
623  iface->translation(2) - cfg_offset_z_, 0.);
624  bool different_cluster =
625  fabs((last_centroid - baserel_centroid).norm()) > cfg_switch_tolerance_;
626 
627  if (! different_cluster && visibility_history >= 0) {
628  iface->set_visibility_history(visibility_history + 1);
629  } else {
630  iface->set_visibility_history(1);
631  }
632 
633  //add the offset and publish
634  double translation[3] = { origin.x() + cfg_offset_x_,
635  origin.y() + cfg_offset_y_,
636  origin.z() + cfg_offset_z_ };
637  double rotation[4] = { quat.x(), quat.y(), quat.z(), quat.w() };
638  iface->set_translation(translation);
639  iface->set_rotation(rotation);
640 
641  } else {
642  if (visibility_history <= 0) {
643  iface->set_visibility_history(visibility_history - 1);
644  } else {
645  iface->set_visibility_history(-1);
646  double translation[3] = { 0, 0, 0 };
647  double rotation[4] = { 0, 0, 0, 1 };
648  iface->set_translation(translation);
649  iface->set_rotation(rotation);
650  }
651  }
652  iface->write();
653 }
654 
655 
656 float
657 LaserClusterThread::calc_line_length(CloudPtr cloud, pcl::PointIndices::Ptr inliers,
658  pcl::ModelCoefficients::Ptr coeff)
659 {
660  if (inliers->indices.size() < 2) return 0.;
661 
662  CloudPtr cloud_line(new Cloud());
663  CloudPtr cloud_line_proj(new Cloud());
664 
665  pcl::ExtractIndices<PointType> extract;
666  extract.setInputCloud(cloud);
667  extract.setIndices(inliers);
668  extract.setNegative(false);
669  extract.filter(*cloud_line);
670 
671  // Project the model inliers
672  pcl::ProjectInliers<PointType> proj;
673  proj.setModelType(pcl::SACMODEL_LINE);
674  proj.setInputCloud(cloud_line);
675  proj.setModelCoefficients(coeff);
676  proj.filter(*cloud_line_proj);
677 
678  Eigen::Vector3f point_on_line, line_dir;
679  point_on_line[0] = cloud_line_proj->points[0].x;
680  point_on_line[1] = cloud_line_proj->points[0].y;
681  point_on_line[2] = cloud_line_proj->points[0].z;
682  line_dir[0] = coeff->values[3];
683  line_dir[1] = coeff->values[4];
684  line_dir[2] = coeff->values[5];
685 
686  ssize_t idx_1 = 0, idx_2 = 0;
687  float max_dist_1 = 0.f, max_dist_2 = 0.f;
688 
689  for (size_t i = 1; i < cloud_line_proj->points.size(); ++i) {
690  const PointType &pt = cloud_line_proj->points[i];
691  Eigen::Vector3f ptv(pt.x, pt.y, pt.z);
692  Eigen::Vector3f diff(ptv - point_on_line);
693  float dist = diff.norm();
694  float dir = line_dir.dot(diff);
695  if (dir >= 0) {
696  if (dist > max_dist_1) {
697  max_dist_1 = dist;
698  idx_1 = i;
699  }
700  }
701  if (dir <= 0) {
702  if (dist > max_dist_2) {
703  max_dist_2 = dist;
704  idx_2 = i;
705  }
706  }
707  }
708 
709  if (idx_1 >= 0 && idx_2 >= 0) {
710  const PointType &pt_1 = cloud_line_proj->points[idx_1];
711  const PointType &pt_2 = cloud_line_proj->points[idx_2];
712 
713  Eigen::Vector3f ptv_1(pt_1.x, pt_1.y, pt_1.z);
714  Eigen::Vector3f ptv_2(pt_2.x, pt_2.y, pt_2.z);
715 
716  return (ptv_1 - ptv_2).norm();
717  } else {
718  return 0.f;
719  }
720 }
void set_frame(const char *new_frame)
Set frame value.
void set_selection_mode(const SelectionMode new_selection_mode)
Set selection_mode value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
LaserClusterInterface Fawkes BlackBoard Interface.
virtual ~LaserClusterThread()
Destructor.
This class tries to translate the found plan to interpreteable data for the rest of the program...
Base class for fawkes tf exceptions.
Definition: exceptions.h:34
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
SetSelectionModeMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
STL namespace.
SetMaxXMessage Fawkes BlackBoard Interface Message.
A class for handling time.
Definition: time.h:91
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to access the transform system.
Definition: tf.h:42
void reset()
Reset pointer.
Definition: refptr.h:464
Thread aspect to use blocked timing.
float get_cache_time() const
Get cache time.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
virtual void init()
Initialize the thread.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
Position3DInterface Fawkes BlackBoard Interface.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_enabled(const bool new_enabled)
Set enabled value.
virtual Interface * open_for_writing_f(const char *interface_type, const char *identifier,...)
Open interface for writing with identifier format string.
Definition: blackboard.cpp:326
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 finalize()
Finalize the thread.
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.
double * translation() const
Get translation value.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
EnableSwitchMessage Fawkes BlackBoard Interface Message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
int32_t visibility_history() const
Get visibility_history value.
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
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
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.
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.