From c3d22b8780bfaf02cacd339e67bf64ce409a1196 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Tue, 16 May 2017 11:51:09 +0200
Subject: [PATCH] add func get Pose/Increment

---
 .../rf2o_laser_odometry/CLaserOdometry2D.h    |  8 ++++++
 src/CLaserOdometry2D.cpp                      | 26 ++++++++++++++++++-
 src/CLaserOdometry2DNode.cpp                  | 12 ---------
 3 files changed, 33 insertions(+), 13 deletions(-)

diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h
index 576546f..e460566 100644
--- a/include/rf2o_laser_odometry/CLaserOdometry2D.h
+++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h
@@ -79,6 +79,12 @@ protected:
 
     void setLaserPose(const tf::Transform& laser_pose);
 
+    mrpt::poses::CPose3D& getIncrement();
+    const mrpt::poses::CPose3D& getIncrement() const;
+
+    mrpt::poses::CPose3D& getPose();
+    const mrpt::poses::CPose3D& getPose() const;
+
 protected:
 
   bool module_initialized,first_laser_scan;
@@ -137,6 +143,8 @@ protected:
 	mrpt::math::CMatrixFloat31 kai_loc_old;
 	mrpt::math::CMatrixFloat31 kai_loc_level;
 
+  mrpt::poses::CPose3D last_increment;
+
   mrpt::poses::CPose3D laser_pose_on_robot;
   mrpt::poses::CPose3D laser_pose_on_robot_inv;
 
diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp
index a7eab3b..9cd98f5 100644
--- a/src/CLaserOdometry2D.cpp
+++ b/src/CLaserOdometry2D.cpp
@@ -26,7 +26,9 @@ using namespace Eigen;
 // CLaserOdometry2D
 //---------------------------------------------
 
-CLaserOdometry2D::CLaserOdometry2D()
+CLaserOdometry2D::CLaserOdometry2D() :
+  module_initialized(false),
+  first_laser_scan(true)
 {
   //
 }
@@ -175,6 +177,26 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
 }
 
 
+mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement()
+{
+  return last_increment;
+}
+
+const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const
+{
+  return last_increment;
+}
+
+mrpt::poses::CPose3D& CLaserOdometry2D::getPose()
+{
+  return robot_pose;
+}
+
+const mrpt::poses::CPose3D& CLaserOdometry2D::getPose() const
+{
+  return robot_pose;
+}
+
 void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
 {
     //==================================================================================
@@ -896,6 +918,8 @@ void CLaserOdometry2D::PoseUpdate()
 	poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
         laser_pose = laser_pose + CPose3D(pose_aux_2D);
 
+  last_increment = pose_aux_2D;
+
 
 
 	//				Compute kai_loc_old
diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp
index 97ebc3c..039940d 100644
--- a/src/CLaserOdometry2DNode.cpp
+++ b/src/CLaserOdometry2DNode.cpp
@@ -224,18 +224,6 @@ void CLaserOdometry2DNode::publish()
   odom_pub.publish(odom);
 }
 
-//void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
-//{
-//	//Set the initial pose
-//	laser_pose = ini_pose;
-//	laser_oldpose = ini_pose;
-
-//    //readLaser(scan);
-//	createImagePyramid();
-//    //readLaser(scan);
-//	createImagePyramid();
-//}
-
 //-----------------------------------------------------------------------------------
 //                                   MAIN
 //-----------------------------------------------------------------------------------