From b2b5df31e0e2e0e5ab8b7f3180256bcc337adb16 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Oct 2016 16:51:33 -0400 Subject: [PATCH] Fixed #122. Fixed assert "commonDepthCallbackImpl() Condition (uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format())) not met!" (making sure that callbacks are initialized after rosparam) --- src/CoreWrapper.cpp | 69 +++++++++++++++++++++++++++++---------------- src/GuiWrapper.cpp | 3 +- src/MapsManager.cpp | 60 ++++++++++++++++++++++++++++----------- 3 files changed, 90 insertions(+), 42 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 56eb0ecf..cca821c4 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -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("info", 1); mapDataPub_ = nh.advertise("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 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 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( diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index 56ab8734..96931483 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -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; iregisterCallback(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); diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 35365ea4..fb5a65a1 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -431,6 +431,8 @@ std::map MapsManager::updateMapCaches( #endif } + bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; + for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) @@ -446,32 +448,58 @@ std::map 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::const_iterator findIter = signatures.find(iter->first); if(findIter != signatures.end())