22 #include "laser-lines-thread.h"
24 #include "line_colors.h"
25 #include "line_func.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>
34 #include <baseapp/run.h>
35 #include <interfaces/LaserLineInterface.h>
36 #include <interfaces/Position3DInterface.h>
37 #include <interfaces/SwitchInterface.h>
38 #include <utils/time/tracker_macros.h>
40 #ifdef HAVE_VISUAL_DEBUGGING
50 #define CFG_PREFIX "/laser-lines/"
61 :
Thread(
"LaserLinesThread",
Thread::OPMODE_WAITFORWAKEUP),
80 input_ = pcl_utils::cloudptr_from_refptr(finput_);
85 line_ifs_.resize(cfg_max_num_lines_, NULL);
86 if (cfg_moving_avg_enabled_) {
87 line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
90 for (
unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
93 if (asprintf(&tmp,
"/laser-lines/%u", i + 1) != -1) {
100 if (cfg_moving_avg_enabled_) {
111 bool autostart =
true;
120 for (
size_t i = 0; i < line_ifs_.size(); ++i) {
122 if (cfg_moving_avg_enabled_) {
131 flines_->header.frame_id = finput_->header.frame_id;
132 flines_->is_dense =
false;
134 lines_ = pcl_utils::cloudptr_from_refptr(flines_);
138 #ifdef USE_TIMETRACKER
141 ttc_full_loop_ = tt_->add_class(
"Full Loop");
142 ttc_msgproc_ = tt_->add_class(
"Message Processing");
143 ttc_extract_lines_ = tt_->add_class(
"Line Extraction");
144 ttc_clustering_ = tt_->add_class(
"Clustering");
146 #ifdef HAVE_VISUAL_DEBUGGING
147 vispub_ =
new ros::Publisher();
148 *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100);
156 #ifdef HAVE_VISUAL_DEBUGGING
166 for (
size_t i = 0; i < line_ifs_.size(); ++i) {
168 if (cfg_moving_avg_enabled_) {
181 TIMETRACK_START(ttc_full_loop_);
185 TIMETRACK_START(ttc_msgproc_);
194 for (
unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
195 line_ifs_[i]->set_visibility_history(0);
196 line_ifs_[i]->write();
212 TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
214 if (input_->points.size() <= 10) {
221 for (
unsigned int i = 0; i < this->known_lines_.size(); ++i) {
222 known_lines_[i].not_visible_update();
227 std::vector<LineInfo> linfos = calc_lines<PointType>(input_,
228 cfg_segm_min_inliers_,
229 cfg_segm_max_iterations_,
230 cfg_segm_distance_threshold_,
231 cfg_segm_sample_max_dist_,
232 cfg_cluster_tolerance_,
239 TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
240 update_lines(linfos);
243 publish_known_lines();
250 cfg_segm_max_iterations_ =
config->
get_uint(CFG_PREFIX
"line_segmentation_max_iterations");
251 cfg_segm_distance_threshold_ =
253 cfg_segm_sample_max_dist_ =
config->
get_float(CFG_PREFIX
"line_segmentation_sample_max_dist");
254 cfg_segm_min_inliers_ =
config->
get_uint(CFG_PREFIX
"line_segmentation_min_inliers");
259 cfg_cluster_tolerance_ =
config->
get_float(CFG_PREFIX
"line_cluster_tolerance");
260 cfg_cluster_quota_ =
config->
get_float(CFG_PREFIX
"line_cluster_quota");
261 cfg_moving_avg_enabled_ =
config->
get_bool(CFG_PREFIX
"moving_avg_enabled");
262 cfg_moving_avg_window_size_ =
config->
get_uint(CFG_PREFIX
"moving_avg_window_size");
264 cfg_switch_tolerance_ =
config->
get_float(CFG_PREFIX
"switch_tolerance");
268 cfg_max_num_lines_ =
config->
get_uint(CFG_PREFIX
"max_num_lines");
274 LaserLinesThread::update_lines(std::vector<LineInfo> &linfos)
276 size_t num_points = 0;
277 for (
size_t i = 0; i < linfos.size(); ++i) {
278 num_points += linfos[i].cloud->points.size();
281 lines_->points.resize(num_points);
283 lines_->width = num_points;
285 vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
286 while (known_it != known_lines_.end()) {
287 btScalar min_dist = numeric_limits<btScalar>::max();
288 auto best_match = linfos.end();
289 for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != linfos.end(); ++it_new) {
290 btScalar d = known_it->distance(*it_new);
296 if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
297 known_it->update(*best_match);
300 linfos.erase(best_match);
303 known_it->not_visible_update();
311 finput_->header.frame_id,
312 cfg_tracking_frame_id_,
313 cfg_switch_tolerance_,
314 cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 0,
318 known_lines_.push_back(tl);
323 std::sort(known_lines_.begin(),
326 return l1.visibility_history < l2.visibility_history;
328 while (known_lines_.size() > cfg_max_num_lines_ && known_lines_[0].visibility_history < 0)
329 known_lines_.erase(known_lines_.begin());
332 std::sort(known_lines_.begin(),
335 return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
337 while (known_lines_.size() > cfg_max_num_lines_)
338 known_lines_.erase(known_lines_.end() - 1);
342 LaserLinesThread::publish_known_lines()
346 for (
size_t i = 0; i < known_lines_.size(); ++i) {
350 for (
size_t p = 0; p < info.
raw.
cloud->points.size(); ++p) {
351 ColorPointType &out_point = lines_->points[oi++];
352 PointType & in_point = info.
raw.
cloud->points[p];
353 out_point.x = in_point.x;
354 out_point.y = in_point.y;
355 out_point.z = in_point.z;
358 out_point.r = line_colors[i][0];
359 out_point.g = line_colors[i][1];
360 out_point.b = line_colors[i][2];
362 out_point.r = out_point.g = out_point.b = 1.0;
369 for (
unsigned int line_if_idx = 0; line_if_idx < cfg_max_num_lines_; ++line_if_idx) {
370 int known_line_idx = -1;
374 for (
unsigned int test_idx = 0; test_idx < known_lines_.size(); ++test_idx) {
377 known_line_idx = test_idx;
381 known_line_idx = test_idx;
385 if (known_line_idx == -1) {
386 set_empty_interface(line_ifs_[line_if_idx]);
387 if (cfg_moving_avg_enabled_) {
388 set_empty_interface(line_avg_ifs_[line_if_idx]);
391 known_lines_[known_line_idx].interface_idx = line_if_idx;
393 set_interface(line_if_idx, line_ifs_[line_if_idx],
false, info, finput_->header.frame_id);
394 if (cfg_moving_avg_enabled_) {
396 line_if_idx, line_avg_ifs_[line_if_idx],
true, info, finput_->header.frame_id);
401 #ifdef HAVE_VISUAL_DEBUGGING
402 publish_visualization(known_lines_,
"laser_lines",
"laser_lines_moving_average");
405 if (finput_->header.frame_id ==
""
409 flines_->header.frame_id = finput_->header.frame_id;
410 pcl_utils::copy_time(finput_, flines_);
412 TIMETRACK_END(ttc_clustering_);
413 TIMETRACK_END(ttc_full_loop_);
415 #ifdef USE_TIMETRACKER
416 if (++tt_loopcount_ >= 5) {
418 tt_->print_to_stdout();
424 LaserLinesThread::set_interface(
unsigned int idx,
428 const std::string & frame_id)
452 std::string frame_name_1, frame_name_2;
454 std::string avg = moving_average ?
"avg_" :
"";
455 if (asprintf(&tmp,
"laser_line_%s%u_e1", avg.c_str(), idx + 1) != -1) {
459 if (asprintf(&tmp,
"laser_line_%s%u_e2", avg.c_str(), idx + 1) != -1) {
472 if (frame_name_1 !=
"" && frame_name_2 !=
"") {
474 double dotprod = Eigen::Vector3f::UnitX().dot(bp_unit);
475 double angle = acos(dotprod) + M_PI;
478 angle = fabs(angle) * -1.;
480 tf::Transform t1(tf::Quaternion(tf::Vector3(0, 0, 1), angle),
482 tf::Transform t2(tf::Quaternion(tf::Vector3(0, 0, 1), angle),
496 tf_it_1->second->send_transform(t1, now, frame_id, frame_name_1);
497 tf_it_2->second->send_transform(t2, now, frame_id, frame_name_2);
500 "Failed to publish laser_line_%u_* transforms, exception follows",
515 if (visibility_history <= 0) {
519 float zero_vector[3] = {0, 0, 0};
531 #ifdef HAVE_VISUAL_DEBUGGING
533 LaserLinesThread::publish_visualization_add_line(visualization_msgs::MarkerArray &m,
534 unsigned int & idnum,
537 const std::string & marker_namespace,
538 const std::string & name_suffix)
562 visualization_msgs::Marker dirvec;
563 dirvec.header.frame_id = finput_->header.frame_id;
564 dirvec.header.stamp = ros::Time::now();
565 dirvec.ns = marker_namespace;
567 dirvec.type = visualization_msgs::Marker::ARROW;
568 dirvec.action = visualization_msgs::Marker::ADD;
569 dirvec.points.resize(2);
576 dirvec.scale.x = 0.02;
577 dirvec.scale.y = 0.04;
578 dirvec.color.r = 0.0;
579 dirvec.color.g = 1.0;
580 dirvec.color.b = 0.f;
581 dirvec.color.a = 1.0;
582 dirvec.lifetime = ros::Duration(2, 0);
583 m.markers.push_back(dirvec);
585 visualization_msgs::Marker testvec;
586 testvec.header.frame_id = finput_->header.frame_id;
587 testvec.header.stamp = ros::Time::now();
588 testvec.ns = marker_namespace;
589 testvec.id = idnum++;
590 testvec.type = visualization_msgs::Marker::ARROW;
591 testvec.action = visualization_msgs::Marker::ADD;
592 testvec.points.resize(2);
593 testvec.points[0].x = 0;
594 testvec.points[0].y = 0;
595 testvec.points[0].z = 0;
599 testvec.scale.x = 0.02;
600 testvec.scale.y = 0.04;
601 testvec.color.r = line_colors[i][0] / 255.;
602 testvec.color.g = line_colors[i][1] / 255.;
603 testvec.color.b = line_colors[i][2] / 255.;
604 testvec.color.a = 1.0;
605 testvec.lifetime = ros::Duration(2, 0);
606 m.markers.push_back(testvec);
609 if (asprintf(&tmp,
"L_%zu%s", i + 1, name_suffix.c_str()) != -1) {
611 std::string
id = tmp;
614 visualization_msgs::Marker text;
615 text.header.frame_id = finput_->header.frame_id;
616 text.header.stamp = ros::Time::now();
617 text.ns = marker_namespace;
619 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
620 text.action = visualization_msgs::Marker::ADD;
623 text.pose.position.z = info.
base_point[2] + .15;
624 text.pose.orientation.w = 1.;
626 text.color.r = text.color.g = text.color.b = 1.0f;
628 text.lifetime = ros::Duration(2, 0);
630 m.markers.push_back(text);
633 if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
634 visualization_msgs::Marker sphere_ep_1;
635 sphere_ep_1.header.frame_id = finput_->header.frame_id;
636 sphere_ep_1.header.stamp = ros::Time::now();
637 sphere_ep_1.ns = marker_namespace;
638 sphere_ep_1.id = idnum++;
639 sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
640 sphere_ep_1.action = visualization_msgs::Marker::ADD;
644 sphere_ep_1.pose.orientation.w = 1.;
645 sphere_ep_1.scale.x = 0.05;
646 sphere_ep_1.scale.y = 0.05;
647 sphere_ep_1.scale.z = 0.05;
648 sphere_ep_1.color.r = line_colors[i][0] / 255.;
649 sphere_ep_1.color.g = line_colors[i][1] / 255.;
650 sphere_ep_1.color.b = line_colors[i][2] / 255.;
651 sphere_ep_1.color.a = 1.0;
652 sphere_ep_1.lifetime = ros::Duration(2, 0);
653 m.markers.push_back(sphere_ep_1);
655 visualization_msgs::Marker sphere_ep_2;
656 sphere_ep_2.header.frame_id = finput_->header.frame_id;
657 sphere_ep_2.header.stamp = ros::Time::now();
658 sphere_ep_2.ns = marker_namespace;
659 sphere_ep_2.id = idnum++;
660 sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
661 sphere_ep_2.action = visualization_msgs::Marker::ADD;
665 sphere_ep_2.pose.orientation.w = 1.;
666 sphere_ep_2.scale.x = 0.05;
667 sphere_ep_2.scale.y = 0.05;
668 sphere_ep_2.scale.z = 0.05;
669 sphere_ep_2.color.r = line_colors[i][0] / 255.;
670 sphere_ep_2.color.g = line_colors[i][1] / 255.;
671 sphere_ep_2.color.b = line_colors[i][2] / 255.;
672 sphere_ep_2.color.a = 1.0;
673 sphere_ep_2.lifetime = ros::Duration(2, 0);
674 m.markers.push_back(sphere_ep_2);
676 visualization_msgs::Marker lineseg;
677 lineseg.header.frame_id = finput_->header.frame_id;
678 lineseg.header.stamp = ros::Time::now();
679 lineseg.ns = marker_namespace;
680 lineseg.id = idnum++;
681 lineseg.type = visualization_msgs::Marker::LINE_LIST;
682 lineseg.action = visualization_msgs::Marker::ADD;
683 lineseg.points.resize(2);
690 lineseg.scale.x = 0.02;
691 lineseg.scale.y = 0.04;
692 lineseg.color.r = line_colors[i][0] / 255.;
693 lineseg.color.g = line_colors[i][1] / 255.;
694 lineseg.color.b = line_colors[i][2] / 255.;
695 lineseg.color.a = 1.0;
696 lineseg.lifetime = ros::Duration(2, 0);
697 m.markers.push_back(lineseg);
702 LaserLinesThread::publish_visualization(
const std::vector<TrackedLineInfo> &linfos,
703 const std::string & marker_namespace,
704 const std::string & avg_marker_namespace)
706 visualization_msgs::MarkerArray m;
708 unsigned int idnum = 0;
710 for (
size_t i = 0; i < linfos.size(); ++i) {
713 publish_visualization_add_line(m, idnum, info.
raw, info.
interface_idx, marker_namespace);
714 publish_visualization_add_line(
719 for (
size_t i = idnum; i < last_id_num_; ++i) {
720 visualization_msgs::Marker delop;
721 delop.header.frame_id = finput_->header.frame_id;
722 delop.header.stamp = ros::Time::now();
723 delop.ns = marker_namespace;
725 delop.action = visualization_msgs::Marker::DELETE;
726 m.markers.push_back(delop);
728 last_id_num_ = idnum;