i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added gazebo_ground_truth.py. Odom: fixed ground truth init when imu is also used.

Browse Source
This commit is contained in:
matlabbe
2022-02-06 17:15:47 -05:00
parent 452be2cc61
commit 096d8731b4
3 changed files with 60 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+5 -1
View File
@@ -538,7 +538,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odometry_->getPose().isIdentity())
// Use only XYZ to handle the case odometry was previously initialized with IMU,
// we assume that the ground truth contains also a real initial orientation
float x,y,z;
odometry_->getPose().getTranslation(x, y, z);
if(x==0.0f && y==0.0f && z==0.0f)
{
// sync with the first value of the ground truth
if(groundTruth.isNull())