[Fawkes Git] branch/vmatare/laser-lines-matching: created (0.5.0-3218-g6a2f4ec)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Mon Sep 5 11:45:29 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, vmatare/laser-lines-matching has been created
        at  6a2f4ec95c06887ff7637e9e44e534b119ee3e34 (commit)

http://git.fawkesrobotics.org/fawkes.git/vmatare/laser-lines-matching

- *Log* ---------------------------------------------------------------
commit 22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Sat Jun 25 19:59:27 2016 +0200
Commit:     Victor Mataré <matare at lih.rwth-aachen.de>
CommitDate: Wed Aug 31 05:19:31 2016 +0200

    laser-lines: match lines combinatorially by distance

http://git.fawkesrobotics.org/fawkes.git/commit/22c0ea8
http://trac.fawkesrobotics.org/changeset/22c0ea8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 702e914cdd1ae1ea878ce7a46e21753b7b859808
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Thu Sep 1 11:05:09 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Thu Sep 1 11:05:09 2016 +0200

    laser-lines: fix line deletion in visualization
    
    The refactoring of the visualization code (moving the loop out of the
    function) caused double creation of deletion messages. Bring back the
    old style and make the visualization function aware of raw and smoothed
    lines.

http://git.fawkesrobotics.org/fawkes.git/commit/702e914
http://trac.fawkesrobotics.org/changeset/702e914

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d4a5c134dbf5991c976f4011eb7794b72c67099d
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Fri Sep 2 15:03:28 2016 +0200
Commit:     Victor Mataré <matare at lih.rwth-aachen.de>
CommitDate: Fri Sep 2 15:04:48 2016 +0200

    laser-lines: catch TransformException
    
    Try to track in input frame as long as transform is unavailable.

http://git.fawkesrobotics.org/fawkes.git/commit/d4a5c13
http://trac.fawkesrobotics.org/changeset/d4a5c13

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 6a2f4ec95c06887ff7637e9e44e534b119ee3e34
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Fri Sep 2 15:18:10 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Mon Sep 5 11:44:42 2016 +0200

    laser-lines: use configurable odom frame for tracking

http://git.fawkesrobotics.org/fawkes.git/commit/6a2f4ec
http://trac.fawkesrobotics.org/changeset/6a2f4ec

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 src/plugins/laser-lines/laser-lines-thread.cpp |  324 ++++++++++++------------
 src/plugins/laser-lines/laser-lines-thread.h   |   16 +-
 src/plugins/laser-lines/line_info.cpp          |   25 ++-
 src/plugins/laser-lines/line_info.h            |    9 +-
 4 files changed, 208 insertions(+), 166 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* 22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e - - - - - - - - - -
Author:  Victor Mataré <matare at lih.rwth-aachen.de>
Date:    Sat Jun 25 19:59:27 2016 +0200
Subject: laser-lines: match lines combinatorially by distance

 src/plugins/laser-lines/Makefile               |    2 +-
 src/plugins/laser-lines/laser-lines-thread.cpp |  153 +++++++++++-------------
 src/plugins/laser-lines/laser-lines-thread.h   |    4 +-
 src/plugins/laser-lines/line_info.cpp          |  111 +++++++++++++++++
 src/plugins/laser-lines/line_info.h            |   26 ++++
 5 files changed, 209 insertions(+), 87 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/Makefile b/src/plugins/laser-lines/Makefile
index 89e85ac..253a730 100644
--- a/src/plugins/laser-lines/Makefile
+++ b/src/plugins/laser-lines/Makefile
@@ -39,7 +39,7 @@ CFLAGS += $(CFLAGS_CPP11) -Wno-unknown-pragmas
 LIBS_laser_lines = fawkescore fawkesutils fawkesaspects fvutils fawkesbaseapp \
 			fawkestf fawkesinterface fawkesblackboard fawkespcl_utils \
 			Position3DInterface SwitchInterface LaserLineInterface
-OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o
+OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o  line_info.o
 
 OBJS_all    = $(OBJS_laser_lines)
 
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index 787b75a..b9faf65 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -117,7 +117,6 @@ LaserLinesThread::init()
     if(cfg_moving_avg_enabled_)
     {
       line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
-      moving_average_windows_.resize(cfg_max_num_lines_);
     }
     for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
       char *tmp;
@@ -270,6 +269,7 @@ LaserLinesThread::loop()
   //logger->log_info(name(), "[L %u] total: %zu   finite: %zu",
   //		     loop_count_, input_->points.size(), in_cloud->points.size());
 
