From a972d2976fb594242d7b0a62250e83f22247c8fd Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 24 Sep 2024 00:09:23 +0200 Subject: [PATCH] Add roslog INFO traces to measure time spent initializing PTGs --- mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp b/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp index 0a58816..c281ad0 100644 --- a/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp +++ b/mrpt_tps_astar_planner/src/mrpt_tps_astar_planner_node.cpp @@ -503,8 +503,12 @@ void TPS_Astar_Planner_Node::initialize_planner() this->get_logger(), "Loaded these planner params:" << planner_->params_as_yaml()); mrpt::config::CConfigFile cfg(ptg_ini_file_); + RCLCPP_INFO_STREAM(this->get_logger(), "Initializing PTGs..."); + ptgs_.initFromConfigFile(cfg, "SelfDriving"); + RCLCPP_INFO_STREAM(this->get_logger(), "PTGs initialized."); + costMapParams_ = mpp::CostEvaluatorCostMap::Parameters::FromYAML( mrpt::containers::yaml::FromFile(costmap_params_file_)); }