Unable to reach GoalPose with xy precision higher than 40cm

[3wnbr1]

Bug report

Required Info:

• Operating System:
  • Ubuntu bionic 18.04.03
• Version or commit hash:
  • 1faa3425cf5a51c3e0d1102687555932663b499e : branch eloquent-devel
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

• Create a YAML configuration file with attached parameters
• Launch the ros2 navigation stack and set a goal pose. The navigation succeeds
• Edit the Yaml and replace xy_goal_tolerance: 0.25 by xy_goal_tolerance: 0.05
• Restart the ros2 navigation stack and set a GoalPose. navigation fails at the end.

Expected behavior

• On the first round, I would expect the robot to stop at around 0.25 cm from the GoalPose
• Next, the robot should stop at around 0.05 cm from the GoalPose

Actual behavior

• At first, the robot stops at around 40cm from its GoalPose but the navigation succeeds
• Then, the robot goes to recovery mode as navigation failed. Never reaches the GoalPose

Additional information

• Map config

image: map.ppm
resolution: 0.02
origin: [-1.5, -1, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

• Navigation config

bt_navigator:
  ros__parameters:
    use_sim_time: True
    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node


controller_server:
  ros__parameters:
    use_sim_time: True
    # Trajectory Scoring Parameters
    #
    # Scores a trajectory based on where the path passes over the costmap.
    # To use this properly, you must use the inflation layer in costmap to
    # expand obstacles by the robot's radius.
    BaseObstacle/class: BaseObstacle
    BaseObstacle:
      scale: 0.01
      sum_scores: false
    # Scores a trajectory based on how well aligned the trajectory is with the goal pose.
    GoalAlign/class: GoalAlign
    GoalAlign:
      aggregation_type: last
      scale: 0.0
    # Scores a trajectory based on how close the trajectory gets the robot to the goal pose.
    GoalDist/class: GoalDist
    GoalDist:
      aggregation_type: last
      scale: 24.0
    # Prevents the robot from just moving backwards and forwards.
    Oscillation/class: Oscillation
    Oscillation:
      scale: 1.0
      x_only_threshold: 0.05
    # Scores a trajectory based on how well it is aligned to the path provided by the global planner.
    PathAlign/class: PathAlign
    PathAlign:
      aggregation_type: last
      scale: 0.0
    # Scores a trajectory based on how far it ends up from the path provided by the global planner.
    PathDist/class: PathDist
    PathDist:
      aggregation_type: last
      scale: 32.0
    # Only allows the robot to rotate to the goal orientation when it is sufficiently close to the goal location
    RotateToGoal/class: RotateToGoal
    RotateToGoal:
      scale: 16.0
    critics:
    - BaseObstacle
    - GoalAlign
    - GoalDist
    - Oscillation
    - PathAlign
    - PathDist
    - RotateToGoal

    # Robot Configuration Parameters
    acc_lim_theta: 2.0
    acc_lim_x: 0.2
    acc_lim_y: 0.0
    decel_lim_theta: -2.0
    decel_lim_x: -0.2
    decel_lim_y: 0.0
    max_vel_theta: 1.0
    max_vel_x: 0.50
    max_vel_y: 0.0
    min_speed_theta: 0.001
    min_speed_xy: -1.0
    max_speed_xy: 1.0
    min_vel_x: -0.50
    min_vel_y: 0.0
    min_theta_velocity_threshold: 0.001
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 1.0


    debug_trajectory_details: false
    discretize_by_time: false
    goal_checker_name: dwb_plugins::SimpleGoalChecker
    local_controller_plugin: dwb_core::DWBLocalPlanner
    publish_cost_grid_pc: false
    publish_evaluation: false
    publish_global_plan: true
    publish_local_plan: true
    publish_trajectories: false
    publish_transformed_plan: true
    trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator
    transform_tolerance: 0.2

    # Forward Simulation Parameters
    sim_time: 1.7
    linear_granularity: 0.025
    use_sim_time: true
    vx_samples: 25
    vy_samples: 0
    vtheta_samples: 5
    controller_frequency: 20.0

    # Goal Tolerance Parameters
    xy_goal_tolerance: 0.25
    yaw_goal_tolerance: 0.05
    latch_xy_goal_tolerance: true

    # Global Plan Parameters
    prune_plan: true
    prune_distance: 1.0


controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True


local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: True
      global_frame: odom
      plugin_names: ["obstacle_layer", "inflation_layer"]
      plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.02
      # robot_radius: 0.15
      footprint: "[[-0.128, -0.095], [-0.128, 0.095], [0.128, 0.095], [0.128, -0.095]]"
      inflation_layer.cost_scaling_factor: 3.0
      obstacle_layer:
        enabled: False
        scan:
          topic: /asterix/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: False
      observation_sources: scan
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True


global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: True
      # robot_radius: 0.15
      footprint: "[[-0.128, -0.095], [-0.128, 0.095], [0.128, 0.095], [0.128, -0.095]]"
      obstacle_layer:
        enabled: True
        scan:
          topic: /asterix/scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
      observation_sources: scan
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

[SteveMacenski]

Did you try tuning your controller to follow paths more closely? I think that problem may largely fall on the controller tuning if you don't tuned it in a way capable of achieving that accuracy. If you play with the controller gains, does that behavior change?

Also, I'd add some print outs / use the ros2 CLI to make sure that your parameter is being correctly read in, just as a precaution, with your 40cm comment.

Followup with that information and we can see what we can do.

[3wnbr1]

I'm 99% confident that my parameters are read correctly since I'm trying to tune them for a few weeks now.

Did you try tuning your controller to follow paths more closely? I think that problem may largely fall on the controller tuning if you don't tuned it in a way capable of achieving that accuracy. If you play with the controller gains, does that behavior change?

I totally agree on this but using the default dwb_core::DWBLocalPlanner, I was not able to find the controller PID/Corrector gains. From your knowledge of the project structure, could you let me know where I might find them or anything equivalent ?

Best regards

[SteveMacenski]

I'm 99% confident that my parameters are read correctly since I'm trying to tune them for a few weeks now.

I mean, I hope that's true, but I did refactor the way parameters are read in not long ago and if there are issues, this may be how they show up. See #1468.

This isn't a PID controller, you may want to read Thrun's paper if you're not familiar with DWA. All of these parameters are dials you can turn, and it wouldnt surprise me if there were actually more that we didn't describe in the default configuration file. If you launch things and then ros2 param list to get them you should see them all if there's any missing. Also those critics are plugins so you can use more or less than what's just there. This ticket also describes something potentially related if we can't scale these critics (#938).

[SteveMacenski]

@3wnbr1 any updates? Did you find your problem was better after some tuning?

[Jconn]

I mean, I hope that's true, but I did refactor the way parameters are read in not long ago and if there are issues, this may be how they show up. See #1468.

Edit: looking at branches a little closer I don't think the refactor caused this.

the rotate_to_goal critic and goal_checker are looking at different xy_goal_tolerance params in eloquent-devel as well as master.
#1567

the goal_checker is picking up the change from your param file but the rotate_to_goal critic is not. If you tighten your xy_goal_tolerance beyond the default, the rotate_to_goal critic could fire and ban you from anything but rotates before you hit your goal. This might be your issue @3wnbr1

[3wnbr1]

@3wnbr1 any updates? Did you find your problem was better after some tuning?

Sorry for the late reply, I tried the second trajectory generator dwb_plugins::LimitedAccelGenerator which best suite my needs by the way. As the speeds and accelerations were defined by the robot, I proceeded to tuning the critics.

All my tests were done on master commit f1aee99

At first, my robot wasn't achieving its goal but entering recovery right before getting around 40cm from it. The reason was there was no valid trajectories (whatever the number of samples in x, y and theta space).

So I said to myself that I might have been too restrictive on the critics so in order to tune the controller, I started by disabling most of the critics. I kept only BaseObstacle GoalDist and PathAlign. I tested different ranges of values for GoalDistfrom 0.01 where the robot stopped earlier to 16.0 where I had the same behavior as before. But things got weird when I defined the GoalDist critic value to 1.0. I attached the video of it and controller parameters are bellow for reference.

ROS2 Navigation Tuning Strange behavior

I noticed that about 40 cms from the goal pose, all trajectories where converging to a "virtual point" while their critics went worse which caused the failure and recovery state. But at some point, setting the critic to 1.0 allowed at least a single valid trajectory and then the robot reached the goal.

TL;DR : Tuning the critics influences the cost (color) of the trajectories but I don't get why they converge to single point close to the goal pose

I would like your feedback on this as the trajectories generated close to the goal look pretty abnormal. Maybe this could be related #1567 or something similar with a default parameter laying around.

controller_server:
  ros__parameters:
    # Robot Configuration Parameters
    use_sim_time: True

    FollowPath:
      acc_lim_theta: 3.0
      acc_lim_x: 1.0
      acc_lim_y: 0.0
      decel_lim_theta: -3.0
      decel_lim_x: -1.0
      decel_lim_y: 0.0
      max_vel_theta: 3.0
      max_vel_x: 0.79
      max_vel_y: 0.0
      min_speed_theta: 0.05
      min_speed_xy: 0.006
      max_speed_xy: 0.80
      min_vel_x: -0.79
      min_vel_y: 0.0

      sim_time: 1.7
      linear_granularity: 0.025
      angular_granularity: 0.025
      vx_samples: 25
      vy_samples: 0
      vtheta_samples: 20

      # Trajectory Scoring Parameters
      #
      # Scores a trajectory based on where the path passes over the costmap.
      # To use this properly, you must use the inflation layer in costmap to
      # expand obstacles by the robot's radius.
      BaseObstacle:
        scale: 0.02
        sum_scores: false
      # Scores a trajectory based on verifying all points along the robot's footprint don't touch an obstacle marked in the costmap.
      ObstacleFootprint:
        scale: 0.02
        sum_scores: false
      # Prevents holonomic robots from spinning as they make their way to the goal.
      Twirling:
        scale: 0.0
      # Scores a trajectory based on how well aligned the trajectory is with the goal pose.
      GoalAlign:
        aggregation_type: last
        scale: 0.01
      # Scores a trajectory based on how close the trajectory gets the robot to the goal pose.
      GoalDist:
        scale: 1.0
        sum_scores: false
      # Prevents the robot from just moving backwards and forwards.
      Oscillation:
        scale: 1.0
        x_only_threshold: 0.05
      # Scores a trajectory based on how well it is aligned to the path provided by the global planner.
      PathAlign:
        aggregation_type: last
        scale: 1.0
      # Scores a trajectory based on how far it ends up from the path provided by the global planner.
      PathDist:
        aggregation_type: last
        scale: 1.0
      # Only allows the robot to rotate to the goal orientation when it is sufficiently close to the goal location
      RotateToGoal:
        scale: 1.0

      critics:
      - BaseObstacle
      #- ObstacleFootprint
      #- GoalAlign
      - GoalDist
      #- Oscillation
      - PathAlign
      # - PathDist
      # - RotateToGoal

      transform_tolerance: 0.2
      prune_plan: True
      prune_distance: 1.0
      debug_trajectory_details: True
      trajectory_generator_name: "dwb_plugins::LimitedAccelGenerator"
      goal_checker_name: "dwb_plugins::SimpleGoalChecker"

      short_circuit_trajectory_evaluation: True
      trans_stopped_velocity: 0.25
      slowing_factor: 5.0
      lookahead_time: -1.5
      stateful: True

      xy_goal_tolerance: 0.05
      yaw_goal_tolerance: 0.25
      latch_xy_goal_tolerance: true

      # Published params
    publish_cost_grid_pc: false
    publish_evaluation: true
    publish_global_plan: true
    publish_local_plan: true
    publish_trajectories: true
    publish_transformed_plan: true


    # Controller
    controller_frequency: 30.0
    controller_plugin_types: ["dwb_core::DWBLocalPlanner"]
    controller_plugin_ids: ["FollowPath"]
    min_theta_velocity_threshold: 0.05
    min_x_velocity_threshold: 0.006
    min_y_velocity_threshold: 1.0

[SteveMacenski]

to kind of summarize that, you were able to fix it with tuning the controller?

Can we close this then?

[3wnbr1]

I would like to understand why the trajectories don't converge to the goal pose but somewhere else. Then it will be just tuning and I will consider this closed. For now, I wasn't able to fix it

Maybe I'm just missing some new param

[SteveMacenski]

#938

Try not using these plugins with known issues.

I can’t tell you that DWB works on getting low tolerances, because frankly I haven’t tested that, but I have tested <20cm with the standard params we give in the config files and that works without an issue. My feeling its a mix of that bug and your configuration.

[3wnbr1]

Thanks for your feedback, I will try to investigate the underlying issue and test with the original config. I'll post updates tomorrow.

[3wnbr1]

After some testing, this strange behavior was due to PathAlign critic. As the underlying issue falls back to #938, I'm closing this.