diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 1b8729f3..f0db42d5 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -49,12 +50,118 @@ class MapAssembler { public: - MapAssembler() + MapAssembler(int & argc, char** argv) { ros::NodeHandle pnh("~"); ros::NodeHandle nh; + std::string configPath; + pnh.param("config_path", configPath, configPath); + + //parameters + rtabmap::ParametersMap parameters; + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid")); + uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM")); + if(!configPath.empty()) + { + if(UFile::exists(configPath.c_str())) + { + ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str()); + rtabmap::ParametersMap allParameters; + Parameters::readINI(configPath.c_str(), allParameters); + // only update odometry parameters + for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + ParametersMap::iterator jter = allParameters.find(iter->first); + if(jter!=allParameters.end()) + { + iter->second = jter->second; + } + } + } + else + { + ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str()); + } + } + for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + std::string vStr; + bool vBool; + int vInt; + double vDouble; + if(pnh.getParam(iter->first, vStr)) + { + ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str()); + iter->second = vStr; + } + else if(pnh.getParam(iter->first, vBool)) + { + ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str()); + iter->second = uBool2Str(vBool); + } + else if(pnh.getParam(iter->first, vDouble)) + { + ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str()); + iter->second = uNumber2Str(vDouble); + } + else if(pnh.getParam(iter->first, vInt)) + { + ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str()); + iter->second = uNumber2Str(vInt); + } + + if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) + { + ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8..."); + iter->second = uNumber2Str(8); + } + } + + rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv); + for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter) + { + rtabmap::ParametersMap::iterator jter = parameters.find(iter->first); + if(jter!=parameters.end()) + { + ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str()); + jter->second = iter->second; + } + } + + // Backward compatibility + for(std::map >::const_iterator iter=Parameters::getRemovedParameters().begin(); + iter!=Parameters::getRemovedParameters().end(); + ++iter) + { + std::string vStr; + if(pnh.getParam(iter->first, vStr)) + { + if(iter->second.first && parameters.find(iter->second.second) != parameters.end()) + { + // can be migrated + parameters.at(iter->second.second)= vStr; + ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", + ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); + } + else + { + if(iter->second.second.empty()) + { + ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!", + ros::this_node::getName().c_str(), iter->first.c_str()); + } + else + { + ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str()); + } + } + } + } + mapsManager_.init(nh, pnh, ros::this_node::getName(), false); + mapsManager_.setParameters(parameters); mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); @@ -127,7 +234,41 @@ private: int main(int argc, char** argv) { ros::init(argc, argv, "map_assembler"); - MapAssembler assembler; + + // process "--params" argument + for(int i=1;ifirst + " = \"" + iter->second + "\""; + std::cout << + str << + std::setw(60 - str.size()) << + " [" << + rtabmap::Parameters::getDescription(iter->first).c_str() << + "]" << + std::endl; + } + ROS_WARN("Node will now exit after showing default parameters because " + "argument \"--params\" is detected!"); + exit(0); + } + else if(strcmp(argv[i], "--udebug") == 0) + { + ULogger::setLevel(ULogger::kDebug); + } + else if(strcmp(argv[i], "--uinfo") == 0) + { + ULogger::setLevel(ULogger::kInfo); + } + } + + MapAssembler assembler(argc, argv); ros::spin(); return 0; } diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 3c5bd1ed..e8052cc0 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -505,37 +505,48 @@ std::map MapsManager::updateMapCaches( else { // generate tmp occupancy grid for negative ids (assuming data is already uncompressed) - // we need the signature - std::map::const_iterator findIter = signatures.find(iter->first); - if(findIter != signatures.end()) + // For negative laser scans, fill empty space? + bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled(); + Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled); + + if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_) { - // For negative laser scans, fill empty space? - bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled(); - Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled); + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(negativeScanEmptyRayTracing_))); + occupancyGrid_->parseParameters(parameters); + } - if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_) - { - ParametersMap parameters; - parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(negativeScanEmptyRayTracing_))); - occupancyGrid_->parseParameters(parameters); - } + cv::Mat rgb, depth, scan; + bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_); + data.uncompressData( + occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, + occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, + !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + 0, + generateGrid?0:&ground, + generateGrid?0:&obstacles); - // normally data should be already uncompressed for negative ids - occupancyGrid_->createLocalMap(findIter->second, ground, obstacles, viewPoint); + 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)); - - // put back - if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_) - { - ParametersMap parameters; - parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled))); - occupancyGrid_->parseParameters(parameters); - } } else { - ROS_WARN("%d signature not found in cache?!?!?", iter->first); + viewPoint = data.gridViewPoint(); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); + } + + // put back + if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_) + { + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled))); + occupancyGrid_->parseParameters(parameters); } } if(ground.cols || obstacles.cols) @@ -543,7 +554,7 @@ std::map MapsManager::updateMapCaches( occupancyGrid_->addToCache(iter->first, ground, obstacles); } } - else + else if(memory) { ROS_ERROR("Data missing for node %d to update the maps", iter->first); } diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 169dc991..29bc7b09 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -258,7 +258,7 @@ void OdometryROS::onInit() std::string vStr; if(pnh.getParam(iter->first, vStr)) { - if(iter->second.first) + if(iter->second.first && parameters_.find(iter->second.second) != parameters_.end()) { // can be migrated parameters_.at(iter->second.second)= vStr;