22 #include "laser-lines-thread.h" 23 #include "line_func.h" 24 #include "line_colors.h" 26 #include <pcl_utils/utils.h> 27 #include <pcl_utils/comparisons.h> 28 #include <utils/time/wait.h> 29 #include <utils/math/angle.h> 30 #ifdef USE_TIMETRACKER 31 # include <utils/time/tracker.h> 33 #include <utils/time/tracker_macros.h> 34 #include <baseapp/run.h> 36 #include <interfaces/Position3DInterface.h> 37 #include <interfaces/SwitchInterface.h> 38 #include <interfaces/LaserLineInterface.h> 40 #ifdef HAVE_VISUAL_DEBUGGING 50 #define CFG_PREFIX "/laser-lines/" 62 :
Thread(
"LaserLinesThread",
Thread::OPMODE_WAITFORWAKEUP),
78 cfg_segm_max_iterations_ =
80 cfg_segm_distance_threshold_ =
82 cfg_segm_sample_max_dist_ =
84 cfg_segm_min_inliers_ =
94 cfg_cluster_tolerance_ =
98 cfg_moving_avg_enabled_ =
100 cfg_moving_avg_window_size_ =
103 cfg_switch_tolerance_ =
113 input_ = pcl_utils::cloudptr_from_refptr(finput_);
117 line_ifs_.resize(cfg_max_num_lines_, NULL);
118 if(cfg_moving_avg_enabled_)
120 line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
122 for (
unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
124 if (asprintf(&tmp,
"/laser-lines/%u", i + 1) != -1) {
126 std::string
id = tmp;
131 if(cfg_moving_avg_enabled_)
146 bool autostart =
true;
150 switch_if_->set_enabled(autostart);
153 for (
size_t i = 0; i < line_ifs_.size(); ++i) {
155 if(cfg_moving_avg_enabled_)
165 flines_->header.frame_id = finput_->header.frame_id;
166 flines_->is_dense =
false;
168 lines_ = pcl_utils::cloudptr_from_refptr(flines_);
172 #ifdef USE_TIMETRACKER 175 ttc_full_loop_ = tt_->add_class(
"Full Loop");
176 ttc_msgproc_ = tt_->add_class(
"Message Processing");
177 ttc_extract_lines_ = tt_->add_class(
"Line Extraction");
178 ttc_clustering_ = tt_->add_class(
"Clustering");
180 #ifdef HAVE_VISUAL_DEBUGGING 181 vispub_ =
new ros::Publisher();
182 *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100);
191 #ifdef HAVE_VISUAL_DEBUGGING 201 for (
size_t i = 0; i < line_ifs_.size(); ++i) {
203 if(cfg_moving_avg_enabled_)
217 TIMETRACK_START(ttc_full_loop_);
221 TIMETRACK_START(ttc_msgproc_);
233 for (
unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
234 line_ifs_[i]->set_visibility_history(0);
235 line_ifs_[i]->write();
250 TIMETRACK_INTER(ttc_msgproc_, ttc_extract_lines_);
252 if (input_->points.size() <= 10) {
259 for (
unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
260 set_line(i, line_ifs_[i],
false);
261 if(cfg_moving_avg_enabled_)
263 set_line(i, line_avg_ifs_[i],
false);
274 std::vector<LineInfo> linfos =
275 calc_lines<PointType>(input_,
276 cfg_segm_min_inliers_, cfg_segm_max_iterations_,
277 cfg_segm_distance_threshold_, cfg_segm_sample_max_dist_,
278 cfg_cluster_tolerance_, cfg_cluster_quota_,
279 cfg_min_length_, cfg_max_length_, cfg_min_dist_, cfg_max_dist_);
282 TIMETRACK_INTER(ttc_extract_lines_, ttc_clustering_);
284 size_t num_points = 0;
285 for (
size_t i = 0; i < linfos.size(); ++i) {
286 num_points += linfos[i].cloud->points.size();
289 lines_->points.resize(num_points);
291 lines_->width = num_points;
293 vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
294 while (known_it != known_lines_.end()) {
295 btScalar min_dist = numeric_limits<btScalar>::max();
296 auto best_match = linfos.end();
297 for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != linfos.end(); ++it_new) {
298 btScalar d = known_it->distance(*it_new);
304 if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
305 known_it->update(*best_match);
308 linfos.erase(best_match);
312 known_it = known_lines_.erase(known_it);
319 finput_->header.frame_id,
320 cfg_tracking_frame_id_,
321 cfg_switch_tolerance_,
322 cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1,
325 known_lines_.push_back(tl);
331 std::sort(known_lines_.begin(), known_lines_.end(),
337 while (known_lines_.size() > cfg_max_num_lines_)
338 known_lines_.erase(known_lines_.end() - 1);
341 std::sort(known_lines_.begin(), known_lines_.end(),
350 unsigned int line_if_idx = 0;
351 for (
size_t i = 0; i < known_lines_.size(); ++i) {
354 if (line_if_idx < cfg_max_num_lines_) {
355 set_line(line_if_idx, line_ifs_[line_if_idx],
true, finput_->header.frame_id, info.
raw);
356 if(cfg_moving_avg_enabled_)
358 set_line(line_if_idx, line_avg_ifs_[line_if_idx],
true, finput_->header.frame_id, info.
smooth);
363 for (
size_t p = 0; p < info.
raw.
cloud->points.size(); ++p) {
364 ColorPointType &out_point = lines_->points[oi++];
365 PointType &in_point = info.
raw.
cloud->points[p];
366 out_point.x = in_point.x;
367 out_point.y = in_point.y;
368 out_point.z = in_point.z;
371 out_point.r = line_colors[i][0];
372 out_point.g = line_colors[i][1];
373 out_point.b = line_colors[i][2];
375 out_point.r = out_point.g = out_point.b = 1.0;
380 for (
unsigned int i = line_if_idx; i < cfg_max_num_lines_; ++i) {
381 set_line(i, line_ifs_[i],
false);
382 if(cfg_moving_avg_enabled_)
384 set_line(i, line_avg_ifs_[i],
false);
388 #ifdef HAVE_VISUAL_DEBUGGING 389 publish_visualization(known_lines_,
"laser_lines",
"laser_lines_moving_average");
393 if (finput_->header.frame_id ==
"" &&
398 flines_->header.frame_id = finput_->header.frame_id;
399 pcl_utils::copy_time(finput_, flines_);
401 TIMETRACK_END(ttc_clustering_);
402 TIMETRACK_END(ttc_full_loop_);
404 #ifdef USE_TIMETRACKER 405 if (++tt_loopcount_ >= 5) {
407 tt_->print_to_stdout();
415 LaserLinesThread::set_line(
unsigned int idx,
418 const std::string &frame_id,
426 float diff = (old_point_on_line - info.
base_point).norm();
428 if (visibility_history >= 0 && (diff <= cfg_switch_tolerance_)) {
435 float if_point_on_line[3] =
437 float if_line_direction[3] =
439 float if_end_point_1[3] =
441 float if_end_point_2[3] =
454 std::string frame_name_1, frame_name_2;
456 if (asprintf(&tmp,
"laser_line_%u_e1", idx+1) != -1) {
460 if (asprintf(&tmp,
"laser_line_%u_e2", idx+1) != -1) {
464 if (frame_name_1 !=
"" && frame_name_2 !=
"") {
466 double dotprod = Eigen::Vector3f::UnitX().dot(bp_unit);
467 double angle = acos(dotprod) + M_PI;
469 if (info.
base_point[1] < 0.) angle = fabs(angle) * -1.;
471 tf::Transform t1(tf::Quaternion(tf::Vector3(0,0,1), angle),
473 tf::Transform t2(tf::Quaternion(tf::Vector3(0,0,1), angle),
480 logger->
log_warn(
name(),
"Failed to publish laser_line_%u_* transforms, exception follows", idx+1);
488 if (visibility_history <= 0) {
492 float zero_vector[3] = { 0, 0, 0 };
506 #ifdef HAVE_VISUAL_DEBUGGING 508 LaserLinesThread::publish_visualization_add_line(visualization_msgs::MarkerArray &m,
512 const std::string &marker_namespace,
513 const std::string &name_suffix)
537 visualization_msgs::Marker dirvec;
538 dirvec.header.frame_id = finput_->header.frame_id;
539 dirvec.header.stamp = ros::Time::now();
540 dirvec.ns = marker_namespace;
542 dirvec.type = visualization_msgs::Marker::ARROW;
543 dirvec.action = visualization_msgs::Marker::ADD;
544 dirvec.points.resize(2);
551 dirvec.scale.x = 0.02;
552 dirvec.scale.y = 0.04;
553 dirvec.color.r = 0.0;
554 dirvec.color.g = 1.0;
555 dirvec.color.b = 0.f;
556 dirvec.color.a = 1.0;
557 dirvec.lifetime = ros::Duration(2, 0);
558 m.markers.push_back(dirvec);
560 visualization_msgs::Marker testvec;
561 testvec.header.frame_id = finput_->header.frame_id;
562 testvec.header.stamp = ros::Time::now();
563 testvec.ns = marker_namespace;
564 testvec.id = idnum++;
565 testvec.type = visualization_msgs::Marker::ARROW;
566 testvec.action = visualization_msgs::Marker::ADD;
567 testvec.points.resize(2);
568 testvec.points[0].x = 0;
569 testvec.points[0].y = 0;
570 testvec.points[0].z = 0;
574 testvec.scale.x = 0.02;
575 testvec.scale.y = 0.04;
576 testvec.color.r = line_colors[i][0] / 255.;
577 testvec.color.g = line_colors[i][1] / 255.;
578 testvec.color.b = line_colors[i][2] / 255.;
579 testvec.color.a = 1.0;
580 testvec.lifetime = ros::Duration(2, 0);
581 m.markers.push_back(testvec);
584 if (asprintf(&tmp,
"L_%zu%s", i+1, name_suffix.c_str()) != -1) {
586 std::string
id = tmp;
589 visualization_msgs::Marker text;
590 text.header.frame_id = finput_->header.frame_id;
591 text.header.stamp = ros::Time::now();
592 text.ns = marker_namespace;
594 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
595 text.action = visualization_msgs::Marker::ADD;
598 text.pose.position.z = info.
base_point[2] + .15;
599 text.pose.orientation.w = 1.;
601 text.color.r = text.color.g = text.color.b = 1.0f;
603 text.lifetime = ros::Duration(2, 0);
605 m.markers.push_back(text);
608 if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
609 visualization_msgs::Marker sphere_ep_1;
610 sphere_ep_1.header.frame_id = finput_->header.frame_id;
611 sphere_ep_1.header.stamp = ros::Time::now();
612 sphere_ep_1.ns = marker_namespace;
613 sphere_ep_1.id = idnum++;
614 sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
615 sphere_ep_1.action = visualization_msgs::Marker::ADD;
619 sphere_ep_1.pose.orientation.w = 1.;
620 sphere_ep_1.scale.x = 0.05;
621 sphere_ep_1.scale.y = 0.05;
622 sphere_ep_1.scale.z = 0.05;
623 sphere_ep_1.color.r = line_colors[i][0] / 255.;
624 sphere_ep_1.color.g = line_colors[i][1] / 255.;
625 sphere_ep_1.color.b = line_colors[i][2] / 255.;
626 sphere_ep_1.color.a = 1.0;
627 sphere_ep_1.lifetime = ros::Duration(2, 0);
628 m.markers.push_back(sphere_ep_1);
630 visualization_msgs::Marker sphere_ep_2;
631 sphere_ep_2.header.frame_id = finput_->header.frame_id;
632 sphere_ep_2.header.stamp = ros::Time::now();
633 sphere_ep_2.ns = marker_namespace;
634 sphere_ep_2.id = idnum++;
635 sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
636 sphere_ep_2.action = visualization_msgs::Marker::ADD;
640 sphere_ep_2.pose.orientation.w = 1.;
641 sphere_ep_2.scale.x = 0.05;
642 sphere_ep_2.scale.y = 0.05;
643 sphere_ep_2.scale.z = 0.05;
644 sphere_ep_2.color.r = line_colors[i][0] / 255.;
645 sphere_ep_2.color.g = line_colors[i][1] / 255.;
646 sphere_ep_2.color.b = line_colors[i][2] / 255.;
647 sphere_ep_2.color.a = 1.0;
648 sphere_ep_2.lifetime = ros::Duration(2, 0);
649 m.markers.push_back(sphere_ep_2);
651 visualization_msgs::Marker lineseg;
652 lineseg.header.frame_id = finput_->header.frame_id;
653 lineseg.header.stamp = ros::Time::now();
654 lineseg.ns = marker_namespace;
655 lineseg.id = idnum++;
656 lineseg.type = visualization_msgs::Marker::LINE_LIST;
657 lineseg.action = visualization_msgs::Marker::ADD;
658 lineseg.points.resize(2);
665 lineseg.scale.x = 0.02;
666 lineseg.scale.y = 0.04;
667 lineseg.color.r = line_colors[i][0] / 255.;
668 lineseg.color.g = line_colors[i][1] / 255.;
669 lineseg.color.b = line_colors[i][2] / 255.;
670 lineseg.color.a = 1.0;
671 lineseg.lifetime = ros::Duration(2, 0);
672 m.markers.push_back(lineseg);
677 LaserLinesThread::publish_visualization(
const std::vector<TrackedLineInfo> &linfos,
678 const std::string &marker_namespace,
679 const std::string &avg_marker_namespace)
681 visualization_msgs::MarkerArray m;
683 unsigned int idnum = 0;
685 for (
size_t i = 0; i < linfos.size(); ++i) {
687 publish_visualization_add_line(m, idnum, info.
raw, i, marker_namespace);
688 publish_visualization_add_line(m, idnum, info.
smooth, i, avg_marker_namespace,
"_avg");
691 for (
size_t i = idnum; i < last_id_num_; ++i) {
692 visualization_msgs::Marker delop;
693 delop.header.frame_id = finput_->header.frame_id;
694 delop.header.stamp = ros::Time::now();
695 delop.ns = marker_namespace;
697 delop.action = visualization_msgs::Marker::DELETE;
698 m.markers.push_back(delop);
700 last_id_num_ = idnum;
Line information container.
virtual void init()
Initialize the thread.
void set_bearing(const float new_bearing)
Set bearing value.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
virtual void finalize()
Finalize the thread.
bool msgq_empty()
Check if queue is empty.
void set_end_point_2(unsigned int index, const float new_end_point_2)
Set end_point_2 value at given index.
void remove_pointcloud(const char *id)
Remove the point cloud.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Eigen::Vector3f end_point_1
line segment end point
LaserLinesThread()
Constructor.
void set_end_point_1(unsigned int index, const float new_end_point_1)
Set end_point_1 value at given index.
Eigen::Vector3f end_point_2
line segment end point
A class for handling time.
void set_length(const float new_length)
Set length value.
void set_line_direction(unsigned int index, const float new_line_direction)
Set line_direction value at given index.
float * point_on_line() const
Get point_on_line value.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
LaserLineInterface Fawkes BlackBoard Interface.
void write()
Write from local copy into BlackBoard memory.
virtual void loop()
Code to execute in the thread.
Logger * logger
This is the Logger member used to access the logger.
Eigen::Vector3f base_point
optimized closest point on line
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
Clock * clock
By means of this member access to the clock is given.
void reset()
Reset pointer.
Thread aspect to use blocked timing.
void set_point_on_line(unsigned int index, const float new_point_on_line)
Set point_on_line value at given index.
void msgq_pop()
Erase first message from queue.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Base class for exceptions in Fawkes.
void set_enabled(const bool new_enabled)
Set enabled value.
virtual ~LaserLinesThread()
Destructor.
int32_t visibility_history() const
Get visibility_history value.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of 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.
Eigen::Vector3f line_direction
line direction vector
float length
length of the detecte line segment
Eigen::Vector3f point_on_line
point on line vector
EnableSwitchMessage Fawkes BlackBoard Interface Message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Container for a line with tracking and smoothing info.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
LineInfo raw
the latest geometry of this line, i.e. unfiltered
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void set_frame_id(const char *new_frame_id)
Set frame_id value.
void update(LineInfo &new_linfo)
Update this line.
float bearing_center
Bearing towards line center, used to select lines "in front of us" when there.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.