nav2_dwb_controller goal_dist.cpp why prepare function not use third input param:goal

[dahuacai]

Issure Code dir: nav2_dwb_controller/dwb_critics/src/goal_dist.cpp

bool GoalDistCritic::prepare(
  const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
  const geometry_msgs::msg::Pose2D &,
  const nav_2d_msgs::msg::Path2D & global_plan)
{
  reset();

  unsigned int local_goal_x, local_goal_y;
  if (!getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y)) {
    return false;
  }

  // Enqueue just the last pose
  int index = costmap_->getIndex(local_goal_x, local_goal_y);
  cell_values_[index] = 0.0;
  queue_->enqueueCell(local_goal_x, local_goal_y);

  propogateManhattanDistances();

  return true;
}

Above funcion default use the costmap last pose to propogateManhattanDistances.it no use third param "goal ",it will case dwa do not heading to goal, instead to select the trajectory that its end pose most fit to costmap end pose.

when i modify the code to following :

bool GoalDistCritic::prepare(
    const std_msgs::Pose2D & pose, const std_msgs::Twist2D & vel,
    const std_msgs::Pose2D & goal, const std_msgs::Path2D & global_plan)
{
  reset();
  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
  unsigned int local_goal_x, local_goal_y;
  bool suc = costmap_->worldToMap(goal.x, goal.y, local_goal_x, local_goal_y);
  lock.unlock();

  //if pose not at costmap then use last costmap cord
  if (!suc && !getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y)) {
    return false;
  }

  // Enqueue just the last pose
  lock.lock();
  double wx, wy;
  int index = costmap_->getIndex(local_goal_x, local_goal_y);

  costmap_->mapToWorld(local_goal_x, local_goal_y, wx, wy);
  lock.unlock();
  cell_values_[index] = 0.0;
  queue_->enqueueCell(local_goal_x, local_goal_y);

  propogateManhattanDistances();

  return true;
}

dwa will tracking the goal better.

[SteveMacenski]

Why would the last goal on costmap when the goal is on the costmap not be the right goal location?

[dahuacai]

In general, we want the dwa to track the lookahead point, which is the "goal", not the last point in the local cost map.The global path's last position in the local cost map, the "last_pose," should not be the target of tracking, or should not be the target of least generation value. If the path is tortuous, if we take the last_pose as the initial point of the Manhattan distance, then the DWA may generate an optimal curve, which is very curved and may undergo strange control oscillations.

[dahuacai]

the first picture use the last pose to propogateManhattanDistances,we find if the path is very tortuous, the dwa generator a curve which do not heading to path ,but heading the last pose. the second picture use the lookahead pose to propogateManhattanDistances,we find it will tracking the path always good.

[SteveMacenski]

Perhaps this is better to consolidate in your new ticket #3968 on DWB general behavior improvements. Please provide a proposal for improvement and happy to review it! A non-end-point intermediary lookahead point instead of the end of the costmap seems sensible. That could resolve some stuck cases when navigating around obstacles - I agree!

---

goal is the goal at the end of the path, not a lookahead point - so that seems in conflict with what you're asking. Also, the goal that that defines is populated in the controller as the last point in the path - so the update your suggest in code above is moot, it would be exactly the same when the goal is within the current costmap window.

  bool suc = costmap_->worldToMap(goal.x, goal.y, local_goal_x, local_goal_y);

That would succeed when goal is within the costmap window, which would be the path's end point, which would be exactly identical to getLastPoseOnCostmap(...). So the original suggestion in your ticket I think is not actually doing anything - but your last comment on a non-costmap-end-point option seems good. The way DWB handles this now seems really naive.

[dahuacai]

OH sorry, if find that,my dwb code has changed so much that I forget that the method of calculating the looahead point is my own code and not inherent to dwb.So the goal in my code normally is lookahead point ,not the raw path end goal in local costmap. getLookAheadPoint method is similar to regulator pure path tracking alg. the getLookAheadPointConsiderObstacle func use theta star to plan a new transform_path to guide dwb to avoid obstacle:

