[testing sprint] remove dead code not used in 10 years from navfn (#1926)

* remove dead code not used in 10 years from navfn

* ping

* inverting period to rate

* removing debug statement that could never be triggered by a single non-looping action server

* type change

Author: SteveMacenski

nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

@@ -258,13 +258,13 @@ class NavFn
 
   /** display callback */
   /**< <n> is the number of cycles between updates  */
-  void display(void fn(NavFn * nav), int n = 100);
-  int displayInt;  /**< save second argument of display() above */
-  void (* displayFn)(NavFn * nav);  /**< display function itself */
+  // void display(void fn(NavFn * nav), int n = 100);
+  // int displayInt;  /**< save second argument of display() above */
+  // void (* displayFn)(NavFn * nav);  /**< display function itself */
 
   /** save costmap */
   /**< write out costmap and start/goal states as fname.pgm and fname.txt */
-  void savemap(const char * fname);
+  // void savemap(const char * fname);
 };
 
 }  // namespace nav2_navfn_planner

nav2_navfn_planner/src/navfn.cpp

@@ -131,8 +131,8 @@ NavFn::NavFn(int xs, int ys)
   start[0] = start[1] = 0;
 
   // display function
-  displayFn = NULL;
-  displayInt = 0;
+  // displayFn = NULL;
+  // displayInt = 0;
 
   // path buffers
   npathbuf = npath = 0;
@@ -607,9 +607,9 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart)
       updateCell(*pb++);
     }
 
-    if (displayInt > 0 && (cycle % displayInt) == 0) {
-      displayFn(this);
-    }
+    // if (displayInt > 0 && (cycle % displayInt) == 0) {
+    //   displayFn(this);
+    // }
 
     // swap priority blocks curP <=> nextP
     curPe = nextPe;
@@ -693,9 +693,9 @@ NavFn::propNavFnAstar(int cycles)
       updateCellAstar(*pb++);
     }
 
-    if (displayInt > 0 && (cycle % displayInt) == 0) {
-      displayFn(this);
-    }
+    // if (displayInt > 0 && (cycle % displayInt) == 0) {
+    //   displayFn(this);
+    // }
 
     // swap priority blocks curP <=> nextP
     curPe = nextPe;
@@ -985,48 +985,48 @@ NavFn::gradCell(int n)
 // <n> is the number of cycles to wait before displaying,
 //     use 0 to turn it off
 
-void
-NavFn::display(void fn(NavFn * nav), int n)
-{
-  displayFn = fn;
-  displayInt = n;
-}
+// void
+// NavFn::display(void fn(NavFn * nav), int n)
+// {
+//   displayFn = fn;
+//   displayInt = n;
+// }
 
 
 //
 // debug writes
 // saves costmap and start/goal
 //
 
-void
-NavFn::savemap(const char * fname)
-{
-  char fn[4096];
-
-  RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points");
-  // write start and goal points
-  snprintf(fn, sizeof(fn), "%s.txt", fname);
-  FILE * fp = fopen(fn, "w");
-  if (!fp) {
-    RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn);
-    return;
-  }
-  fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]);
-  fclose(fp);
-
-  // write cost array
-  if (!costarr) {
-    return;
-  }
-  snprintf(fn, sizeof(fn), "%s.pgm", fname);
-  fp = fopen(fn, "wb");
-  if (!fp) {
-    RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn);
-    return;
-  }
-  fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff);
-  fwrite(costarr, 1, nx * ny, fp);
-  fclose(fp);
-}
+// void
+// NavFn::savemap(const char * fname)
+// {
+//   char fn[4096];
+
+//   RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points");
+//   // write start and goal points
+//   snprintf(fn, sizeof(fn), "%s.txt", fname);
+//   FILE * fp = fopen(fn, "w");
+//   if (!fp) {
+//     RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn);
+//     return;
+//   }
+//   fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]);
+//   fclose(fp);
+
+//   // write cost array
+//   if (!costarr) {
+//     return;
+//   }
+//   snprintf(fn, sizeof(fn), "%s.pgm", fname);
+//   fp = fopen(fn, "wb");
+//   if (!fp) {
+//     RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn);
+//     return;
+//   }
+//   fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff);
+//   fwrite(costarr, 1, nx * ny, fp);
+//   fclose(fp);
+// }
 
 }  // namespace nav2_navfn_planner

nav2_navfn_planner/src/navfn_planner.cpp

@@ -319,11 +319,11 @@ NavfnPlanner::getPlanFromPotential(
 
   int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4);
   if (path_len == 0) {
-    RCLCPP_DEBUG(node_->get_logger(), "No path found\n");
     return false;
   }
 
-  RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps\n", path_len);
+  auto cost = planner_->getLastPathCost();
+  RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps, %f cost\n", path_len, cost);
 
   // extract the plan
   float * x = planner_->getPathX();
@@ -402,10 +402,6 @@ bool
 NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
 {
   if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) {
-    RCLCPP_ERROR(
-      node_->get_logger(), "worldToMap failed: wx,wy: %f,%f, "
-      "size_x,size_y: %d,%d", wx, wy,
-      costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
     return false;
   }
 

nav2_planner/src/planner_server.cpp

@@ -233,7 +233,6 @@ PlannerServer::computePlan()
     }
 
     if (action_server_->is_preempt_requested()) {
-      RCLCPP_INFO(get_logger(), "Preempting the goal pose.");
       goal = action_server_->accept_pending_goal();
     }
 

nav2_system_tests/src/planning/CMakeLists.txt

@@ -51,7 +51,6 @@ ament_add_gtest(test_planner_plugin_failures
 )
 
 ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies})
-
 target_link_libraries(test_planner_plugin_failures
   stdc++fs
 )

nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -37,7 +37,7 @@ TEST(testPluginMap, Failures)
 {
   auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
   rclcpp_lifecycle::State state;
-  obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 0.0001));
+  obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0));
   obj->onConfigure(state);
   obj->create_subscription<nav_msgs::msg::Path>(
     "plan", rclcpp::SystemDefaultsQoS(), callback);