global planner caching problem
jaykorea opened this issue · 0 comments
jaykorea commented
I am using dlux global planner with nav_core_adpater with "move_base_flex"
As I set the goal point with "move_base_simple/goal" and the path is blocked by obstacle(no plan path found), it returns with succeed("move_base_flex/move_base/status" - Action "move_base" succeeded!" msg came out) and can not get to goal even if the obstacle is move away(sending the exact same goal point, It only returns move_base succeeded message).
I think global planner is caching the path and it does not consider updated costmap.
what would be the problem?
below is my dlux global planner params.
GlobalPlannerAdapter:
planner_name: dlux_global_planner::DluxGlobalPlanner
DluxGlobalPlanner:
potential_calculator: dlux_plugins::AStar
traceback: dlux_plugins::GradientPath
publish_potential: False
print_statistics: False
neutral_cost: 66
scale: 3
unkown_interpretation: "lethal" # lethal, expensive, free
path_caching: true
improvement_threshold: 0
Or Can I just resolve this isssue with flag ?
bool DluxGlobalPlanner::hasValidCachedPath(const geometry_msgs::Pose2D& local_goal,
unsigned int goal_x, unsigned int goal_y)
{
bool ret = false;
if(previous_plan_successful_ && cached_path_cost_ >= 0 && cached_goal_x_ == goal_x && cached_goal_y_ == goal_y &&
isPlanValid(cached_path_))
{
ret = true;
}
//bool ret = cached_path_cost_ >= 0 && cached_goal_x_ == goal_x && cached_goal_y_ == goal_y &&
// isPlanValid(cached_path_);
cached_goal_x_ = goal_x;
cached_goal_y_ = goal_y;
return ret;
//return cached_path_cost_ >= 0 && isPlanValid(cached_path_);
}
nav_2d_msgs::Path2D DluxGlobalPlanner::makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal)
{
previous_plan_successful_ = false; // reset the variable
.
.
.
other codes
.
.
.
// If there is a cached path available and the new path cost has not sufficiently improved
if (cached_plan_available && !shouldReturnNewPath(path, path_cost))
{
return cached_path_;
}
cached_path_cost_ = path_cost;
cached_path_ = path;
if (!path.poses.empty()) previous_plan_successful_ = true;
return path;
}