std_msgs::Pose2D 
DWALocalPlanner::getLookAheadPoint(
  const std_msgs::Pose2DStamped & pose,
  const double & lookahead_dist,
  const std_msgs::Path2D & transformed_plan)
{
  // Find the first pose which is at a distance greater than the lookahead distance
  auto goal_pose_it = std::find_if(
    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const std_msgs::Pose2D & ps) {
      return hypot(pose.pose.x - ps.x, pose.pose.y - ps.y) >= lookahead_dist;
    });

  // If the no pose is not far enough, take the last pose
  if (goal_pose_it == transformed_plan.poses.end()) {
    goal_pose_it = std::prev(transformed_plan.poses.end());
  }

  return *goal_pose_it;
}

  std_msgs::Pose2D DWALocalPlanner::getLookAheadPointConsiderObstacle(
    const std_msgs::Pose2DStamped & pose,
    const double & lookahead_dist,
    std_msgs::Path2D & transformed_plan) {
    if (transformed_plan.poses.empty()) {
      return std_msgs::Pose2D();
    }

    Time start_time,end_time;
    start_time.SetNow();
    std_msgs::Pose2D look_ahead_point;
    bool find_obs = false;
    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(local_costmap_ptr_->getMutex()));
    for (auto it = transformed_plan.poses.begin(); it!= transformed_plan.poses.end(); it++) {
      unsigned int mx = 0, my = 0;
      bool suc = local_costmap_ptr_->worldToMap(it->x, it->y, mx, my);
      if (suc && local_costmap_ptr_->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
          local_costmap_ptr_->getCost(mx, my) <= nav2_costmap_2d::LETHAL_OBSTACLE) {
        find_obs = true;
        break;
      }
    }

    if (!find_obs) {
      look_ahead_point = getLookAheadPoint(pose, lookahead_dist, transformed_plan);
      ROBOT_INFO("getLookAheadPointConsiderObstacle.use getLookAheadPoint:(%f,%f).loc pose:(%f,%f)",
        look_ahead_point.x, look_ahead_point.y, pose.pose.x, pose.pose.y);
      return look_ahead_point;
    }

    double dist_threshold = std::max(local_costmap_ptr_->getSizeInCellsX(), local_costmap_ptr_->getSizeInCellsY()) *
      local_costmap_ptr_->getResolution() / 2.0;

    std_msgs::PoseStamped start, end;
    start.pose.position.x = pose.pose.x;
    start.pose.position.y = pose.pose.y;
    auto goal_pose_it = transformed_plan.poses.rbegin();
    bool find = false;
    size_t num_points = transformed_plan.poses.size();
    int i = 0;
    double res = local_costmap_ptr_->getResolution();
    double min_lookahead = dist_threshold * lookahead_threshold_scale_;
    std_msgs::Pose2D goal_pose;
    for (auto it = transformed_plan.poses.rbegin(); it!= transformed_plan.poses.rend(); it++) {
      unsigned int mx = 0, my = 0;
      //if remain path dist below to min lookahead distance, break      
      if ((num_points - i) * res < min_lookahead) {
        break;
      }
      bool suc = local_costmap_ptr_->worldToMap(it->x, it->y, mx, my);
      if (suc && local_costmap_ptr_->getCost(mx, my) < nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
        end.pose.position.x = it->x;
        end.pose.position.y = it->y;
        lock.unlock();
        std_msgs::Path path_local = theta_star_planner_.createPlan(start, end);
        lock.lock();
        bool fulfill_min_lookahead = false;
        int fulfill_index = 0;
        if (!path_local.poses.empty()) {
          for (auto & p : path_local.poses) {
            // find a valid lookahead point from new path
            if (hypot(pose.pose.x - p.pose.position.x, pose.pose.y - p.pose.position.y) > lookahead_dist) {
              goal_pose.x = p.pose.position.x;
              goal_pose.y = p.pose.position.y;

              transformed_plan = base_math_library::PathToPath2D(path_local);//change transformed_plan due to critic will use this
              pub_->publishTransformedPlan(transformed_plan);
              end_time.SetNow();
              return goal_pose;
            }

            //if no valid lookahead point found in new path, try to find a valid min lookahead point
            if (!fulfill_min_lookahead &&
                hypot(pose.pose.x - p.pose.position.x, pose.pose.y - p.pose.position.y) > min_lookahead) {
                fulfill_min_lookahead = true;
            }
            else if(!fulfill_min_lookahead) {
              fulfill_index++;
            }
          }

          if (fulfill_min_lookahead && fulfill_index < path_local.poses.size()) {
            transformed_plan = base_math_library::PathToPath2D(path_local);//critic will use this
            pub_->publishTransformedPlan(transformed_plan);
            goal_pose.x = path_local.poses[fulfill_index].pose.position.x;
            goal_pose.y = path_local.poses[fulfill_index].pose.position.y;
            return goal_pose;
          }
        }
      }
      i++;
    }
       return std_msgs::Pose2D();
  }

