[Fawkes Git] branch/vmatare/laser-lines-matching-refactored: laser-lines: catch TransformException

Victor Mataré matare at lih.rwth-aachen.de
Fri Sep 2 15:04:52 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  d4a5c134dbf5991c976f4011eb7794b72c67099d (commit)
      from  702e914cdd1ae1ea878ce7a46e21753b7b859808 (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 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

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


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


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

- *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);




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


More information about the fawkes-commits mailing list