From c65999c7568c106d7ecac1ecb791a297d8545955 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Tue, 16 Jan 2024 21:42:17 +0100 Subject: [PATCH] `planner` - global path's age detection of `RecoveryManager` integrated with the planner class --- src/humap_planner.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/humap_planner.cpp b/src/humap_planner.cpp index 38493b03..5ddefec9 100644 --- a/src/humap_planner.cpp +++ b/src/humap_planner.cpp @@ -207,6 +207,9 @@ void HumapPlanner::reconfigure(HumapConfigConstPtr cfg) { cfg_->getPlannerBehaviors()->oscillation_buffer_length / cfg_->getGeneral()->sim_period )) ); + recovery_.setGlobalPathPlanMaxAge( + cfg_->getGeneral()->path_planning_period * cfg_->getPlannerBehaviors()->path_plan_update_violation_multiplier + ); } bool HumapPlanner::checkTrajectory( @@ -439,6 +442,8 @@ base_local_planner::Trajectory HumapPlanner::findBestTrajectory( obstacle_costs_ ); + recovery_.checkGlobalPlanAge(); + auto traj_robot_g = Trajectory(result_traj_, true); auto traj_robot_poses = traj_robot_g.getPoses(); std::vector trajs_people; @@ -551,6 +556,7 @@ bool HumapPlanner::setPlan(const std::vector& orig_g stop_rotate_controller_.resetLatching(); bool plan_updated = planner_util_->setPlan(orig_global_plan); + recovery_.markGlobalPlanUpdate(); // get goal pose transformed to the local frame (usually 'odom') geometry_msgs::PoseStamped goal_new;