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