+  {
   std::vector<LineInfo> linfos =
     calc_lines<PointType>(input_,
 			  cfg_segm_min_inliers_, cfg_segm_max_iterations_,
@@ -289,96 +289,77 @@ LaserLinesThread::loop()
   lines_->height = 1;
   lines_->width  = num_points;
 
-  // sort lines by bearing to stabilize IDs
-  std::sort(linfos.begin(), linfos.end(),
-	    [](const LineInfo &l1, const LineInfo &l2) -> bool
-	    {
-	      return l1.bearing < l2.bearing;
-	    });
-
-  if (linfos.size() > cfg_max_num_lines_){
-    // Ignore lines if more than cfg_max_num_lines_ found.
-    // Since the lines are orderd by bearing, this will drop
-    // the line with the highest bearing.
-    linfos.resize(cfg_max_num_lines_);
+  vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
+  while (known_it != known_lines_.end()) {
+    btScalar min_dist = numeric_limits<btScalar>::max();
+    auto best_match = linfos.end();
+    for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != linfos.end(); ++it_new) {
+      btScalar d = known_it->distance(*it_new);
+      if (d < min_dist) {
+	min_dist = d;
+	best_match = it_new;
+      }
+    }
+    if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
+      known_it->update(*best_match);
+
+      // Important: erase line because all lines remaining after this are considered "new" (see below)
+      linfos.erase(best_match);
+      ++known_it;
+    }
+    else // No match for this line, so kill it
+      known_it = known_lines_.erase(known_it);
   }
 
+  for (LineInfo &l : linfos) {
+    // Only unmatched lines remaining, so these are the "new" lines
+    TrackedLineInfo tl(
+	tf_listener,
+	finput_->header.frame_id,
+	cfg_switch_tolerance_,
+	cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1);
+    tl.update(l);
+    known_lines_.push_back(tl);
+  }
 
-  std::vector<LineInfo> linfos_filtered;
-  linfos_filtered.resize(linfos.size());
-  if(cfg_moving_avg_enabled_)
-  {
-    
-    for(size_t info_cntr = 0; info_cntr < linfos.size(); ++info_cntr)
-    {
-      std::deque<LineInfo> &moving_avg_window = moving_average_windows_[info_cntr];
-      LineInfo &info = linfos[info_cntr];
-      LineInfo &filtered = linfos_filtered[info_cntr];
-      moving_avg_window.push_front(LineInfo(info));
-      if(moving_avg_window.size() > cfg_moving_avg_window_size_) {
-        moving_avg_window.pop_back();
+  }
+
+  // When there are too many lines, delete the ones farthest away
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+	return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
       }
-      
-      float if_point_on_line_avg[3] = {0,0,0};
-      float if_line_direction_avg[3] = {0,0,0};
-      float if_end_point_1_avg[3] = {0,0,0};
-      float if_end_point_2_avg[3] = {0,0,0};
-      float bearing_avg = 0;
-      float length_avg = 0;
-      
-      for(const LineInfo &avg_info: moving_avg_window) {
-        if_point_on_line_avg[0] += avg_info.base_point[0];
-        if_point_on_line_avg[1] += avg_info.base_point[1];
-        if_point_on_line_avg[2] += avg_info.base_point[2];
-        if_line_direction_avg[0] += avg_info.line_direction[0];
-        if_line_direction_avg[1] += avg_info.line_direction[1];
-        if_line_direction_avg[2] += avg_info.line_direction[2];
-        if_end_point_1_avg[0] += avg_info.end_point_1[0];
-        if_end_point_1_avg[1] += avg_info.end_point_1[1];
-        if_end_point_1_avg[2] += avg_info.end_point_1[2];
-        if_end_point_2_avg[0] += avg_info.end_point_2[0];
-        if_end_point_2_avg[1] += avg_info.end_point_2[1];
-        if_end_point_2_avg[2] += avg_info.end_point_2[2];
-        bearing_avg += avg_info.bearing;
-        length_avg += avg_info.length;
+  );
+  while (known_lines_.size() > cfg_max_num_lines_)
+    known_lines_.erase(known_lines_.end() - 1);
+
+  // Then sort by bearing to stabilize blackboard interface assignment
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+	return l1.bearing_center < l2.bearing_center;
       }
-      
-      size_t queue_size = moving_avg_window.size();
-      filtered.base_point = {if_point_on_line_avg[0] / queue_size,
-                             if_point_on_line_avg[1] / queue_size,
-                             if_point_on_line_avg[2] / queue_size};
-      filtered.line_direction = {if_line_direction_avg[0] / queue_size,
-                                 if_line_direction_avg[1] / queue_size,
-                                 if_line_direction_avg[2] / queue_size};
-      filtered.end_point_1 = {if_end_point_1_avg[0] / queue_size,
-                              if_end_point_1_avg[1] / queue_size,
-                              if_end_point_1_avg[2] / queue_size};
-      filtered.end_point_2 = {if_end_point_2_avg[0] / queue_size,
-                              if_end_point_2_avg[1] / queue_size,
-                              if_end_point_2_avg[2] / queue_size};
-      filtered.bearing = bearing_avg / queue_size;
-      filtered.length = length_avg / queue_size;
-    }
-  }
+  );
+
   // set line parameters
   size_t oi = 0;
   unsigned int line_if_idx = 0;
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    const LineInfo &info_avg = linfos_filtered[i];
+  for (size_t i = 0; i < known_lines_.size(); ++i) {
+    const TrackedLineInfo &info = known_lines_[i];
 
     if (line_if_idx < cfg_max_num_lines_) {
-	    set_line(line_if_idx, line_ifs_[line_if_idx], true, finput_->header.frame_id, info);
+      set_line(line_if_idx, line_ifs_[line_if_idx], true, finput_->header.frame_id, info.raw);
       if(cfg_moving_avg_enabled_)
       {
-        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info_avg);
+        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info.smooth);
       }
       line_if_idx++;
     }
 