now use above code:

auto carrot_pose = getLookAheadPointConsiderObstacle(pose, lookahead_dist_, transformed_plan);
  if (carrot_pose.x == 0.0 && carrot_pose.y == 0.0) {
    ROBOT_ERROR("dwa_local_planner.getLookAheadPointConsiderObstacle return 0.0,0.0");
    *error = std_msgs::RobotError::CONTROLLER_EXCEPTION;
    return cmd_vel;
  }
  carrot_pose_stamped_.header = pose.header;
  carrot_pose_stamped_.pose.position.x = carrot_pose.x;
  carrot_pose_stamped_.pose.position.y = carrot_pose.y;
  carrot_pose_stamped_.pose.SetRPY(0, 0, carrot_pose.theta);
  pub_->PublishCarrotPose(carrot_pose_stamped_);

  for (TrajectoryCritic::Ptr & critic : critics_) {
    if (!critic->prepare(pose.pose, twist_vel, carrot_pose, transformed_plan)) {
      *error = std_msgs::RobotError::CONTROLLER_EXCEPTION;
      return last_cmd_twist_ = cmd_vel;
    }
  }

[SteveMacenski]

Personally, I think the best place to put something like this is in the GoalDistCritic, basically rewrite / rename getLastPoseOnCostmap to get some better lookahead point, which one option could still be the last point on the costmap. I think its useful to provide the actual goal the is actually requested in the critic functions without modifications for those that might care.

For the same reasons I mentioned in your other ticket, I don't necessarily agree that cost information should play a role in the lookahead point selection, but that could certainly be a parameterized option if you feel it helps in some situations. Even just selecting a non-costmap-end goal point might do the vast majority of the heavy lifting for you to get out of stuck situations. Do you see that in your testing as really putting it over the top?

If interested, I'd be happy to review a draft PR *without the A bits**. We're going to have to do some work on the code you mentioned above though:

• This modification in GoalDistCritic, not as modifying the controller's goal.
• Understanding from your experimentation why the cost stuff helps or if that's just tuning

Overall, I think this could be a nice behavioral improvement. Now how to deal with the cost bits are something to be discussed, but I really don't feel the planner as you've implemented it is a suitable solution.

---

Note: please edit your posts if your codeblocks aren't rendering properly. I've been editing your posts to make them readable - you can use code blocks by using "```" on new lines before and after the code.

[dahuacai]

My main objective is to use a point from the original transform path(part of global path in local costmap) as a goal when the original local transform path is obstructed by obstacles within the local cost map. I aim to plan a local path to this goal using the A* algorithm. This local path will then be used as the new Transformed_path. The DWB algorithm will evaluate this new local path and generate a trajectory that tracks this transformed path. The goal serves as a guide to reach the global path points, while the new transformed path aims to concatenate the new obstacle-avoiding path with the original global path. The DWB algorithm aims to optimize the local path as much as possible.

[SteveMacenski]

I aim to plan a local path to this goal using the A* algorithm

I don't think that this is a suitable contribution back to the stack, but I appreciate letting us know and perhaps if in some months/years others agree that this is a valuable contribution back, we can circle back to it. However, I do not feel right now that this is a good, general solution. But this is a good reason why the critics are plugins, so you can create your own objective functions for your own specific applications -- alot of the reason why I don't like that this is implemented outside of the critic structure where it seems to more naturally belong.

I think the contribution of using a non-end-of-costmap point for the goal distance critic is sensible, but the A* contributions I do not.

[dahuacai]

Why it is permissible to use multiple evaluation criteria to drive DWB to track the original path(transformed_path), but not to use new replanning of obstruction-free paths(new_transformed path) to drive DWB to improve global obstacle avoidance. I think this is an optimization that probably doesn't change the original intent of DWB.

[dahuacai]

But as you say, using a plug-in form might be more appropriate