[Fawkes Git] branch/vmatare/laser-lines-matching-refactored: laser-lines: use /frames/odom as tracking frame

Victor Mataré matare at lih.rwth-aachen.de
Fri Sep 2 15:18:49 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-refactored has been updated
        to  c3342d961529c48429658fd4a62673cfaac1a04d (commit)
      from  d4a5c134dbf5991c976f4011eb7794b72c67099d (commit)

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

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit c3342d961529c48429658fd4a62673cfaac1a04d
Author:     Victor Mataré <matare at lih.rwth-aachen.de>
AuthorDate: Fri Sep 2 15:18:10 2016 +0200
Commit:     Victor Mataré <matare at lih.rwth-aachen.de>
CommitDate: Fri Sep 2 15:18:10 2016 +0200

    laser-lines: use /frames/odom as tracking frame

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

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


- *Summary* -----------------------------------------------------------
 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(-)


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

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

 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