22#include "laser-lines-thread.h"
24#include "line_colors.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>
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");
274LaserLinesThread::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);
342LaserLinesThread::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();
424LaserLinesThread::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
533LaserLinesThread::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);
702LaserLinesThread::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;
virtual ~LaserLinesThread()
Destructor.
virtual void finalize()
Finalize the thread.
LaserLinesThread()
Constructor.
virtual void read_config()
Read all configuration values.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Line information container.
float length
length of the detecte line segment
pcl::PointCloud< pcl::PointXYZ >::Ptr cloud
point cloud consisting only of points account to this line
Eigen::Vector3f line_direction
line direction vector
Eigen::Vector3f end_point_1
line segment end point
EIGEN_MAKE_ALIGNED_OPERATOR_NEW float bearing
bearing to point on line
Eigen::Vector3f base_point
optimized closest point on line
Eigen::Vector3f end_point_2
line segment end point
Container for a line with tracking and smoothing info.
LineInfo raw
the latest geometry of this line, i.e. unfiltered
LineInfo smooth
moving-average geometry of this line (cf. length of history buffer)
int interface_idx
id of the interface, this line is written to, -1 when not yet assigned
int visibility_history
visibility history of this line, negative for "no sighting"
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
Interface for configuration change handling.
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.
virtual void add_change_handler(ConfigurationChangeHandler *h)
Add a configuration change handler.
Base class for exceptions in Fawkes.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
MessageType * msgq_first_safe(MessageType *&msg) noexcept
Get first message casted to the desired type without exceptions.
LaserLineInterface Fawkes BlackBoard Interface.
void set_end_point_1(unsigned int index, const float new_end_point_1)
Set end_point_1 value at given index.
void set_end_point_frame_1(const char *new_end_point_frame_1)
Set end_point_frame_1 value.
int32_t visibility_history() const
Get visibility_history value.
void set_line_direction(unsigned int index, const float new_line_direction)
Set line_direction value at given index.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
void set_point_on_line(unsigned int index, const float new_point_on_line)
Set point_on_line value at given index.
void set_length(const float new_length)
Set length value.
void set_frame_id(const char *new_frame_id)
Set frame_id value.
void set_bearing(const float new_bearing)
Set bearing value.
void set_end_point_frame_2(const char *new_end_point_frame_2)
Set end_point_frame_2 value.
void set_end_point_2(unsigned int index, const float new_end_point_2)
Set end_point_2 value at given index.
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.
Logger * logger
This is the Logger member used to access the logger.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
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.
void reset()
Reset pointer.
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.
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.