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

8
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -79,6 +79,12 @@ protected:
void setLaserPose(const tf::Transform& laser_pose); 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: protected:
bool module_initialized,first_laser_scan; bool module_initialized,first_laser_scan;
@@ -137,6 +143,8 @@ protected:
mrpt::math::CMatrixFloat31 kai_loc_old; mrpt::math::CMatrixFloat31 kai_loc_old;
mrpt::math::CMatrixFloat31 kai_loc_level; 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;
mrpt::poses::CPose3D laser_pose_on_robot_inv; mrpt::poses::CPose3D laser_pose_on_robot_inv;

26
src/CLaserOdometry2D.cpp
View File

@@ -26,7 +26,9 @@ using namespace Eigen;
// CLaserOdometry2D // 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) 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); 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); laser_pose = laser_pose + CPose3D(pose_aux_2D);
last_increment = pose_aux_2D;
// Compute kai_loc_old // Compute kai_loc_old

12
src/CLaserOdometry2DNode.cpp
View File

@@ -224,18 +224,6 @@ void CLaserOdometry2DNode::publish()
odom_pub.publish(odom); 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 // MAIN
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------