debug print frontier costs

a4aleem · May 19, 2017 · 4fc9609 · 4fc9609
1 parent 8e341b7
commit 4fc9609
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp
@@ -183,6 +183,9 @@ void Explore::makePlan()
   // find frontiers
   auto frontiers = search_.searchFrom({});
   ROS_DEBUG("found %lu frontiers", frontiers.size());
+  for (size_t i = 0; i < frontiers.size(); ++i) {
+    ROS_DEBUG("frontier %zd cost: %f", i, frontiers[i].cost);
+  }
 
   if (frontiers.empty()) {
     stop();