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

Accepting global pose even if adjusting it with odometry fails

Browse Source
This commit is contained in:
matlabbe
2017-05-24 14:36:35 -04:00
parent b6b5a1795e
commit 1cbff792fb
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+9 -3
View File
@@ -929,7 +929,7 @@ void CoreWrapper::commonDepthCallbackImpl(
userData = rtabmap_ros::userDataFromROS(*userDataMsg); userData = rtabmap_ros::userDataFromROS(*userDataMsg);
if(!userData_.empty()) if(!userData_.empty())
{ {
ROS_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
userData_ = cv::Mat(); userData_ = cv::Mat();
} }
} }
@@ -974,15 +974,21 @@ void CoreWrapper::commonDepthCallbackImpl(
lastPoseStamp_, lastPoseStamp_,
tfListener_, tfListener_,
waitForTransform_?waitForTransformDuration_:0.0); waitForTransform_?waitForTransformDuration_:0.0);
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
if(!correction.isNull()) if(!correction.isNull())
{ {
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= correction; globalPose *= correction;
}
else
{
NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
"If odometry is small since it received the global pose and "
"covariance is large, this should not be a problem.");
}
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone(); cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance); data.setGlobalPose(globalPose, globalPoseCovariance);
} }
} }
}
globalPose_.header.stamp = ros::Time(0); globalPose_.header.stamp = ros::Time(0);
process(lastPoseStamp_, process(lastPoseStamp_,
src/MsgConversion.cpp
+2 -2
View File
@@ -1043,7 +1043,7 @@ rtabmap::Transform getTransform(
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
ROS_WARN("%s",ex.what()); ROS_WARN("(getting transform %s -> %s) %s", fromFrameId.c_str(), toFrameId.c_str(), ex.what());
} }
return transform; return transform;
} }
@@ -1080,7 +1080,7 @@ rtabmap::Transform getTransform(
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
ROS_WARN("%s",ex.what()); ROS_WARN("(getting transform movement of %s according to fixed %s) %s", sourceTargetFrame.c_str(), fixedFrame.c_str(), ex.what());
} }
return transform; return transform;
} }