-    for (size_t p = 0; p < info.cloud->points.size(); ++p) {
+    for (size_t p = 0; p < info.raw.cloud->points.size(); ++p) {
       ColorPointType &out_point = lines_->points[oi++];
-      PointType &in_point  = info.cloud->points[p];
+      PointType &in_point  = info.raw.cloud->points[p];
       out_point.x = in_point.x;
       out_point.y = in_point.y;
       out_point.z = in_point.z;
@@ -394,17 +375,23 @@ LaserLinesThread::loop()
   }
 
   for (unsigned int i = line_if_idx; i < cfg_max_num_lines_; ++i) {
-	  set_line(i, line_ifs_[i], false);
+    set_line(i, line_ifs_[i], false);
     if(cfg_moving_avg_enabled_)
     {
-	    set_line(i, line_avg_ifs_[i], false);
+      set_line(i, line_avg_ifs_[i], false);
     }
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  publish_visualization(linfos, "laser_lines");
-  if(cfg_moving_avg_enabled_) {
-	  publish_visualization(linfos_filtered, "laser_lines_moving_average", "_avg");
+  { unsigned int line_id = 0;
+  for (size_t i = 0; i < known_lines_.size(); i++) {
+    publish_visualization(known_lines_[i].raw, i, line_id, "laser_lines");
+    line_id++;
+    if (cfg_moving_avg_enabled_) {
+      publish_visualization(known_lines_[i].smooth, i, line_id, "laser_lines_moving_average", "_avg");
+      line_id++;
+    }
+  }
   }
 #endif
 
@@ -429,6 +416,7 @@ LaserLinesThread::loop()
 }
 
 
+
 void
 LaserLinesThread::set_line(unsigned int idx,
                            fawkes::LaserLineInterface *iface,
@@ -523,15 +511,12 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos,
+LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, unsigned int idnum,
                                         std::string marker_namespace, std::string name_suffix)
 {
   visualization_msgs::MarkerArray m;
-  unsigned int idnum = 0;
 
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    
+  {
     /*
     visualization_msgs::Marker basevec;
     basevec.header.frame_id = finput_->header.frame_id;
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index 136cf13..a531a44 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -105,7 +105,7 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const std::vector<LineInfo> &linfos,
+  void publish_visualization(const LineInfo &linfo, size_t i, unsigned int idnum,
                              std::string marker_namespace, std::string name_suffix = "");
 #endif
 
@@ -117,7 +117,7 @@ class LaserLinesThread
 
   std::vector<fawkes::LaserLineInterface *> line_ifs_;
   std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
-  std::vector<std::deque<LineInfo>> moving_average_windows_;
+  std::vector<TrackedLineInfo> known_lines_;
 
   fawkes::SwitchInterface *switch_if_;
 
diff --git a/src/plugins/laser-lines/line_info.h b/src/plugins/laser-lines/line_info.h
index 9c60765..e77c154 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -25,6 +25,10 @@
 #include <Eigen/Geometry>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <boost/circular_buffer.hpp>
+#include <memory>
+#include <tf/types.h>
+#include <tf/transformer.h>
 
 /** Line information container.
  * All points and angles are in the sensor reference frame
@@ -49,4 +53,26 @@ class LineInfo {
   						///< points account to this line
 };
 
+
+class TrackedLineInfo {
+public:
+  LineInfo raw;   	///< the latest geometry of this line, i.e. unfiltered
+  LineInfo smooth;	///< moving-average geometry of this line (cf. length of history buffer)
+  fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;	///< last reference point (in odom frame) for line tracking
+  fawkes::tf::Transformer *transformer;	///< Transformer used to transform from input_frame_id_to odom
+  std::string input_frame_id;	///< Input frame ID of raw line infos (base_laser usually)
+  float cfg_switch_tolerance;	///< Configured line jitter threshold
+  boost::circular_buffer<LineInfo> history;	///< history of raw line geometries for computing moving average
+  float bearing_center; 	///< Bearing towards line center, used to select lines "in front of us" when there
+
+  TrackedLineInfo(
+      fawkes::tf::Transformer *tfer,
+      const std::string &input_frame_id,
+      float cfg_switch_tolerance,
+      unsigned int cfg_moving_avg_len);
+
+  btScalar distance(const LineInfo &linfo) const;
+  void update(LineInfo &new_linfo);
+};
+
 #endif

- *commit* 702e914cdd1ae1ea878ce7a46e21753b7b859808 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Thu Sep 1 11:05:09 2016 +0200
Subject: laser-lines: fix line deletion in visualization

 src/plugins/laser-lines/laser-lines-thread.cpp |  318 ++++++++++++------------
 src/plugins/laser-lines/laser-lines-thread.h   |   15 +-
 2 files changed, 173 insertions(+), 160 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index b9faf65..0581f61 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -39,7 +39,6 @@
 
 #ifdef HAVE_VISUAL_DEBUGGING
 #  include <ros/ros.h>
-#  include <visualization_msgs/MarkerArray.h>
 #endif
 
 #include <iostream>
@@ -383,16 +382,7 @@ LaserLinesThread::loop()
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  { unsigned int line_id = 0;
-  for (size_t i = 0; i < known_lines_.size(); i++) {
-    publish_visualization(known_lines_[i].raw, i, line_id, "laser_lines");
-    line_id++;
-    if (cfg_moving_avg_enabled_) {
-      publish_visualization(known_lines_[i].smooth, i, line_id, "laser_lines_moving_average", "_avg");
-      line_id++;
-    }
-  }
-  }
+  publish_visualization(known_lines_, "laser_lines", "laser_lines_moving_average");
 #endif
 
   //*lines_ = *tmp_lines;
@@ -511,15 +501,16 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, unsigned int idnum,
-                                        std::string marker_namespace, std::string name_suffix)
+LaserLinesThread::publish_visualization_add_line(visualization_msgs::MarkerArray &m,
+                                                 unsigned int &idnum,
+                                                 const LineInfo &info,
+                                                 const size_t i,
+                                                 const std::string &marker_namespace,
+                                                 const std::string &name_suffix)
 {
-  visualization_msgs::MarkerArray m;
-
-  {
-    /*
-    visualization_msgs::Marker basevec;
-    basevec.header.frame_id = finput_->header.frame_id;
+	/*
+	  visualization_msgs::Marker basevec;
+	  basevec.header.frame_id = finput_->header.frame_id;
     basevec.header.stamp = ros::Time::now();
     basevec.ns = marker_namespace;
     basevec.id = idnum++;
@@ -537,145 +528,160 @@ LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, unsigned
     basevec.color.a = 1.0;
     basevec.lifetime = ros::Duration(2, 0);
     m.markers.push_back(basevec);
-    */
-
-    visualization_msgs::Marker dirvec;
-    dirvec.header.frame_id = finput_->header.frame_id;
-    dirvec.header.stamp = ros::Time::now();
-    dirvec.ns = marker_namespace;
-    dirvec.id = idnum++;
-    dirvec.type = visualization_msgs::Marker::ARROW;
-    dirvec.action = visualization_msgs::Marker::ADD;
-    dirvec.points.resize(2);
-    dirvec.points[0].x = info.base_point[0];
-    dirvec.points[0].y = info.base_point[1];
-    dirvec.points[0].z = info.base_point[2];
-    dirvec.points[1].x = info.base_point[0] + info.line_direction[0];
-    dirvec.points[1].y = info.base_point[1] + info.line_direction[1];
-    dirvec.points[1].z = info.base_point[2] + info.line_direction[2];
-    dirvec.scale.x = 0.02;
-    dirvec.scale.y = 0.04;
-    dirvec.color.r = 0.0;
-    dirvec.color.g = 1.0;
-    dirvec.color.b = 0.f;
-    dirvec.color.a = 1.0;
-    dirvec.lifetime = ros::Duration(2, 0);
-    m.markers.push_back(dirvec);
-
-    visualization_msgs::Marker testvec;
-    testvec.header.frame_id = finput_->header.frame_id;
-    testvec.header.stamp = ros::Time::now();
-    testvec.ns = marker_namespace;
-    testvec.id = idnum++;
-    testvec.type = visualization_msgs::Marker::ARROW;
-    testvec.action = visualization_msgs::Marker::ADD;
-    testvec.points.resize(2);
-    testvec.points[0].x = 0; //info.point_on_line[0];
-    testvec.points[0].y = 0; //info.point_on_line[1];
-    testvec.points[0].z = 0; //info.point_on_line[2];
-    testvec.points[1].x = info.base_point[0];
-    testvec.points[1].y = info.base_point[1];
-    testvec.points[1].z = info.base_point[2];
-    testvec.scale.x = 0.02;
-    testvec.scale.y = 0.04;
-    testvec.color.r = line_colors[i][0] / 255.;
-    testvec.color.g = line_colors[i][1] / 255.;
-    testvec.color.b = line_colors[i][2] / 255.;
-    testvec.color.a = 1.0;
-    testvec.lifetime = ros::Duration(2, 0);
-    m.markers.push_back(testvec);
+	*/
 
-    char *tmp;
-    if (asprintf(&tmp, "L_%zu%s", i+1, name_suffix.c_str()) != -1) {
-      // Copy to get memory freed on exception
-      std::string id = tmp;
-      free(tmp);
-
-      visualization_msgs::Marker text;
-      text.header.frame_id = finput_->header.frame_id;
-      text.header.stamp = ros::Time::now();
-      text.ns = marker_namespace;
-      text.id = idnum++;
-      text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-      text.action = visualization_msgs::Marker::ADD;
-      text.pose.position.x = info.base_point[0];
-      text.pose.position.y = info.base_point[1];
-      text.pose.position.z = info.base_point[2] + .15;
-      text.pose.orientation.w = 1.;
-      text.scale.z = 0.15;
-      text.color.r = text.color.g = text.color.b = 1.0f;
-      text.color.a = 1.0;
-      text.lifetime = ros::Duration(2, 0);
-      text.text = id;
-      m.markers.push_back(text);
-    }
+	visualization_msgs::Marker dirvec;
+	dirvec.header.frame_id = finput_->header.frame_id;
+	dirvec.header.stamp = ros::Time::now();
+	dirvec.ns = marker_namespace;
+	dirvec.id = idnum++;
+	dirvec.type = visualization_msgs::Marker::ARROW;
+	dirvec.action = visualization_msgs::Marker::ADD;
+	dirvec.points.resize(2);
+	dirvec.points[0].x = info.base_point[0];
+	dirvec.points[0].y = info.base_point[1];
+	dirvec.points[0].z = info.base_point[2];
+	dirvec.points[1].x = info.base_point[0] + info.line_direction[0];
+	dirvec.points[1].y = info.base_point[1] + info.line_direction[1];
+	dirvec.points[1].z = info.base_point[2] + info.line_direction[2];
+	dirvec.scale.x = 0.02;
+	dirvec.scale.y = 0.04;
+	dirvec.color.r = 0.0;
+	dirvec.color.g = 1.0;
+	dirvec.color.b = 0.f;
+	dirvec.color.a = 1.0;
+	dirvec.lifetime = ros::Duration(2, 0);
+	m.markers.push_back(dirvec);
+
+	visualization_msgs::Marker testvec;
+	testvec.header.frame_id = finput_->header.frame_id;
+	testvec.header.stamp = ros::Time::now();
+	testvec.ns = marker_namespace;
+	testvec.id = idnum++;
+	testvec.type = visualization_msgs::Marker::ARROW;
+	testvec.action = visualization_msgs::Marker::ADD;
+	testvec.points.resize(2);
+	testvec.points[0].x = 0; //info.point_on_line[0];
+	testvec.points[0].y = 0; //info.point_on_line[1];
+	testvec.points[0].z = 0; //info.point_on_line[2];
+	testvec.points[1].x = info.base_point[0];
+	testvec.points[1].y = info.base_point[1];
+	testvec.points[1].z = info.base_point[2];
+	testvec.scale.x = 0.02;
+	testvec.scale.y = 0.04;
+	testvec.color.r = line_colors[i][0] / 255.;
+	testvec.color.g = line_colors[i][1] / 255.;
+	testvec.color.b = line_colors[i][2] / 255.;
+	testvec.color.a = 1.0;
+	testvec.lifetime = ros::Duration(2, 0);
+	m.markers.push_back(testvec);
+
+	char *tmp;
+	if (asprintf(&tmp, "L_%zu%s", i+1, name_suffix.c_str()) != -1) {
+		// Copy to get memory freed on exception
+		std::string id = tmp;
+		free(tmp);
+
+		visualization_msgs::Marker text;
+		text.header.frame_id = finput_->header.frame_id;
+		text.header.stamp = ros::Time::now();
+		text.ns = marker_namespace;
+		text.id = idnum++;
+		text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+		text.action = visualization_msgs::Marker::ADD;
+		text.pose.position.x = info.base_point[0];
+		text.pose.position.y = info.base_point[1];
+		text.pose.position.z = info.base_point[2] + .15;
+		text.pose.orientation.w = 1.;
+		text.scale.z = 0.15;
+		text.color.r = text.color.g = text.color.b = 1.0f;
+		text.color.a = 1.0;
+		text.lifetime = ros::Duration(2, 0);
+		text.text = id;
+		m.markers.push_back(text);
+	}
 
-    if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
-      visualization_msgs::Marker sphere_ep_1;
-      sphere_ep_1.header.frame_id = finput_->header.frame_id;
-      sphere_ep_1.header.stamp = ros::Time::now();
-      sphere_ep_1.ns = marker_namespace;
-      sphere_ep_1.id = idnum++;
-      sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
-      sphere_ep_1.action = visualization_msgs::Marker::ADD;
-      sphere_ep_1.pose.position.x = info.end_point_1[0];
-      sphere_ep_1.pose.position.y = info.end_point_1[1];
-      sphere_ep_1.pose.position.z = info.end_point_1[2];
-      sphere_ep_1.pose.orientation.w = 1.;
-      sphere_ep_1.scale.x = 0.05;
-      sphere_ep_1.scale.y = 0.05;
-      sphere_ep_1.scale.z = 0.05;
-      sphere_ep_1.color.r = line_colors[i][0] / 255.;
-      sphere_ep_1.color.g = line_colors[i][1] / 255.;
-      sphere_ep_1.color.b = line_colors[i][2] / 255.;
-      sphere_ep_1.color.a = 1.0;
-      sphere_ep_1.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(sphere_ep_1);
-
-      visualization_msgs::Marker sphere_ep_2;
-      sphere_ep_2.header.frame_id = finput_->header.frame_id;
-      sphere_ep_2.header.stamp = ros::Time::now();
-      sphere_ep_2.ns = marker_namespace;
-      sphere_ep_2.id = idnum++;
-      sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
-      sphere_ep_2.action = visualization_msgs::Marker::ADD;
-      sphere_ep_2.pose.position.x = info.end_point_2[0];
-      sphere_ep_2.pose.position.y = info.end_point_2[1];
-      sphere_ep_2.pose.position.z = info.end_point_2[2];
-      sphere_ep_2.pose.orientation.w = 1.;
-      sphere_ep_2.scale.x = 0.05;
-      sphere_ep_2.scale.y = 0.05;
-      sphere_ep_2.scale.z = 0.05;
-      sphere_ep_2.color.r = line_colors[i][0] / 255.;
-      sphere_ep_2.color.g = line_colors[i][1] / 255.;
-      sphere_ep_2.color.b = line_colors[i][2] / 255.;
-      sphere_ep_2.color.a = 1.0;
-      sphere_ep_2.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(sphere_ep_2);
-
-      visualization_msgs::Marker lineseg;
-      lineseg.header.frame_id = finput_->header.frame_id;
-      lineseg.header.stamp = ros::Time::now();
-      lineseg.ns = marker_namespace;
-      lineseg.id = idnum++;
-      lineseg.type = visualization_msgs::Marker::LINE_LIST;
-      lineseg.action = visualization_msgs::Marker::ADD;
-      lineseg.points.resize(2);
-      lineseg.points[0].x = info.end_point_1[0];
-      lineseg.points[0].y = info.end_point_1[1];
-      lineseg.points[0].z = info.end_point_1[2];
-      lineseg.points[1].x = info.end_point_2[0];
-      lineseg.points[1].y = info.end_point_2[1];
-      lineseg.points[1].z = info.end_point_2[2];
-      lineseg.scale.x = 0.02;
-      lineseg.scale.y = 0.04;
-      lineseg.color.r = line_colors[i][0] / 255.;
-      lineseg.color.g = line_colors[i][1] / 255.;
-      lineseg.color.b = line_colors[i][2] / 255.;
-      lineseg.color.a = 1.0;
-      lineseg.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(lineseg);
-    }
+	if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
+		visualization_msgs::Marker sphere_ep_1;
+		sphere_ep_1.header.frame_id = finput_->header.frame_id;
+		sphere_ep_1.header.stamp = ros::Time::now();
+		sphere_ep_1.ns = marker_namespace;
+		sphere_ep_1.id = idnum++;
+		sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
+		sphere_ep_1.action = visualization_msgs::Marker::ADD;
+		sphere_ep_1.pose.position.x = info.end_point_1[0];
+		sphere_ep_1.pose.position.y = info.end_point_1[1];
+		sphere_ep_1.pose.position.z = info.end_point_1[2];
+		sphere_ep_1.pose.orientation.w = 1.;
+		sphere_ep_1.scale.x = 0.05;
+		sphere_ep_1.scale.y = 0.05;
+		sphere_ep_1.scale.z = 0.05;
+		sphere_ep_1.color.r = line_colors[i][0] / 255.;
+		sphere_ep_1.color.g = line_colors[i][1] / 255.;
+		sphere_ep_1.color.b = line_colors[i][2] / 255.;
+		sphere_ep_1.color.a = 1.0;
+		sphere_ep_1.lifetime = ros::Duration(2, 0);
+		m.markers.push_back(sphere_ep_1);
+
+		visualization_msgs::Marker sphere_ep_2;
+		sphere_ep_2.header.frame_id = finput_->header.frame_id;
+		sphere_ep_2.header.stamp = ros::Time::now();
+		sphere_ep_2.ns = marker_namespace;
+		sphere_ep_2.id = idnum++;
+		sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
+		sphere_ep_2.action = visualization_msgs::Marker::ADD;
+		sphere_ep_2.pose.position.x = info.end_point_2[0];
+		sphere_ep_2.pose.position.y = info.end_point_2[1];
+		sphere_ep_2.pose.position.z = info.end_point_2[2];
+		sphere_ep_2.pose.orientation.w = 1.;
+		sphere_ep_2.scale.x = 0.05;
+		sphere_ep_2.scale.y = 0.05;
+		sphere_ep_2.scale.z = 0.05;
+		sphere_ep_2.color.r = line_colors[i][0] / 255.;
+		sphere_ep_2.color.g = line_colors[i][1] / 255.;
+		sphere_ep_2.color.b = line_colors[i][2] / 255.;
+		sphere_ep_2.color.a = 1.0;
+		sphere_ep_2.lifetime = ros::Duration(2, 0);
+		m.markers.push_back(sphere_ep_2);
+
+		visualization_msgs::Marker lineseg;
+		lineseg.header.frame_id = finput_->header.frame_id;
+		lineseg.header.stamp = ros::Time::now();
+		lineseg.ns = marker_namespace;
+		lineseg.id = idnum++;
+		lineseg.type = visualization_msgs::Marker::LINE_LIST;
+		lineseg.action = visualization_msgs::Marker::ADD;
+		lineseg.points.resize(2);
+		lineseg.points[0].x = info.end_point_1[0];
+		lineseg.points[0].y = info.end_point_1[1];
+		lineseg.points[0].z = info.end_point_1[2];
+		lineseg.points[1].x = info.end_point_2[0];
+		lineseg.points[1].y = info.end_point_2[1];
+		lineseg.points[1].z = info.end_point_2[2];
+		lineseg.scale.x = 0.02;
+		lineseg.scale.y = 0.04;
+		lineseg.color.r = line_colors[i][0] / 255.;
+		lineseg.color.g = line_colors[i][1] / 255.;
+		lineseg.color.b = line_colors[i][2] / 255.;
+		lineseg.color.a = 1.0;
+		lineseg.lifetime = ros::Duration(2, 0);
+		m.markers.push_back(lineseg);
+	}
+}
+
+void
+LaserLinesThread::publish_visualization(const std::vector<TrackedLineInfo> &linfos,
+                                        const std::string &marker_namespace,
+                                        const std::string &avg_marker_namespace)
+{
+  visualization_msgs::MarkerArray m;
+
+  unsigned int idnum = 0;
+ 
+  for (size_t i = 0; i < linfos.size(); ++i) {
+	  const TrackedLineInfo &info = linfos[i];
+	  publish_visualization_add_line(m, idnum, info.raw, i, marker_namespace);
+	  publish_visualization_add_line(m, idnum, info.smooth, i, avg_marker_namespace, "_avg");
   }
 
   for (size_t i = idnum; i < last_id_num_; ++i) {
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index a531a44..a7e4138 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -40,10 +40,9 @@
 #include <Eigen/StdVector>
 #include <pcl/ModelCoefficients.h>
 
-#include <deque>
-
 #ifdef HAVE_VISUAL_DEBUGGING
 #  include <plugins/ros/aspect/ros.h>
+#  include <visualization_msgs/MarkerArray.h>
 
 namespace ros {
   class Publisher;
@@ -105,8 +104,16 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const LineInfo &linfo, size_t i, unsigned int idnum,
-                             std::string marker_namespace, std::string name_suffix = "");
+  void publish_visualization(const std::vector<TrackedLineInfo> &linfos,
+                             const std::string &marker_namespace,
+                             const std::string &avg_marker_namespace);
+
+  void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
+                                      unsigned int &idnum,
+                                      const LineInfo &info,
+                                      const size_t i,
+                                      const std::string &marker_namespace,
+                                      const std::string &name_suffix = "");
 #endif
 
  private:

- *commit* d4a5c134dbf5991c976f4011eb7794b72c67099d - - - - - - - - - -
Author:  Victor Mataré <matare at lih.rwth-aachen.de>
Date:    Fri Sep 2 15:03:28 2016 +0200
Subject: laser-lines: catch TransformException

 src/plugins/laser-lines/laser-lines-thread.cpp |    3 ++-
 src/plugins/laser-lines/line_info.cpp          |   22 ++++++++++++++++++----
 src/plugins/laser-lines/line_info.h            |    7 ++++++-
 3 files changed, 26 insertions(+), 6 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index 0581f61..a51b438 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -316,7 +316,8 @@ LaserLinesThread::loop()
 	tf_listener,
 	finput_->header.frame_id,
 	cfg_switch_tolerance_,
-	cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1);
+	cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1,
+	logger, name());
     tl.update(l);
     known_lines_.push_back(tl);
   }
