Basic connection now working between the controller/explore and 3d fr…

…ontier search, but has placeholders for full frontier list, and logic needs verify

CMakeLists.txt

@@ -24,7 +24,9 @@ find_package(catkin REQUIRED COMPONENTS
   octomap_ros
   nodelet
   mean_shift_clustering
+  messages_88
 )
+
 find_package(Boost)
 find_package(PCL 1.8 REQUIRED COMPONENTS
     common

include/uav_frontier_exploration_3d/FrontierServer.h

@@ -8,6 +8,8 @@
 #include <std_srvs/SetBool.h>
 #include <std_msgs/Int32.h>
 
+#include "messages_88/GetFrontiers.h"
+
 namespace frontier_server
 {
   enum ExplorationState {
@@ -44,6 +46,7 @@ namespace frontier_server
 
     protected:
       KeySet findFrontier(PCLPointCloudI& changedCells);
+      bool searchFrom(messages_88::GetFrontiers::Request& req, messages_88::GetFrontiers::Response& resp);
       void searchForParentsAndPublish();
       void updateGlobalFrontier(KeySet& globalFrontierCell);
       void clusterFrontierAndPublish();
@@ -111,6 +114,9 @@ namespace frontier_server
       geometry_msgs::PoseStamped m_uavCurrentReference;
       ros::ServiceServer m_serviceExploration;  
       ExplorationState m_currentState = ExplorationState::OFF;
+
+      // service
+      ros::ServiceServer search_server_;
     };
 }
 #endif

package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>uav_frontier_exploration_3d</name>
   <version>1.0.0</version>
   <description>A ROS package that implements a 3d exploration algorithm.</description>
@@ -11,45 +11,32 @@
 
 
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>geographic_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>tf</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>octomap</build_depend>
-  <build_depend>octomap_ros</build_depend>
-  <build_depend>octomap_msgs</build_depend> 
-  <build_depend>pcl_ros</build_depend>
-  <build_depend>pcl_conversions</build_depend>
-  <build_depend>nodelet</build_depend>
+
+  <depend>messages_88</depend>
+
+  <depend>gazebo</depend>
+  <depend>gazebo_ros</depend>
+  <depend>geometry_msgs</depend>
+  <depend>geographic_msgs</depend>
+  <depend>mean_shift_clustering</depend>
+  <depend>nav_msgs</depend>
+  <depend>nodelet</depend>
+  <depend>octomap</depend>
+  <depend>octomap_ros</depend>
+  <depend>octomap_msgs</depend> 
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
+  <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
+
+
   <build_depend>libpcl-all-dev</build_depend>
-  <build_depend>mean_shift_clustering</build_depend>
-  <build_depend>gazebo</build_depend>
-  <build_depend>gazebo_ros</build_depend>
-
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>geographic_msgs</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>std_msgs</run_depend>
-  <run_depend>tf</run_depend>
-  <run_depend>visualization_msgs</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>octomap</run_depend>
-  <run_depend>octomap_ros</run_depend>
-  <run_depend>octomap_msgs</run_depend>
-  <run_depend>pcl_ros</run_depend>
-  <run_depend>pcl_conversions</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>libpcl-all</run_depend>
-  <run_depend>mean_shift_clustering</run_depend>
-  <run_depend>gazebo</run_depend>
-  <run_depend>gazebo_ros</run_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/FrontierServer.cpp

@@ -41,6 +41,8 @@ namespace frontier_server
 		// Initialize position hold service if exploration is off
 		m_serviceExploration = m_nh.advertiseService("exploration/toggle",
 			&FrontierServer::toggleExplorationServiceCb, this);
+
+		search_server_ = m_nh.advertiseService("get_frontiers", &FrontierServer::searchFrom, this);
 	}
 
 	FrontierServer::~FrontierServer()
@@ -52,6 +54,86 @@ namespace frontier_server
 		}	
 	}
 
