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

Transform received goals in /map frame (if not already in /map frame)

Browse Source
This commit is contained in:
matlabbe
2016-06-22 12:03:02 -04:00
parent 5da8245ebf
commit 01b05a0f5c
1 changed files with 14 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+14
View File
@@ -1697,6 +1697,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);
}