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

Fixed #122. Fixed assert "commonDepthCallbackImpl() Condition (uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format())) not met!" (making sure that callbacks are initialized after rosparam)

Browse Source
This commit is contained in:
matlabbe
2016-10-02 16:51:33 -04:00
parent db91736443
commit b2b5df31e0
3 changed files with 90 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+45 -24
View File
@@ -118,8 +118,6 @@ void CoreWrapper::onInit()
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
setupCallbacks(nh, pnh);
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
@@ -201,6 +199,12 @@ void CoreWrapper::onInit()
NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
bool subscribeStereo = false;
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
if(subscribeStereo)
{
NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
}
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -328,7 +332,11 @@ void CoreWrapper::onInit()
// Backward compatibility (MapsManager)
mapsManager_.backwardCompatibilityParameters(parameters_);
if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d()) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
bool subscribeScan2d = false;
bool subscribeScan3d = false;
pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
{
NODELET_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is "
"true. The occupancy grid map will be constructed from "
@@ -363,18 +371,6 @@ void CoreWrapper::onInit()
NODELET_INFO("Create intermediate nodes");
}
}
bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
if(isRGBD)
{
// RGBD SLAM
if(!this->isSubscribedToDepth() && !this->isSubscribedToStereo() && !this->isSubscribedToRGBD())
{
NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
"parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
"to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure "
"detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str());
}
}
if(paused_)
{
@@ -399,10 +395,6 @@ void CoreWrapper::onInit()
}
mapsManager_.setParameters(parameters_);
if(this->isSubscribedToStereo())
{
NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
}
// Init RTAB-Map
rtabmap_.init(parameters_, databasePath_);
@@ -455,8 +447,18 @@ void CoreWrapper::onInit()
Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
}
setupCallbacks(nh, pnh); // do it at the end
if(!this->isDataSubscribed())
{
bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
if(isRGBD)
{
NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
"parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
"to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure "
"detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str());
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle rgb_pnh(pnh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
@@ -854,6 +856,12 @@ void CoreWrapper::commonDepthCallbackImpl(
cv::flip(scan, flipScan, 1);
scan = flipScan;
}
if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
{
// backward compatibility, project 2D scan in /base_link frame
scan = util3d::transformLaserScan(scan, scanLocalTransform);
scanLocalTransform = Transform::getIdentity();
}
}
else if(scan3dMsg.get() != 0)
{
@@ -1040,6 +1048,12 @@ void CoreWrapper::commonStereoCallback(
cv::flip(scan, flipScan, 1);
scan = flipScan;
}
if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
{
// backward compatibility, project 2D scan in /base_link frame
scan = util3d::transformLaserScan(scan, scanLocalTransform);
scanLocalTransform = Transform::getIdentity();
}
}
else if(scan3dMsg.get() != 0)
{
@@ -1120,12 +1134,19 @@ void CoreWrapper::process(
this->publishStats(stamp);
std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
// create a tmp signature with latest sensory data
// create a tmp signature with latest sensory data if latest signature was ignored
std::map<int, rtabmap::Signature> tmpSignature;
SensorData tmpData = data;
tmpData.setId(-1);
tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom));
if(rtabmap_.getMemory() == 0 ||
filteredPoses.size() == 0 ||
rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0)
{
SensorData tmpData = data;
tmpData.setId(-1);
tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom));
}
// Update maps
filteredPoses = mapsManager_.updateMapCaches(
src/GuiWrapper.cpp
+1 -2
View File
@@ -76,8 +76,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
setupCallbacks(nh, pnh);
QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
for(int i=1; i<argc; ++i)
{
@@ -173,6 +171,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this);
setupCallbacks(nh, pnh); // do it at the end
if(!this->isDataSubscribed())
{
defaultSub_ = nh.subscribe("odom", queueSize_, &GuiWrapper::defaultCallback, this);
src/MapsManager.cpp
+44 -16
View File
@@ -431,6 +431,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
#endif
}
bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false;
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{
if(!iter->second.isNull())
@@ -446,32 +448,58 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
else if(memory)
{
data = memory->getSignatureDataConst(iter->first, false, false, false, true);
data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
}
if(data.id() != 0)
{
cv::Mat ground, obstacles;
data.uncompressData(
0,
0,
0,
0,
&ground,
&obstacles);
UDEBUG("Adding grid map %d to cache...", iter->first);
cv::Point3f viewPoint;
if(iter->first > 0 || data.gridCellSize())
cv::Mat ground, obstacles;
if(iter->first > 0)
{
viewPoint = data.gridViewPoint();
gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint));
cv::Mat rgb, depth, scan;
bool generateGrid = data.gridCellSize() == 0.0f;
static bool warningShown = false;
if(occupancySavedInDB && generateGrid && !warningShown)
{
warningShown = true;
UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
"any occupancy grid output) but it cannot be found "
"in memory. For convenience, the occupancy "
"grid is regenerated. Make sure parameter \"%s\" is true to "
"avoid this warning for the next locations added to map. For older "
"locations already in database without an occupancy grid map, you can use the "
"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
"save them back in the database for next sessions. This warning is only shown once.",
data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
}
data.uncompressData(
occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
0,
generateGrid?0:&ground,
generateGrid?0:&obstacles);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
else
{
viewPoint = data.gridViewPoint();
gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint));
}
}
else
{
// generate tmp occupancy grid for negative ids
// generate tmp occupancy grid for negative ids (assuming data is already uncompressed)
// we need the signature
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
if(findIter != signatures.end())