[Fawkes Git] branch/timn/map-lasergen-fix: created (0.5.0-3082-gfa019ab)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Wed Apr 13 23:12:48 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, timn/map-lasergen-fix has been created
        at  fa019abc62abce6e43c43b2848d3fe930abee732 (commit)

http://git.fawkesrobotics.org/fawkes.git/timn/map-lasergen-fix

- *Log* ---------------------------------------------------------------
commit fa019abc62abce6e43c43b2848d3fe930abee732
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Wed Apr 13 23:10:14 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Wed Apr 13 23:10:14 2016 +0200

    map-lasergen: fix calculation of laser position
    
    The static transform from base_link to base_laser (or whatever the frame
    names are) was simply added to the pose. This is ok in exactly one case:
    when the orientation of the robot and of the laser align.
    
    The fix is to properly project/rotate the laser pose.

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

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


- *Summary* -----------------------------------------------------------


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

- *commit* fa019abc62abce6e43c43b2848d3fe930abee732 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Wed Apr 13 23:10:14 2016 +0200
Subject: map-lasergen: fix calculation of laser position

 src/plugins/amcl/map_lasergen_thread.cpp |    8 ++++++--
 1 files changed, 6 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/amcl/map_lasergen_thread.cpp b/src/plugins/amcl/map_lasergen_thread.cpp
index c605b37..3d1ce70 100644
--- a/src/plugins/amcl/map_lasergen_thread.cpp
+++ b/src/plugins/amcl/map_lasergen_thread.cpp
@@ -158,9 +158,13 @@ MapLaserGenThread::loop()
 
     //logger->log_info(name(), "Pos: (%f,%f,%f)", pos_x_, pos_y_, pos_theta_);
 
-    laser_pos_x_ = pos_x_ + laser_pose_.getOrigin().x();
-    laser_pos_y_ = pos_y_ + laser_pose_.getOrigin().y();
+    const float lx = laser_pose_.getOrigin().x();
+    const float ly = laser_pose_.getOrigin().y();
     laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
+    const float sin_theta = sinf(laser_pos_theta_);
+    const float cos_theta = cosf(laser_pos_theta_);
+    laser_pos_x_ = pos_x_ + (lx * cos_theta - ly * sin_theta);
+    laser_pos_y_ = pos_y_ + (lx * sin_theta + ly * cos_theta);
   }
 
   tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));




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


More information about the fawkes-commits mailing list