Merge pull request #19 from SoftServeSAG/F#1_adding_frontier_segmenta…

…tion

F#1 adding frontier segmentation

explore_lite_revised/include/explore/explore.h

@@ -90,6 +90,7 @@ class Explore
   ros::Publisher marker_array_publisher_;
   tf::TransformListener tf_listener_;
 
+
   Costmap2DClient costmap_client_;
   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>
       move_base_client_;

explore_lite_revised/include/explore/frontier_search.h

@@ -9,14 +9,21 @@ namespace frontier_exploration
  * @brief Represents a frontier
  *
  */
+
 struct Frontier {
-  std::uint32_t size;
-  double min_distance;
-  double cost;
+    //todo make a class of it, make methods static methods of class
+//    std::uint32_t size{0};
+  double min_distance{std::numeric_limits<double>::infinity()};
+  double cost{0.0};
   geometry_msgs::Point initial;
   geometry_msgs::Point centroid;
-  geometry_msgs::Point middle;
-  std::vector<geometry_msgs::Point> points;
+//  geometry_msgs::Point closest_point; // for now we ommit it as recomputing is qwestionable
+  std::pair<geometry_msgs::Point,geometry_msgs::Point> interpolated_line;
+//  std::vector<geometry_msgs::Point> points;
+  std::vector<geometry_msgs::Point> vectors_to_points;
+  geometry_msgs::Point reference_robot_pose;
+    geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt);
+    geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame);
 };
 
 /**
@@ -26,16 +33,15 @@ struct Frontier {
 class FrontierSearch
 {
 public:
-  FrontierSearch()
-  {
-  }
+  FrontierSearch() = default;
 
   /**
    * @brief Constructor for search task
    * @param costmap Reference to costmap data to search.
    */
   FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale,
-                 double gain_scale, double min_frontier_size);
+                 double gain_scale, double min_frontier_size,
+                 int use_every_k_point,double max_frontier_angular_size);
 
   /**
    * @brief Runs search implementation, outward from the start position
@@ -45,6 +51,12 @@ class FrontierSearch
   std::vector<Frontier> searchFrom(geometry_msgs::Point position);
 
 protected:
+    std::pair<geometry_msgs::Point, geometry_msgs::Point> approxFrontierByPlanarFarthest(Frontier &fr,
+                                                                                         geometry_msgs::Point &reference_robot);
+    std::pair<geometry_msgs::Point, geometry_msgs::Point> approximateFrontierByViewAngle(Frontier &fr);
+    std::vector<Frontier> splitFrontier(Frontier& fr);
+
+
   /**
    * @brief Starting from an initial cell, build a frontier from valid adjacent
    * cells
@@ -54,8 +66,8 @@ class FrontierSearch
    * as frontiers
    * @return new frontier
    */
