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

rtabmapviz: use child_frame_id from odom topic (when used) instead of frame_id parameter for base frame

Browse Source
This commit is contained in:
matlabbe
2020-12-18 17:04:28 -05:00
parent 2b0e313a6d
commit 43dbd8ad13
1 changed files with 20 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+20 -14
View File
@@ -436,9 +436,11 @@ void GuiWrapper::commonDepthCallback(
UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size())); UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size()));
std_msgs::Header odomHeader; std_msgs::Header odomHeader;
std::string frameId = frameId_;
if(odomMsg.get()) if(odomMsg.get())
{ {
odomHeader = odomMsg->header; odomHeader = odomMsg->header;
frameId = odomMsg->child_frame_id;
} }
else else
{ {
@@ -465,7 +467,7 @@ void GuiWrapper::commonDepthCallback(
odomHeader.frame_id = odomFrameId_; odomHeader.frame_id = odomFrameId_;
} }
Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {
@@ -519,7 +521,7 @@ void GuiWrapper::commonDepthCallback(
imageMsgs, imageMsgs,
depthMsgs, depthMsgs,
cameraInfoMsgs, cameraInfoMsgs,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
rgb, rgb,
@@ -537,7 +539,7 @@ void GuiWrapper::commonDepthCallback(
{ {
if(!rtabmap_ros::convertScanMsg( if(!rtabmap_ros::convertScanMsg(
scan2dMsg, scan2dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -552,7 +554,7 @@ void GuiWrapper::commonDepthCallback(
{ {
if(!rtabmap_ros::convertScan3dMsg( if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg, scan3dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -612,9 +614,11 @@ void GuiWrapper::commonStereoCallback(
const cv::Mat & localDescriptors) const cv::Mat & localDescriptors)
{ {
std_msgs::Header odomHeader; std_msgs::Header odomHeader;
std::string frameId = frameId_;
if(odomMsg.get()) if(odomMsg.get())
{ {
odomHeader = odomMsg->header; odomHeader = odomMsg->header;
frameId = odomMsg->child_frame_id;
} }
else else
{ {
@@ -633,7 +637,7 @@ void GuiWrapper::commonStereoCallback(
odomHeader.frame_id = odomFrameId_; odomHeader.frame_id = odomFrameId_;
} }
Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {
@@ -686,7 +690,7 @@ void GuiWrapper::commonStereoCallback(
rightImageMsg, rightImageMsg,
leftCamInfoMsg, leftCamInfoMsg,
rightCamInfoMsg, rightCamInfoMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
left, left,
@@ -704,7 +708,7 @@ void GuiWrapper::commonStereoCallback(
{ {
if(!rtabmap_ros::convertScanMsg( if(!rtabmap_ros::convertScanMsg(
scan2dMsg, scan2dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -719,7 +723,7 @@ void GuiWrapper::commonStereoCallback(
{ {
if(!rtabmap_ros::convertScan3dMsg( if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg, scan3dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -772,9 +776,11 @@ void GuiWrapper::commonLaserScanCallback(
const rtabmap_ros::GlobalDescriptor & globalDescriptor) const rtabmap_ros::GlobalDescriptor & globalDescriptor)
{ {
std_msgs::Header odomHeader; std_msgs::Header odomHeader;
std::string frameId = frameId_;
if(odomMsg.get()) if(odomMsg.get())
{ {
odomHeader = odomMsg->header; odomHeader = odomMsg->header;
frameId = odomMsg->child_frame_id;
} }
else else
{ {
@@ -793,7 +799,7 @@ void GuiWrapper::commonLaserScanCallback(
odomHeader.frame_id = odomFrameId_; odomHeader.frame_id = odomFrameId_;
} }
Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {
@@ -843,7 +849,7 @@ void GuiWrapper::commonLaserScanCallback(
{ {
if(!rtabmap_ros::convertScanMsg( if(!rtabmap_ros::convertScanMsg(
scan2dMsg, scan2dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -858,7 +864,7 @@ void GuiWrapper::commonLaserScanCallback(
{ {
if(!rtabmap_ros::convertScan3dMsg( if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg, scan3dMsg,
frameId_, frameId,
odomSensorSync_?odomHeader.frame_id:"", odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp, odomHeader.stamp,
scan, scan,
@@ -881,11 +887,11 @@ void GuiWrapper::commonLaserScanCallback(
//just get scan local transform to adjust camera frame //just get scan local transform to adjust camera frame
if(!scan2dMsg.ranges.empty()) if(!scan2dMsg.ranges.empty())
{ {
fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); fakeCameraLocalTransform = getTransform(frameId, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
} }
else if(!scan3dMsg.data.empty()) else if(!scan3dMsg.data.empty())
{ {
fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); fakeCameraLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
} }
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
@@ -932,7 +938,7 @@ void GuiWrapper::commonOdomCallback(
std_msgs::Header odomHeader = odomMsg->header; std_msgs::Header odomHeader = odomMsg->header;
Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0); Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, odomMsg->child_frame_id, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get()) if(odomMsg.get())
{ {