Skip to content

global planner caching problem #103

@jaykorea

Description

@jaykorea

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;

}

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions