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

CoreWrapper: 2d optimized map should not be loaded in SLAM mode (see also https://github.com/introlab/rtabmap/commit/f6e17be2b4196d588305fe20e080406a4841983a).

Browse Source
This commit is contained in:
matlabbe
2021-03-12 17:50:40 -05:00
parent 0119f3e1fa
commit e9e140af1f
3 changed files with 35 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+29 -10
View File
@@ -623,20 +623,39 @@ void CoreWrapper::onInit()
// Init RTAB-Map
rtabmap_.init(parameters_, databasePath_);
if(rtabmap_.getMemory() && useSavedMap_)
if(rtabmap_.getMemory())
{
float xMin, yMin, gridCellSize;
cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
if(!map.empty())
if(useSavedMap_ && !rtabmap_.getMemory()->isIncremental())
{
NODELET_INFO("rtabmap: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
float xMin, yMin, gridCellSize;
cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
if(!map.empty())
{
NODELET_INFO("rtabmap: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
}
}
}
if(databasePath_.size() && rtabmap_.getMemory())
{
NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
if(rtabmap_.getMemory()->getWorkingMem().size()>1)
{
NODELET_INFO("rtabmap: Working Memory = %d, Local map = %d.",
(int)rtabmap_.getMemory()->getWorkingMem().size()-1,
(int)rtabmap_.getLocalOptimizedPoses().size());
}
if(databasePath_.size())
{
NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
}
if(rtabmap_.getMemory()->isIncremental())
{
NODELET_INFO("rtabmap: SLAM mode (%s=true)", Parameters::kMemIncrementalMemory().c_str());
}
else
{
NODELET_INFO("rtabmap: Localization mode (%s=false)", Parameters::kMemIncrementalMemory().c_str());
}
}
// setup services
src/MapAssemblerNode.cpp
+1
View File
@@ -162,6 +162,7 @@ public:
}
}
ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
mapsManager_.backwardCompatibilityParameters(pnh, parameters);
mapsManager_.setParameters(parameters);
src/nodelets/point_cloud_aggregator.cpp
+5 -4
View File
@@ -70,7 +70,8 @@ public:
exactSync3_(0),
approxSync3_(0),
exactSync2_(0),
approxSync2_(0)
approxSync2_(0),
waitForTransformDuration_(0.1)
{}
virtual ~PointCloudAggregator()
@@ -104,6 +105,7 @@ private:
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("approx_sync", approx, approx);
pnh.param("count", count, count);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
cloudSub_1_.subscribe(nh, "cloud1", 1);
cloudSub_2_.subscribe(nh, "cloud2", 1);
@@ -233,7 +235,6 @@ private:
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
{
rtabmap::Transform cloudDisplacement;
bool notsync = false;
if(!fixedFrameId_.empty() &&
cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
{
@@ -244,8 +245,7 @@ private:
cloudMsgs[i]->header.stamp, //stampSource
cloudMsgs[0]->header.stamp, //stampTarget
tfListener_,
0.1);
notsync = true;
waitForTransformDuration_);
}
pcl::PCLPointCloud2 cloud2;
@@ -335,6 +335,7 @@ private:
std::string frameId_;
std::string fixedFrameId_;
double waitForTransformDuration_;
tf::TransformListener tfListener_;
};