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

added sensorToBase transform on global_pose

Browse Source
This commit is contained in:
matlabbe
2017-05-24 14:55:08 -04:00
parent 1cbff792fb
commit c8787cd005
1 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+6 -4
View File
@@ -958,14 +958,17 @@ void CoreWrapper::commonDepthCallbackImpl(
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform baseToSensor = rtabmap_ros::getTransform(
frameId_,
Transform sensorToBase = rtabmap_ros::getTransform(
globalPose_.header.frame_id,
frameId_,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!baseToSensor.isNull())
if(!sensorToBase.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
@@ -974,7 +977,6 @@ void CoreWrapper::commonDepthCallbackImpl(
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
if(!correction.isNull())
{
globalPose *= correction;