+	bool FrontierServer::searchFrom(messages_88::GetFrontiers::Request& req, messages_88::GetFrontiers::Response& resp) {
+		// TODO clean up and verify the logic flow, revise so returns all frontiers with scores
+		// Also it switches goals too fast, ends up making no progress, debug
+	// case ExplorationState::CHECKFORFRONTIERS:
+		m_octomapServer.runDefault();
+		m_octomapServer.publishVolume();
+		m_uavCurrentPose = m_octomapServer.getCurrentUAVPosition();
+		// if(!m_currentGoalReached)
+			ROS_WARN_STREAM_THROTTLE(3.0,
+			m_bestFrontierPoint.x() << " " << m_bestFrontierPoint.y() << " " 
+			<< m_bestFrontierPoint.z() << " -> Goal published!");
+		// Update octomap
+		m_octree = m_octomapServer.getOcTree();	
+		// Get changed cells
+		m_changedCells = m_octomapServer.getChangedCells();
+		// if (m_changedCells.size() > 0)
+			// setStateAndPublish(ExplorationState::ON);
+
+	// case ExplorationState::ON:
+		KeySet globalFrontierCells = findFrontier(m_changedCells);
+		// Delete frontiers that are explored now
+		updateGlobalFrontier(globalFrontierCells);	
+		// Find frontiers on the upper level (m_explorationDepth) and publish it
+		searchForParentsAndPublish();
+		// If there is no parents switch to CHECKFORFRONTIERS
+		// if (!m_parentFrontierCells.size() > 0) 
+		// 	setStateAndPublish(ExplorationState::CHECKFORFRONTIERS);
+		// // If the previous goal is reached
+		// // else if (m_currentGoalReached)
+		// // 	setStateAndPublish(ExplorationState::POINTREACHED);
+		// else 
+			// setStateAndPublish(ExplorationState::CHECKFORFRONTIERS);
+
+	// case ExplorationState::POINTREACHED:
+		// Find Best Frontier
+		m_currentGoalReached = false;
+		// Delete candidates that are too close to prevoius assigned points
+
+		clusterFrontierAndPublish();
+		point3d currentPoint3d(m_uavCurrentPose.position.x, 
+			m_uavCurrentPose.position.y, m_uavCurrentPose.position.z);
+		// TODO make it return all frontiers and their scores
+		m_bestFrontierPoint = 
+				m_bestFrontierServer.bestFrontierInfGain(m_octree, currentPoint3d, m_clusteredCellsUpdated);
+		// m_bestFrontierPoint = 
+		// 	m_bestFrontierServer.closestFrontier(m_octree, currentPoint3d, m_clusteredCellsUpdated);
+		m_logfile << "Best frontier: " << m_bestFrontierPoint << endl;
+
+		m_allUAVGoals.push_back(m_bestFrontierPoint);
+		cout << "Best frontier: " << m_bestFrontierPoint << endl;
+		publishBestFrontier();
+		publishUAVGoal(m_bestFrontierPoint);
+		// ros::Duration(0.05).sleep();
+		// setStateAndPublish(ExplorationState::CHECKFORFRONTIERS);
+		messages_88::Frontier front;
+		// Just fill in placeholder details for now
+		geometry_msgs::Point point;
+		point.x = m_bestFrontierPoint.x();
+		point.y = m_bestFrontierPoint.y();
+		point.z = m_bestFrontierPoint.z();
+		front.centroid = point;
+		front.initial = point;
+		front.middle = point;
+		std::vector<geometry_msgs::Point> points;
+		points.push_back(point);
+		front.points = points;
+		front.size = 1;
+		front.min_distance = 10.0;
+		front.cost = 1.0;
+		std::vector<double> scores;
+		scores.push_back(1.0);
+		scores.push_back(1.0);
+		scores.push_back(1.0);
+		front.utility_scores = scores;
+		std::vector<messages_88::Frontier> frontiers;
+		frontiers.push_back(front);
+		resp.frontiers.frontier_list = frontiers;
+		return true;
+	}
+
 	bool FrontierServer::configureFromFile(string config_filename)
 	{
 		cout << "FrontierServer - Configuring uav exploration from file: " << endl;