DWBLocalPlanner: NavToPose fails if prune_plan is disabled

[DylanDeCoeyer-Quimesis]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04.1 LTS
• ROS2 Version:
  • Humble
• Version or commit hash:
  • 1.1.5
• DDS implementation:
  • Fast DDS

Steps to reproduce issue

Set the DWBLocalPlanner parameters as follow:

prune_plan: False
prune_distance: 0.5

Set the dimensions of local costmap as follow:

width: 8
height: 8

Perform a NavToPose using DWBLocalPlanner as path follower, to a goal set at ~10m.

Expected behavior

The robot drives to the goal.

Actual behavior

The robot drives a few meters (~4, i.e. the half size of the local costmap), then stops. The following error message is displayed:

[controller_server]: Resulting plan has 0 poses in it.

Additional information

---

Feature request

Feature description

Implementation considerations

[SteveMacenski]

Is this not a rolling costmap? If this is setup as a local costmap, I can't see any reason why the prune would matter. As we move, the size should roll with the robot's state and as such its not possible to reach the edge (unless your costmap size is too small for the velocity you're asking it to go with the simulation time)

Send your DWB/controller server / costmap local parameters

[DylanDeCoeyer-Quimesis]

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["PositionAndOrientationGoalChecker"]
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.1
      movement_time_allowance: 15.0
    # Goal checker parameters
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
    PositionAndOrientationGoalChecker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      shorten_transformed_plan: True
      prune_plan: True
      prune_distance: 0.5
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 1.0
      max_vel_y: 0.0
      max_vel_theta: 0.5
      min_speed_xy: 0.0
      max_speed_xy: 1.0
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 0.15
      acc_lim_y: 0.0
      acc_lim_theta: 0.3
      decel_lim_x: -0.45
      decel_lim_y: 0.0
      decel_lim_theta: -0.5
      vx_samples: 5
      vy_samples: 2
      vtheta_samples: 20
      sim_time: 1.5
      discretize_by_time: True
      time_granularity: 0.5
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["KinematicCritic", "GoalDistCritic", "PathDistCritic", "PathAlignCritic"]
      GoalDistCritic.scale: 12.0
      PathDistCritic.scale: 24.0
      PathDistCritic.aggregation_type: "product"
      PathAlignCritic.forward_point_distance: 0.5
      PathAlignCritic.scale: 32.0
      PathAlignCritic.aggregation_type: "product"
      KinematicCritic.scale: 1.0
      KinematicCritic.minimal_turning_radius: 1.5

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 8
      height: 8
      resolution: 0.05
      footprint: "[ [0.496, 0.433], [0.496, -0.433], [-0.312, -0.433], [-0.312, 0.433] ]"
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.713
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: false
        footprint_clearing_enabled: true
        min_obstacle_height: 0.08
        max_obstacle_height: 1.5
        observation_sources: pointcloudL pointcloudR
        pointcloudL:
          data_type: PointCloud2
          topic: /camera_left/depth/color/points
          min_obstacle_height: -0.50
          max_obstacle_height: 3.00
          obstacle_min_range: 0.1
          obstacle_max_range: 8.0
          raytrace_min_range: 0.0
          raytrace_max_range: 9.0
          clearing: true
          marking: true
        pointcloudR:
          data_type: PointCloud2
          topic: /camera_right/depth/color/points
          min_obstacle_height: -0.50
          max_obstacle_height: 3.00
          obstacle_min_range: 0.1
          obstacle_max_range: 8.0
          raytrace_min_range: 0.0
          raytrace_max_range: 9.0
          clearing: true
          marking: true
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

[SteveMacenski]

If you try the default DWB parameters, does it work? I'm seeing you have very few samples for xv and different critics. Its very possible the issue here are your custom critics or you're trying too few samples in a congested scene where there are no valid trajectories that were put to score.

Your costmap all looks fine to me -- even larger than by default, so I see no reason why the costmap would be the cause of this. You should have plenty of space to move.

[mdtoom]

Maybe it is related to this line https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp#L489 , which prevent the dwb_controller from finding the correct transformation_begin when the controller is started more than prune_distance on the path. At least that is how I understand it.
What is the use of that line?

@SteveMacenski

[mdtoom]

To eleborate: what I think is going wrong is that when the robot is farther than 2 * prune_distance on the path, the prune_point that is being selected is prune_distance from the beginning of the path. Because the prune_point limits the search for transformation_begin: prune_point = transformation_begin.
transformation_begin is more than prune_distance from the robot and therefore the search for transformation_end terminates immediately (euclidean_distance(pose, robot_pose.pose) > transform_end_threshold == true). This means an empty path.

[SteveMacenski]

That is checking on the global path for the first point a certain integrated distance away from the path starting point. It uses the range between the path's first pose and that pose to find the closest point on the path to the robot. This restricts the window in case you have exceptionally long paths and/or loops in the path. As the robot tracks the path, the passed poses are removed from the global path so that in the next cycle, it starts fresh where as point 0 should be the closest point on the path to the last iteration's robot position. So that prune_distance should be proportional to the maximum amount of distance traversed you could possibly expect in a given iteration, plus some tolerance. But typically we set it as the costmap window half size as a good generous default since there's few cases users really need it to be any significantly more or less than that. That's what the dist_threshold param is doing, but we merge these concepts more explicitly in RPP.

To eleborate: what I think is going wrong is that when the robot is farther than 2 * prune_distance on the pat

Ah, increase that parameter then to be properly proportional to the distance you could traverse at your control rate and maximum speed.

[SteveMacenski]

Seems a straightforward enough solution - make sure to tune the values for your application!

[DylanDeCoeyer-Quimesis]

As the robot tracks the path, the passed poses are removed from the global path so that in the next cycle, it starts fresh where as point 0 should be the closest point on the path to the last iteration's robot position.

@SteveMacenski this is true only when prune_plan is set to True. The bug I reported here happens when prune_plan is False. In this case, the passed poses are not removed and the prune_point is rapidly stuck at prune_distance from the (fixed) starting point. The analysis of @mdtoom looks correct to me.

[SteveMacenski]

That is expected behavior with the parameters that you've set. If you don't wish to prune old portions of the path, you'll then have to increase your distance to search for the closest point on the path to account for your path lengths.

The other options:

• Add a parameter to disable the maximum distance to search
• More realistically, just have a special parameter value for prune_dist this setting it to something negative will be set in code to std::numeric_limit<double>::max() so that it will take the end point on the path always to check the full thing (which is precisely the same as in the discussion in Regulated Pure Pursuit Lookahead Point resets to start of the path instead of nearest point to robot if the path is larger than max of costmap dimensions or max_robot_pose_search_distance #3367) <-- this is my preference
• Complete your PR to sep. out the prune from shorten distances which has review items pending nav2_dwb_controller: add forward_prune_distance parameter #3374 <-- this I think you'll still want / need

[DylanDeCoeyer-Quimesis]

Ok, I understand. It is pretty non-intuitive that the prune_distance parameter still plays a critical role when prune_plan is disabled, and that it must be set to the maximum path length by the user (or -1). I believe that prune_distance shall be hard-coded to std::numeric_limit<double>::max() when prune_plan is disabled (through if/else in the code). What do you think?