i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
+10 -4
View File
@@ -929,7 +929,7 @@ void CoreWrapper::commonDepthCallbackImpl(
userData = rtabmap_ros::userDataFromROS(*userDataMsg);
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();
}
}
@@ -974,13 +974,19 @@ void CoreWrapper::commonDepthCallbackImpl(
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
if(!correction.isNull())
{
Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
globalPose *= correction;
cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
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();
data.setGlobalPose(globalPose, globalPoseCovariance);
}
}
globalPose_.header.stamp = ros::Time(0);
src/MsgConversion.cpp
+2 -2
View File
@@ -1043,7 +1043,7 @@ rtabmap::Transform getTransform(
}
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;
}
@@ -1080,7 +1080,7 @@ rtabmap::Transform getTransform(
}
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;
}