mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

add func get Pose/Increment

Browse Source
This commit is contained in:
Jeremie Deray
2017-05-16 11:51:09 +02:00
committed by Jeremie Deray
parent 241358540a
commit c3d22b8780
3 changed files with 33 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/CLaserOdometry2D.cpp
View File

@@ -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