[src/include] Change findFrontier and updateGlobalFrontier

robotics-88 · May 12, 2021 · 0f74981 · 0f74981
1 parent bf944e3
commit 0f74981
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 14 deletions.
diff --git a/include/uav_frontier_exploration_3d/FrontierServer.h b/include/uav_frontier_exploration_3d/FrontierServer.h
@@ -44,9 +44,9 @@ namespace frontier_server
       void run();
 
     protected:
-      void findFrontier(PCLPointCloudI& changedCells);
+      KeySet findFrontier(PCLPointCloudI& changedCells);
       void searchForParentsAndPublish();
-      void updateGlobalFrontier();
+      void updateGlobalFrontier(KeySet& globalFrontierCell);
       void clusterFrontierAndPublish();
       void pointReachedCallback(std_msgs::Bool msg);
       void currentReferenceCallback(geometry_msgs::PoseStamped msg);
@@ -97,7 +97,7 @@ namespace frontier_server
       string m_configFilename;
 
       // The newest frontier cells
-      KeySet m_globalFrontierCells, m_globalFrontierCellsUpdated, 
+      KeySet m_globalFrontierCellsUpdated, 
         m_clusteredCells, m_clusteredCellsUpdated, m_parentFrontierCells,
         m_invalidParentCells;
 

diff --git a/src/FrontierServer.cpp b/src/FrontierServer.cpp
@@ -83,9 +83,10 @@ namespace frontier_server
 		return true;
 	}
 
-	void FrontierServer::findFrontier(PCLPointCloudI& changedCells)
+	KeySet FrontierServer::findFrontier(PCLPointCloudI& changedCells)
 	{
 		ros::WallTime startTime = ros::WallTime::now();
+		KeySet globalFrontierCells;
 		// m_logfile << "findFrontier" << endl;
 		bool unknownCellFlag {false};
 		bool freeCellFlag {false};
@@ -112,7 +113,7 @@ namespace frontier_server
 			{
 				OCTOMAP_ERROR_STR("Error in search: [" 
 					<< changedCellPoint << "] is out of OcTree bounds!");
-				return;
+				return globalFrontierCells;
 			}
 
 			// Check point state: free/occupied
@@ -134,24 +135,24 @@ namespace frontier_server
 						freeCellFlag = true;
 				}
 				if(unknownCellFlag && freeCellFlag)
-					m_globalFrontierCells.insert(changedCellKey);
+					globalFrontierCells.insert(changedCellKey);
 					frontierSize++;
 			}
 		}
 
-		// cout << "FrontierServer - frontiers: " << frontierSize << endl;
 		m_logfile << "Number of frontiers:" << frontierSize << endl;
 		double total_time = (ros::WallTime::now() - startTime).toSec();
 		m_logfile << "findFrontier - used total: "<< total_time << " sec" <<endl;
+		return globalFrontierCells;
 	}
 
-	void FrontierServer::updateGlobalFrontier()
+	void FrontierServer::updateGlobalFrontier(KeySet& globalFrontierCells)
 	{
 		// m_logfile << "updateGlobalFrontier" << endl;
 		ros::WallTime startTime = ros::WallTime::now();
 		int frontierSize {0};
 		m_globalFrontierCellsUpdated.clear();
-		for(KeySet::iterator iter = m_globalFrontierCells.begin(), end = m_globalFrontierCells.end();
+		for(KeySet::iterator iter = globalFrontierCells.begin(), end = globalFrontierCells.end();
 			iter!= end; ++iter)
 		{
 			frontierSize++;
@@ -164,7 +165,7 @@ namespace frontier_server
 		// Find in globalFrontier cells that are not frontier anymore --vol2
 		// Delete them from globalFrontierCells
 		std::vector<octomap::point3d> changedCellNeighbor;
-		for(KeySet::iterator cell_iter = m_globalFrontierCells.begin(), end = m_globalFrontierCells.end();
+		for(KeySet::iterator cell_iter = globalFrontierCells.begin(), end = globalFrontierCells.end();
 			cell_iter!= end; )
 		{
 			// If current cell if free, check its neighbors
@@ -187,7 +188,7 @@ namespace frontier_server
 			}
 			if(!unknownCellFlag || occupiedCellFlag)
 			{
-				// m_globalFrontierCells.erase(*cell_iter);
+				// globalFrontierCells.erase(*cell_iter);
 				deleted++;	
 			}
 			else m_globalFrontierCellsUpdated.insert(*cell_iter);
@@ -409,7 +410,8 @@ namespace frontier_server
 		{
 			ros::WallTime startTime = ros::WallTime::now();
 			ros::spinOnce();
-
+			// Initialize before switch
+			KeySet globalFrontierCells;
 			switch (m_currentState)
 			{
 				case ExplorationState::OFF:
@@ -434,9 +436,9 @@ namespace frontier_server
 					break;
 
 				case ExplorationState::ON:
-					findFrontier(m_changedCells);
+					globalFrontierCells = findFrontier(m_changedCells);
 					// Delete frontiers that are explored now
-					updateGlobalFrontier();	
+					updateGlobalFrontier(globalFrontierCells);	
 					// Find frontiers on the upper level (m_explorationDepth) and publish it
 					searchForParentsAndPublish();
 					// If there is no parents switch to CHECKFORFRONTIERS