explore: adjust min_frontier size filtering according to map resolution

* param min_frontier_size is now double. Shoudl be size in meters.
a4aleem · Dec 15, 2017 · 46223c4 · 46223c4
1 parent c3ac7b1
commit 46223c4
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 14 deletions.
diff --git a/explore/doc/wiki_doc.txt b/explore/doc/wiki_doc.txt
@@ -96,9 +96,9 @@ param {
   11.desc = Transform tolerance to use when transforming robot pose.
 
   12.name = ~min_frontier_size
-  12.default = `10`
-  12.type = int
-  12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. Currently there is a bug causing the default value to be used all the time.
+  12.default = `0.5`
+  12.type = double
+  12.desc = Minimum size of the frontier to consider the frontier as the exploration goal. In meters.
 }
 
 req_tf {

diff --git a/explore/include/explore/frontier_search.h b/explore/include/explore/frontier_search.h
@@ -35,7 +35,7 @@ class FrontierSearch
    * @param costmap Reference to costmap data to search.
    */
   FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale,
-                 double gain_scale, size_t min_frontier_size);
+                 double gain_scale, double min_frontier_size);
 
   /**
    * @brief Runs search implementation, outward from the start position
@@ -82,7 +82,7 @@ class FrontierSearch
   unsigned char* map_;
   unsigned int size_x_, size_y_;
   double potential_scale_, gain_scale_;
-  size_t min_frontier_size_;
+  double min_frontier_size_;
 };
 }
 #endif
diff --git a/explore/launch/explore.launch b/explore/launch/explore.launch
@@ -10,6 +10,6 @@
   <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="15"/>
+  <param name="min_frontier_size" value="0.75"/>
 </node>
 </launch>
diff --git a/explore/launch/explore_costmap.launch b/explore/launch/explore_costmap.launch
@@ -10,6 +10,6 @@
   <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="3"/>
+  <param name="min_frontier_size" value="0.5"/>
 </node>
 </launch>
diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp
@@ -59,19 +59,19 @@ Explore::Explore()
   , last_markers_count_(0)
 {
   double timeout;
-  int min_frontier_size;
+  double min_frontier_size;
   private_nh_.param("planner_frequency", planner_frequency_, 1.0);
   private_nh_.param("progress_timeout", timeout, 30.0);
   progress_timeout_ = ros::Duration(timeout);
   private_nh_.param("visualize", visualize_, false);
   private_nh_.param("potential_scale", potential_scale_, 1e-3);
   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, 10);
+  private_nh_.param("min_frontier_size", min_frontier_size, 0.5);
 
   search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                  potential_scale_, gain_scale_,
-                                                 size_t(min_frontier_size));
+                                                 min_frontier_size);
 
   if (visualize_) {
     marker_array_publisher_ =
@@ -180,6 +180,7 @@ void Explore::makePlan()
 {
   // find frontiers
   auto pose = costmap_client_.getRobotPose();
+  // get frontiers sorted according to cost
   auto frontiers = search_.searchFrom(pose.position);
   ROS_DEBUG("found %lu frontiers", frontiers.size());
   for (size_t i = 0; i < frontiers.size(); ++i) {
@@ -206,7 +207,6 @@ void Explore::makePlan()
     stop();
     return;
   }
-  // todo sort frontiers
   geometry_msgs::Point target_position = frontier->centroid;
 
   // time out if we are not making any progress

diff --git a/explore/src/frontier_search.cpp b/explore/src/frontier_search.cpp
@@ -16,7 +16,7 @@ using costmap_2d::FREE_SPACE;
 
 FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap,
                                double potential_scale, double gain_scale,
-                               size_t min_frontier_size)
+                               double min_frontier_size)
   : costmap_(costmap)
   , potential_scale_(potential_scale)
   , gain_scale_(gain_scale)
@@ -75,8 +75,8 @@ std::vector<Frontier> FrontierSearch::searchFrom(geometry_msgs::Point position)
       } else if (isNewFrontierCell(nbr, frontier_flag)) {
         frontier_flag[nbr] = true;
         Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
-        // TODO consider map resolution here
-        if (new_frontier.size >= min_frontier_size_) {
+        if (new_frontier.size * costmap_->getResolution() >=
+            min_frontier_size_) {
           frontier_list.push_back(new_frontier);
         }
       }