dwb_local_planner gives negative trajectories

[dschnabel]

I think there's something configured not quite right in my navigation stack, specifically in my local planner. If I set a goal 1 meter in front of the robot, then the global plan looks correct (the trajectory pointing in a straight line towards the goal) but the local plan points in the opposite direction. The robot starts backing away from the goal and does random rotations. It's not following the global plan at all and crashes into obstacles.

I would like to know why I'm getting trajectories that point away from my goal. Where would I start to look for the problem?

Used configuration for local planner:

DWBLocalPlanner:
  update_costmap_before_planning: true
  prune_plan: true
  prune_distance: 1.0
  short_circuit_trajectory_evaluation: true
  debug_trajectory_details: false

  trajectory_generator_name: dwb_plugins::LimitedAccelGenerator
  # velocities
  min_vel_x: -0.3
  max_vel_x: 0.3
  min_vel_y: 0.0
  max_vel_y: 0.0
  max_vel_theta: 2.0
  # acceleration
  acc_lim_x: 0.357823171
  acc_lim_y: 0.0
  acc_lim_theta: 8.731380878
  # deceleration
  decel_lim_x: -1.20849956
  decel_lim_y: 0.0
  decel_lim_theta: -10.948097518
  # absolute speeds (in either direction)
  min_speed_xy: 0.1
  max_speed_xy: 0.3
  min_speed_theta: 0.3

  goal_checker_name: dwb_plugins::StoppedGoalChecker
  trans_stopped_velocity: 0.1
  rot_stopped_velocity: 0.3

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

  # RotateToGoal critic
  RotateToGoal:
    xy_goal_tolerance: 0.25
    trans_stopped_velocity: 0.25
    slowing_factor: 5.0
    lookahead_time: -1.0

  # Oscillation critic
  Oscillation:
    x_only_threshold: 0.05
    oscillation_reset_dist: 0.05
    oscillation_reset_angle: 0.2
    oscillation_reset_time: -1.0

  # ObstacleFootprint critic
  ObstacleFootprint:
    footprint: [[0.11,0.15],[0.11,-0.15],[-0.27,-0.15],[-0.27,0.15]]
    sum_scores: false

  # GoalAlign critic
  GoalAlign:
    forward_point_distance: 0.325

  # PathAlign critic
  PathAlign:
    forward_point_distance: 0.325

[dschnabel]

I noticed that my navigation stack was using 100% CPU and frequently missed update loops. This was because I had built all my packages in debug mode without optimization (CMAKE_BUILD_TYPE=Debug). After I rebuilt everything in release mode (CMAKE_BUILD_TYPE=Release) the CPU usage went down and no more missed update loops.

This also improved the path given by dwb_local_planner, so that my robot is now driving forward most of the time and more closely follows the global path. However, occasionally the robot stops and drives backwards bumping into obstacles. I need to further investigate and will update my findings here. Until then I'll leave the issue open.

[dschnabel]

Fiddled a lot with this lately. I had to make a few code changes to the PreferForwardCritic and also stopped using the ObstacleFootprintCritic because it's using too much CPU on my Raspberry Pi 4.

Instead I added a mechanism to the dwb_local_planner to force recalculating a new global plan if there's an obstacle detected in the costmap which is on or too close to the global plan. This is giving me pretty good driving behavior.

Anyway, closing this issue as the mentioned code changes give me the results I was looking for. If interested I can share the code but not sure how helpful it would be to others.