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

Merge branch 'master' of github.com:introlab/rtabmap_ros into devel

Browse Source
This commit is contained in:
matlabbe
2016-06-22 12:04:07 -04:00
parent 400a38174e 01b05a0f5c
commit 43d71ed139
2 changed files with 15 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+14
View File
@@ -1712,6 +1712,20 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
ROS_ERROR("Pose received is null!");
return;
}
// transform goal in /map frame
if(mapFrameId_.compare(msg->header.frame_id) != 0)
{
Transform t = this->getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp);
if(t.isNull())
{
ROS_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
msg->header.frame_id.c_str(), mapFrameId_.c_str());
return;
}
targetPose = t * targetPose;
}
goalCommonCallback(0, "", targetPose, msg->header.stamp);
}