diff --git a/src/plugins/laser-lines/line_info.cpp b/src/plugins/laser-lines/line_info.cpp
index bd7689c..5307dd6 100644
--- a/src/plugins/laser-lines/line_info.cpp
+++ b/src/plugins/laser-lines/line_info.cpp
@@ -29,12 +29,16 @@ TrackedLineInfo::TrackedLineInfo(
     fawkes::tf::Transformer *tfer,
     const string &input_frame_id,
     float cfg_switch_tolerance,
-    unsigned int cfg_moving_avg_len)
+    unsigned int cfg_moving_avg_len,
+    fawkes::Logger *logger,
+    string plugin_name)
 : transformer(tfer),
   input_frame_id(input_frame_id),
   cfg_switch_tolerance(cfg_switch_tolerance),
   history(cfg_moving_avg_len),
-  bearing_center(0)
+  bearing_center(0),
+  logger(logger),
+  plugin_name(plugin_name)
 {}
 
 
@@ -49,7 +53,12 @@ btScalar TrackedLineInfo::distance(const LineInfo &linfo) const
 	      linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
 	  ), fawkes::Time(0,0), input_frame_id);
   fawkes::tf::Stamped<fawkes::tf::Point> bp_odom_new;
-  transformer->transform_point("/odom", bp_new, bp_odom_new);
+  try {
+    transformer->transform_point("/odom", bp_new, bp_odom_new);
+  } catch (fawkes::tf::TransformException &e) {
+    // Continue without tf, track in input frame instead. Warning follows on update() call.
+    bp_odom_new = bp_new;
+  }
 
   return (bp_odom_new - this->base_point_odom).length();
 }