-  Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference,
-                            std::vector<bool>& frontier_flag);
+  std::vector<Frontier> buildNewFrontier(unsigned int initial_cell, unsigned int reference,
+                                         std::vector<bool> &frontier_flag);
 
   /**
    * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate
@@ -83,6 +95,8 @@ class FrontierSearch
   unsigned int size_x_, size_y_;
   double potential_scale_, gain_scale_;
   double min_frontier_size_;
+  int use_every_k_point_{1};
+  double max_frontier_angular_size_{10.0}; // TODO make a parameter on parameter server
 };
 }
 #endif

explore_lite_revised/launch/explore.launch

@@ -1,16 +1,5 @@
 <launch>
 <node pkg="explore_lite_revised" type="explore_lite_revised" respawn="false" name="explore" output="screen">
-  <param name="robot_base_frame" value="base_link"/>
-  <param name="costmap_topic" value="map"/>
-  <param name="costmap_updates_topic" value="map_updates"/>
-  <param name="visualize" value="true"/>
-  <param name="planner_frequency" value="0.33"/>
-  <param name="progress_timeout" value="30.0"/>
-  <param name="potential_scale" value="3.0"/>
-  <param name="orientation_scale" value="0.0"/>
-  <param name="gain_scale" value="1.0"/>
-  <param name="transform_tolerance" value="0.3"/>
-  <!--<param name="min_frontier_size" value="0.75"/>-->
-  <param name="min_frontier_size" value="0.3"/>
+  <rosparam file="$(find explore_lite_revised)/param/explore_lite_revised.yaml" command="load" />
 </node>
 </launch>

explore_lite_revised/launch/explore_costmap.launch

@@ -1,15 +1,5 @@
 <launch>
 <node pkg="explore_lite_revised" type="explore_lite_revised" respawn="false" name="explore" output="screen">
-  <param name="robot_base_frame" value="base_link"/>
-  <param name="costmap_topic" value="move_base/global_costmap/costmap"/>
-  <param name="costmap_updates_topic" value="move_base/global_costmap/costmap_updates"/>
-  <param name="visualize" value="true"/>
-  <param name="planner_frequency" value="0.33"/>
-  <param name="progress_timeout" value="30.0"/>
-  <param name="potential_scale" value="3.0"/>
-  <param name="orientation_scale" value="0.0"/>
-  <param name="gain_scale" value="1.0"/>
-  <param name="transform_tolerance" value="0.3"/>
-  <param name="min_frontier_size" value="0.5"/>
+  <rosparam file="$(find explore_lite_revised)/param/explore_lite_revised.yaml" command="load" />
 </node>
 </launch>

explore_lite_revised/src/explore.cpp

@@ -50,6 +50,7 @@ inline static bool operator==(const geometry_msgs::Point& one,
 
 namespace explore
 {
+
 Explore::Explore()
   : private_nh_("~")
   , tf_listener_(ros::Duration(10.0))
@@ -59,7 +60,8 @@ Explore::Explore()
   , last_markers_count_(0)
 {
   double timeout;
-  double min_frontier_size;
+  int use_each_k_point;
+  double min_frontier_size, max_frontier_angular_size; // parameters for FrontierSearch class
   private_nh_.param("planner_frequency", planner_frequency_, 1.0);
   private_nh_.param("progress_timeout", timeout, 30.0);
   progress_timeout_ = ros::Duration(timeout);
@@ -68,10 +70,12 @@ Explore::Explore()
   private_nh_.param("orientation_scale", orientation_scale_, 0.0);
   private_nh_.param("gain_scale", gain_scale_, 1.0);
   private_nh_.param("min_frontier_size", min_frontier_size, 0.5);
+  private_nh_.param("use_each_k_point", use_each_k_point, 1);
+  private_nh_.param("max_frontier_angular_size", max_frontier_angular_size, 10.0);
 
   search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                  potential_scale_, gain_scale_,
-                                                 min_frontier_size);
+                                                 min_frontier_size,  use_each_k_point, max_frontier_angular_size);
 
   if (visualize_) {
     marker_array_publisher_ =
@@ -92,86 +96,125 @@ Explore::~Explore()
   stop();
 }
 
+std_msgs::ColorRGBA *blue;
+std_msgs::ColorRGBA *red;
+std_msgs::ColorRGBA *green;
+
+    visualization_msgs::Marker *default_msg_template;
+
+// todo consicer cleanup
+void set_default_frontier_marker(visualization_msgs::Marker& m){
+  m.header.stamp = ros::Time::now();
+  m.ns = "frontiers";
+}
+
+
 void Explore::visualizeFrontiers(
     const std::vector<frontier_exploration::Frontier>& frontiers)
 {
-  std_msgs::ColorRGBA blue;
-  blue.r = 0;
-  blue.g = 0;
-  blue.b = 1.0;
-  blue.a = 1.0;
-  std_msgs::ColorRGBA red;
-  red.r = 1.0;
-  red.g = 0;
-  red.b = 0;
-  red.a = 1.0;
-  std_msgs::ColorRGBA green;
-  green.r = 0;
-  green.g = 1.0;
-  green.b = 0;
-  green.a = 1.0;
 
   ROS_DEBUG("visualising %lu frontiers", frontiers.size());
   visualization_msgs::MarkerArray markers_msg;
   std::vector<visualization_msgs::Marker>& markers = markers_msg.markers;
   visualization_msgs::Marker m;
-
+  m = *default_msg_template;
   m.header.frame_id = costmap_client_.getGlobalFrameID();
-  m.header.stamp = ros::Time::now();
-  m.ns = "frontiers";
-  m.scale.x = 1.0;
-  m.scale.y = 1.0;
-  m.scale.z = 1.0;
-  m.color.r = 0;
-  m.color.g = 0;
-  m.color.b = 255;
-  m.color.a = 255;
-  // lives forever
-  m.lifetime = ros::Duration(0);
-  m.frame_locked = true;
 
   // weighted frontiers are always sorted
   double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;
 
+
+  m.id = 0;
+
+  m.action = visualization_msgs::Marker::DELETEALL;
+  markers.push_back(m);
+  ++m.id;
   m.action = visualization_msgs::Marker::ADD;
-  size_t id = 0;
+
+// TODO when the work on visualisation will stabilize, make a separate functuion or even subpackage for it
   for (auto& frontier : frontiers) {
-    m.type = visualization_msgs::Marker::POINTS;
-    m.id = int(id);
+    m.header.frame_id = costmap_client_.getGlobalFrameID();
+
+      m.type = visualization_msgs::Marker::SPHERE_LIST;
+      ++m.id;
+      m.pose.position = {};
+      m.scale.x = 0.2;
+      m.scale.y = 0.2;
+      m.scale.z = 0.2;
+      m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second};
+      m.color = *red;
+      m.header.stamp = ros::Time::now();
+      markers.push_back(m);
+
+
+
+      m.type = visualization_msgs::Marker::LINE_STRIP;
+    ++m.id;
     m.pose.position = {};
     m.scale.x = 0.1;
     m.scale.y = 0.1;
     m.scale.z = 0.1;
-    m.points = frontier.points;
-    if (goalOnBlacklist(frontier.centroid)) {
-      m.color = red;
-    } else {
-      m.color = blue;
-    }
+    m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second};
+    m.color = *red;
+    m.header.stamp = ros::Time::now();
     markers.push_back(m);
-    ++id;
-    m.type = visualization_msgs::Marker::ARROW;
-    m.id = int(id);
-    m.pose.position = frontier.initial;
-    // scale frontier according to its cost (costier frontiers will be smaller)
-    double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);
-    m.scale.x = scale;
-    m.scale.y = scale;
-    m.scale.z = scale;
+
+    m.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    ++m.id;
+    m.text = "F";
+    m.pose.position = frontier.interpolated_line.first;
+    m.scale.x = 0.2;
+    m.scale.y = 0.2;
+    m.scale.z = 0.2;
     m.points = {};
-    m.color = green;
+    m.color = *blue;
+    m.header.stamp = ros::Time::now();
+    markers.push_back(m);
+
+    m.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    ++m.id;
+    m.text = "M";
+    m.pose.position = frontier.centroid;
+    m.scale.x = 0.2;
+    m.scale.y = 0.2;
+    m.scale.z = 0.2;
+    m.points = {};
+    m.color = *blue;
+    m.header.stamp = ros::Time::now();
     markers.push_back(m);
-    ++id;
-  }
-  size_t current_markers_count = markers.size();
 
-  // delete previous markers, which are now unused
-  m.action = visualization_msgs::Marker::DELETE;
-  for (; id < last_markers_count_; ++id) {
-    m.id = int(id);
+    m.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    ++m.id;
+    m.text = "L";
+    m.pose.position = frontier.interpolated_line.second;
+    m.scale.x = 0.2;
+    m.scale.y = 0.2;
+    m.scale.z = 0.2;
+    m.points = {};
+    m.color = *blue;
+    m.header.stamp = ros::Time::now();
+    markers.push_back(m);
+
+    m.type = visualization_msgs::Marker::POINTS;
+    ++m.id;
+    m.pose.position = {};
+    m.scale.x = 0.1;
+    m.scale.y = 0.1;
+    m.scale.z = 0.1;
+    std::vector<geometry_msgs::Point> translated_vector{ frontier.vectors_to_points.begin(), frontier.vectors_to_points.end()};
+    for (auto &i : translated_vector) {
+        i.x += frontier.reference_robot_pose.x;
+        i.y += frontier.reference_robot_pose.y;
+    }
+    m.points = translated_vector;
+    m.color = goalOnBlacklist(frontier.centroid) ? *red : *blue;
+    m.header.stamp = ros::Time::now();
     markers.push_back(m);
+
   }
 
+  size_t current_markers_count = markers.size();
+
   last_markers_count_ = current_markers_count;
   marker_array_publisher_.publish(markers_msg);
 }
@@ -188,6 +231,11 @@ void Explore::makePlan()
   }
 
   if (frontiers.empty()) {
+    visualization_msgs::MarkerArray empty_marker_msg;
+//    empty_marker_msg.markers
+    marker_array_publisher_.publish(empty_marker_msg);
+    visualizeFrontiers(frontiers);
+    ROS_WARN_STREAM("NO FRONTIERS LEFT, PUBLISHING EMPTY LIST");
     stop();
     return;
   }
@@ -287,16 +335,49 @@ void Explore::start()
 
 void Explore::stop()
 {
+//  marker_array_publisher_.publish();
   move_base_client_.cancelAllGoals();
   exploring_timer_.stop();
   ROS_INFO("Exploration stopped.");
 }
 
+void initialize_variables(){
+// TODO rewrite in better style with alocator
+  red = new std_msgs::ColorRGBA();
+  blue = new std_msgs::ColorRGBA();
+  green = new std_msgs::ColorRGBA();
+
+  blue->r = 0;
+  blue->g = 0;
+  blue->b = 1.0;
+  blue->a = 1.0;
+
+  red->r = 1.0;
+  red->g = 0;
+  red->b = 0;
+  red->a = 1.0;
+
+  green->r = 0;
+  green->g = 1.0;
+  green->b = 0;
+  green->a = 1.0;
+
+  default_msg_template = new visualization_msgs::Marker();
+  default_msg_template->ns = "frontiers";
+  // lives forever
+  default_msg_template->lifetime = ros::Duration(0);
+  default_msg_template->frame_locked = false;
+};
+
 }  // namespace explore
 
+
+
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "explore");
+  explore::initialize_variables();
   if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                      ros::console::levels::Debug)) {
     ros::console::notifyLoggerLevelsChanged();