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
57using namespace std;
58
59/** @class LaserClusterThread "laser-cluster-thread.h"
60 * Main thread of laser-cluster plugin.
61 * @author Tim Niemueller
62 */
63
64using namespace fawkes;
65
66/** Constructor.
67 * @param cfg_name configuration name
68 * @param cfg_prefix configuration path prefix
69 */
70LaserClusterThread::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
85void
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
226void
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
246void
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) {
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
564void
565LaserClusterThread::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
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
644float
645LaserClusterThread::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}
virtual ~LaserClusterThread()
Destructor.
LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
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
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
Definition: interface.h:340
SetMaxXMessage Fawkes BlackBoard Interface Message.
SetSelectionModeMessage Fawkes BlackBoard Interface Message.
LaserClusterInterface Fawkes BlackBoard Interface.
void set_selection_mode(const SelectionMode new_selection_mode)
Set selection_mode value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void remove_pointcloud(const char *id)
Remove the point cloud.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
double * translation() const
Get translation value.
void set_frame(const char *new_frame)
Set frame value.
void reset()
Reset pointer.
Definition: refptr.h:455
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
Base class for fawkes tf exceptions.
Definition: exceptions.h:31
float get_cache_time() const
Get cache time.
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.
This class tries to translate the found plan to interpreteable data for the rest of the program.
Fawkes library namespace.