@@ -66,7 +75,12 @@ void TrackedLineInfo::update(LineInfo &linfo)
 	  fawkes::tf::Point(
 	      linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
 	  ), fawkes::Time(0,0), input_frame_id);
-  transformer->transform_point("/odom", bp_new, this->base_point_odom);
+  try {
+    transformer->transform_point("/odom", bp_new, this->base_point_odom);
+  } catch (fawkes::tf::TransformException &e) {
+    logger->log_warn(plugin_name.c_str(), "Can't transform to odom. Attempting to track in %s.", input_frame_id);
+    this->base_point_odom = bp_new;
+  }
   this->history.push_back(linfo);
 
   Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
diff --git a/src/plugins/laser-lines/line_info.h b/src/plugins/laser-lines/line_info.h
index e77c154..d5e427e 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -29,6 +29,7 @@
 #include <memory>
 #include <tf/types.h>
 #include <tf/transformer.h>
+#include <logging/logger.h>
 
 /** Line information container.
  * All points and angles are in the sensor reference frame
@@ -64,12 +65,16 @@ public:
   float cfg_switch_tolerance;	///< Configured line jitter threshold
   boost::circular_buffer<LineInfo> history;	///< history of raw line geometries for computing moving average
   float bearing_center; 	///< Bearing towards line center, used to select lines "in front of us" when there
+  fawkes::Logger *logger;
+  std::string plugin_name;
 
   TrackedLineInfo(
       fawkes::tf::Transformer *tfer,
       const std::string &input_frame_id,
       float cfg_switch_tolerance,
-      unsigned int cfg_moving_avg_len);
+      unsigned int cfg_moving_avg_len,
+      fawkes::Logger *logger,
+      std::string plugin_name);
 
   btScalar distance(const LineInfo &linfo) const;
   void update(LineInfo &new_linfo);

- *commit* 6a2f4ec95c06887ff7637e9e44e534b119ee3e34 - - - - - - - - - -
Author:  Victor Mataré <matare at lih.rwth-aachen.de>
Date:    Fri Sep 2 15:18:10 2016 +0200
Subject: laser-lines: use configurable odom frame for tracking

 src/plugins/laser-lines/laser-lines-thread.cpp |    3 +++
 src/plugins/laser-lines/laser-lines-thread.h   |    1 +
 src/plugins/laser-lines/line_info.cpp          |    9 ++++++---
 src/plugins/laser-lines/line_info.h            |    6 ++++--
 4 files changed, 14 insertions(+), 5 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index a51b438..a6092d8 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -107,6 +107,8 @@ LaserLinesThread::init()
   cfg_result_frame_          = config->get_string(CFG_PREFIX"result_frame");
   cfg_max_num_lines_         = config->get_uint(CFG_PREFIX"max_num_lines");
 
+  cfg_tracking_frame_id_     = config->get_string("/frames/odom");
+
   finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pcl_.c_str());
   input_ = pcl_utils::cloudptr_from_refptr(finput_);
 
@@ -315,6 +317,7 @@ LaserLinesThread::loop()
     TrackedLineInfo tl(
 	tf_listener,
 	finput_->header.frame_id,
+	cfg_tracking_frame_id_,
 	cfg_switch_tolerance_,
 	cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1,
 	logger, name());
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index a7e4138..b8a461e 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -149,6 +149,7 @@ class LaserLinesThread
   float        cfg_max_dist_;
   bool         cfg_moving_avg_enabled_;
   unsigned int cfg_moving_avg_window_size_;
+  std::string  cfg_tracking_frame_id_;
 
   unsigned int loop_count_;
 
diff --git a/src/plugins/laser-lines/line_info.cpp b/src/plugins/laser-lines/line_info.cpp
index 5307dd6..97cd804 100644
--- a/src/plugins/laser-lines/line_info.cpp
+++ b/src/plugins/laser-lines/line_info.cpp
@@ -28,12 +28,14 @@ using namespace std;
 TrackedLineInfo::TrackedLineInfo(
     fawkes::tf::Transformer *tfer,
     const string &input_frame_id,
+    const string &tracking_frame_id,
     float cfg_switch_tolerance,
     unsigned int cfg_moving_avg_len,
     fawkes::Logger *logger,
     string plugin_name)
 : transformer(tfer),
   input_frame_id(input_frame_id),
+  tracking_frame_id(tracking_frame_id),
   cfg_switch_tolerance(cfg_switch_tolerance),
   history(cfg_moving_avg_len),
   bearing_center(0),
@@ -54,7 +56,7 @@ btScalar TrackedLineInfo::distance(const LineInfo &linfo) const
 	  ), fawkes::Time(0,0), input_frame_id);
   fawkes::tf::Stamped<fawkes::tf::Point> bp_odom_new;
   try {
-    transformer->transform_point("/odom", bp_new, bp_odom_new);
+    transformer->transform_point(tracking_frame_id, bp_new, bp_odom_new);
   } catch (fawkes::tf::TransformException &e) {
     // Continue without tf, track in input frame instead. Warning follows on update() call.
     bp_odom_new = bp_new;
@@ -76,9 +78,10 @@ void TrackedLineInfo::update(LineInfo &linfo)
 	      linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
 	  ), fawkes::Time(0,0), input_frame_id);
   try {
-    transformer->transform_point("/odom", bp_new, this->base_point_odom);
+    transformer->transform_point(tracking_frame_id, bp_new, this->base_point_odom);
   } catch (fawkes::tf::TransformException &e) {
-    logger->log_warn(plugin_name.c_str(), "Can't transform to odom. Attempting to track in %s.", input_frame_id);
+    logger->log_warn(plugin_name.c_str(), "Can't transform to %s. Attempting to track in %s.",
+	tracking_frame_id, input_frame_id);
     this->base_point_odom = bp_new;
   }
   this->history.push_back(linfo);
diff --git a/src/plugins/laser-lines/line_info.h b/src/plugins/laser-lines/line_info.h
index d5e427e..7dddbc9 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -62,15 +62,17 @@ public:
   fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;	///< last reference point (in odom frame) for line tracking
   fawkes::tf::Transformer *transformer;	///< Transformer used to transform from input_frame_id_to odom
   std::string input_frame_id;	///< Input frame ID of raw line infos (base_laser usually)
+  std::string tracking_frame_id;	///< Track lines relative to this frame (e.g. odom helps compensate movement)
   float cfg_switch_tolerance;	///< Configured line jitter threshold
   boost::circular_buffer<LineInfo> history;	///< history of raw line geometries for computing moving average
   float bearing_center; 	///< Bearing towards line center, used to select lines "in front of us" when there
-  fawkes::Logger *logger;
-  std::string plugin_name;
+  fawkes::Logger *logger;	///< Logger pointer of the calling class
+  std::string plugin_name;	///< Plugin name of the calling class
 
   TrackedLineInfo(
       fawkes::tf::Transformer *tfer,
       const std::string &input_frame_id,
+      const std::string &tracking_frame_id,
       float cfg_switch_tolerance,
       unsigned int cfg_moving_avg_len,
       fawkes::Logger *logger,




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list