From 0f64e304231c110bcc909b7b256fae5f3622804a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 18 Mar 2021 10:46:35 +0900 Subject: [PATCH] planner_cspace: fix goal unreachable condition (#595) --- planner_cspace/src/planner_3d.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 9b9bf57b..1c088987 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -1748,7 +1748,9 @@ class Planner3dNode } const Astar::Vec s_rough(s[0], s[1], 0); - if (cost_estim_cache_[s_rough] == std::numeric_limits::max()) + // If goal gets occupied, cost_estim_cache_ is not updated to reduce + // computational cost for clearing huge map. In this case, cm_[e] is 100. + if (cost_estim_cache_[s_rough] == std::numeric_limits::max() || cm_[e] >= 100) { status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND; ROS_WARN("Goal unreachable.");