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

rtabmap: Added global_pose input topic (can be used to add GPS/global localization values for graph optimization using g2o)

Browse Source
This commit is contained in:
matlabbe
2017-05-23 17:47:56 -04:00
parent f81edcf790
commit b6b5a1795e
2 changed files with 50 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+45
View File
@@ -491,6 +491,7 @@ void CoreWrapper::onInit()
}
userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
}
CoreWrapper::~CoreWrapper()
@@ -937,6 +938,9 @@ void CoreWrapper::commonDepthCallbackImpl(
userData = userData_;
userData_ = cv::Mat();
}
SensorData data(scan,
LaserScanInfo(
scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
@@ -950,6 +954,37 @@ void CoreWrapper::commonDepthCallbackImpl(
userData);
data.setGroundTruth(groundTruthPose);
//global pose
if(!globalPose_.header.stamp.isZero())
{
// assume sensor is fixed
Transform baseToSensor = rtabmap_ros::getTransform(
frameId_,
globalPose_.header.frame_id,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
if(!baseToSensor.isNull())
{
// Correction of the global pose accounting the odometry movement since we received it
Transform correction = rtabmap_ros::getTransform(
frameId_,
odomFrameId,
globalPose_.header.stamp,
lastPoseStamp_,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0);
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);
}
}
}
globalPose_.header.stamp = ros::Time(0);
process(lastPoseStamp_,
data,
lastPose_,
@@ -1338,6 +1373,14 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
}
}
void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg)
{
if(!paused_)
{
globalPose_ = *globalPoseMsg;
}
}
void CoreWrapper::goalCommonCallback(
int id,
const std::string & label,
@@ -1552,6 +1595,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
mapsManager_.clear();
previousStamp_ = ros::Time(0);
userData_ = cv::Mat();
globalPose_.header.stamp = ros::Time(0);
return true;
}
@@ -1605,6 +1649,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
currentMetricGoal_.setNull();
latestNodeWasReached_ = false;
userData_ = cv::Mat();
globalPose_.header.stamp = ros::Time(0);
NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
UFile::copy(databasePath_, databasePath_+".back");