diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 08c10c91..6b0c5a1e 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -196,6 +196,7 @@ void CoreWrapper::onInit()
 				groundTruthBaseFrameId_.c_str());
 	}
 	NODELET_INFO("rtabmap: map_frame_id  = %s", mapFrameId_.c_str());
+	NODELET_INFO("rtabmap: use_action_for_goal  = %s", useActionForGoal_?"true":"false");
 	NODELET_INFO("rtabmap: tf_delay      = %f", tfDelay);
 	NODELET_INFO("rtabmap: tf_tolerance  = %f", tfTolerance);
 	NODELET_INFO("rtabmap: odom_sensor_sync   = %s", odomSensorSync_?"true":"false");
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index b4632da8..d2979dc0 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -635,7 +635,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 					std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
 					if(mter != gridMaps_.end())
 					{
-						if(!mter->second.first.first.empty() || !mter->second.first.second.empty())
+						if(!mter->second.first.first.empty() || !mter->second.first.second.empty() || !mter->second.second.empty())
 						{
 							occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second);
 						}
@@ -653,11 +653,12 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 					if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end())
 					{
 						if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) &&
-						   (mter->second.first.second.empty() || mter->second.first.second.channels() > 2))
+						   (mter->second.first.second.empty() || mter->second.first.second.channels() > 2) &&
+						   (mter->second.second.empty() || mter->second.second.channels() > 2))
 						{
 							octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second);
 						}
-						else if(!mter->second.first.first.empty() && !mter->second.first.second.empty())
+						else if(!mter->second.first.first.empty() && !mter->second.first.second.empty() && !mter->second.second.empty())
 						{
 							ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. "
 									"Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "