Merge pull request #23 from SoftServeSAG/F#2_intoduce_hidden_frontier…

…s_priority

F#2 intoduce hidden frontiers priority

explore_lite_revised/CMakeLists.txt

@@ -51,8 +51,7 @@ include_directories(
 add_executable(explore_lite_revised
   src/costmap_client.cpp
   src/explore.cpp
-  src/frontier_search.cpp
-)
+  src/frontier_search.cpp)
 add_dependencies(explore_lite_revised ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(explore_lite_revised ${catkin_LIBRARIES})
 

explore_lite_revised/include/explore/explore.h

@@ -53,6 +53,8 @@
 
 namespace explore
 {
+
+
 /**
  * @class Explore
  * @brief A class adhering to the robot_actions::Action interface that moves the

explore_lite_revised/include/explore/explore_utils.h

@@ -0,0 +1,18 @@
+//
+// Created by user on 3/19/19.
+//
+
+#ifndef PROJECT_EXPLORE_UTILS_H
+#define PROJECT_EXPLORE_UTILS_H
+
+#include <costmap_2d/costmap_2d.h>
+
+namespace frontier_exploration{
+    static geometry_msgs::Point makePointMsg(const double x, const double y, const double z){
+        geometry_msgs::Point p;
+        p.x = x; p.y = y; p.z = z;
+        return p;
+    }
+}
+
+#endif //PROJECT_EXPLORE_UTILS_H

explore_lite_revised/include/explore/frontier_search.h

@@ -5,14 +5,14 @@
 
 namespace frontier_exploration
 {
-/**
+
+    /**
  * @brief Represents a frontier
  *
  */
 
 struct Frontier {
-    //todo make a class of it, make methods static methods of class
-//    std::uint32_t size{0};
+    //todo improve this struct functionality
   double min_distance{std::numeric_limits<double>::infinity()};
   double cost{0.0};
   geometry_msgs::Point initial;
@@ -25,6 +25,7 @@ struct Frontier {
     geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt);
     geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame);
     bool hidden{false};
+    Frontier() = default;
 };
 
 /**

explore_lite_revised/src/frontier_search.cpp

@@ -1,17 +1,19 @@
-#include <explore/frontier_search.h>
 
 #include <mutex>
 
 #include <costmap_2d/cost_values.h>
 #include <costmap_2d/costmap_2d.h>
 #include <geometry_msgs/Point.h>
 
-#include <explore/costmap_tools.h>
-
 #include <boost/geometry.hpp>
 #include <geometry_msgs/Vector3.h>
 #include <cmath>
 
+#include <explore/costmap_tools.h>
+#include <explore/frontier_search.h>
+#include <explore/explore_utils.h>
+
+
 namespace frontier_exploration
 {
 using costmap_2d::LETHAL_OBSTACLE;
@@ -34,21 +36,20 @@ inline double angular_vector_distance(const geometry_msgs::Point &p1,const  geom
 inline geometry_msgs::Point getClosestPointTo(const std::vector<geometry_msgs::Point> &distance_vectors, const geometry_msgs::Point &goal){
     auto min_elem_iter = std::min_element(distance_vectors.begin(), distance_vectors.end(),
             [](const geometry_msgs::Point &p1,const geometry_msgs::Point &p2)
-    {return p1.x + p1.y < p2.x + p2.y;}); //using Manhatten distance
+            {return p1.x + p1.y < p2.x + p2.y;}); //using Manhatten distance
     return *min_elem_iter;
 }
 
-geometry_msgs::Point Frontier::toReferenceFrame(const geometry_msgs::Point &pt_in_absolute_frame){
-    geometry_msgs::Point pt_in_reference_frame;
-    pt_in_reference_frame.x = pt_in_absolute_frame.x - reference_robot_pose.x;
-    pt_in_reference_frame.y = pt_in_absolute_frame.y - reference_robot_pose.y;
-    return  pt_in_reference_frame;
+inline geometry_msgs::Point Frontier::toReferenceFrame(const geometry_msgs::Point &pt_in_absolute_frame){
+    return makePointMsg(
+            pt_in_absolute_frame.x - reference_robot_pose.x,
+            pt_in_absolute_frame.y - reference_robot_pose.y,
+            0);
 }
-geometry_msgs::Point  Frontier::fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame){
-    geometry_msgs::Point pt_in_absolute_frame;
-    pt_in_absolute_frame.x = pt_in_reference_frame.x + reference_robot_pose.x;
-    pt_in_absolute_frame.y = pt_in_reference_frame.y + reference_robot_pose.y;
-    return  pt_in_absolute_frame;
+inline geometry_msgs::Point  Frontier::fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame){
+    return makePointMsg(pt_in_reference_frame.x + reference_robot_pose.x,
+                        pt_in_reference_frame.y + reference_robot_pose.y,
+                        0);
 }
 
 FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
@@ -105,13 +106,15 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
 
     std::vector<Frontier> FrontierSearch::splitFrontier(Frontier& fr){
         Frontier fr1, fr2;
-
         std::vector<Frontier> frontiers{fr1, fr2}, vector_buf, res;
         size_t splits = frontiers.size();
         size_t current_element {0}, split_size {fr.vectors_to_points.size() / splits};
         for (auto& this_fr: frontiers){
             this_fr.reference_robot_pose = fr.reference_robot_pose;
-            this_fr.vectors_to_points = std::vector<geometry_msgs::Point>(fr.vectors_to_points.begin() + current_element, fr.vectors_to_points.begin() + current_element + split_size);
+            this_fr.vectors_to_points = std::vector<geometry_msgs::Point>(
+                    fr.vectors_to_points.begin() + current_element,
+                    fr.vectors_to_points.begin() + current_element + split_size);
+
             current_element += split_size;
             this_fr.interpolated_line = {this_fr.vectors_to_points.front(), this_fr.vectors_to_points.back()};
             this_fr.centroid = *(this_fr.vectors_to_points.begin() + split_size / 2);  // not true centroid, but who cares... //todo maybe
@@ -326,6 +329,9 @@ bool FrontierSearch::is_hidden(frontier_exploration::Frontier &fr, double thresh
 
 double FrontierSearch::frontierCost(const Frontier& frontier)
 {
+    // TODO add rotation angle heuristics
+    // TODO geodesial distance heuristics
+    // todo add wall reachability for successfull mapping
   return (potential_scale_ * frontier.min_distance
   // * costmap_->getResolution()
           )