Add more profiler and debug traces

MRPT · Sep 23, 2024 · 538ec8c · 538ec8c
1 parent d6edb61
commit 538ec8c
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/apps/path-planner-cli/path-planner-cli.cpp b/apps/path-planner-cli/path-planner-cli.cpp
@@ -300,17 +300,22 @@ static void do_plan_path()
     }
 
     // Insert custom progress callback:
-    planner->progressCallback_ = [](const mpp::ProgressCallbackData& pcd) {
+    planner->progressCallback_ = [](const mpp::ProgressCallbackData& pcd)
+    {
         std::cout << "[progressCallback] bestCostFromStart: "
                   << pcd.bestCostFromStart
                   << " bestCostToGoal: " << pcd.bestCostToGoal
                   << " bestPathLength: " << pcd.bestPath.size() << std::endl;
     };
 
     // PTGs config file:
+    std::cout << "[PTGs] Initializing PTGs..." << std::endl;
+
     mrpt::config::CConfigFile cfg(arg_ptgs_file.getValue());
     pi.ptgs.initFromConfigFile(cfg, arg_config_file_section.getValue());
 
+    std::cout << "[PTGs] Done." << std::endl;
+
     // ==================================================
     // ACTUAL PATH PLANNING
     // ==================================================

diff --git a/mrpt_path_planning/src/algos/TPS_Astar.cpp b/mrpt_path_planning/src/algos/TPS_Astar.cpp
@@ -128,12 +128,16 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
     // ------------------------------------------------------------------
     tree.edges_to_children.clear();
 
+    MRPT_LOG_DEBUG_STREAM("Initializing lattice...");
+    mrpt::system::CTimeLoggerEntry tle_lat(profiler_(), "plan.lattice");
     grid_.setSize(
         in.worldBboxMin.x, in.worldBboxMax.x,  // x
         in.worldBboxMin.y, in.worldBboxMax.y,  // y
         params_.grid_resolution_xy, params_.grid_resolution_yaw,  // res
         in.worldBboxMin.phi, in.worldBboxMax.phi  // phi / yaw
     );
+    tle_lat.stop();
+    MRPT_LOG_DEBUG_STREAM("Lattice init done.");
 
     // ----------------------------------------
     //