locusrobotics / robot_navigation

Spiritual successor to ros-planning/navigation.
444 stars 149 forks source link

global planner caching problem #103

Open jaykorea opened 1 year ago

jaykorea commented 1 year ago

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;

}