The dwa_local_planner is trying to go to global path's back parts

[cosmicog]

Here is video, I'm using dwa_local_planner and it confuses at near to goal position and some other locations. Especially, when goal point's X and footprint center's X are close, it always confuses.

Here is Planner's other crazy movement video.

rqt_console says at struggling:

...
Velocity -0.600, 0.000, 0.442 discarded by cost function  0 with cost: -5.000000
Velocity -0.600, 0.000, 0.316 discarded by cost function  0 with cost: -5.000000
Velocity -0.600, 0.000, 0.189 discarded by cost function  0 with cost: -5.000000
Velocity -0.600, 0.000, 0.063 discarded by cost function  0 with cost: -5.000000
Evaluated 62 trajectories, found 32 valid`
Velocity 0.600, 0.000, 1.200 discarded by cost function  0 with cost: -5.000000
Velocity 0.600, 0.000, 1.074 discarded by cost function  0 with cost: -5.000000
Velocity 0.600, 0.000, 0.947 discarded by cost function  0 with cost: -5.000000
Velocity 0.600, 0.000, 0.821 discarded by cost function  0 with cost: -5.000000
Velocity 0.600, 0.000, 0.695 discarded by cost function  0 with cost: -5.000000
Velocity 0.600, 0.000, 0.568 discarded by cost function  0 with cost: -5.00000
...

I've replaced footprint to square but the result didn't change. Dear @DLu, @mikeferguson, still no answer / comment on this 4 days old ROS Answers thread. Is it bug or wrong parameter setting? Thank you!

Here is parameters:

DWAPlannerROS:

  # Robot Configuration Parameters
  acc_lim_x: 10.0 # (double, default: 2.5) The x acceleration limit of the robot in meters/sec^2
  acc_lim_y: 0.0 # (double, default: 2.5)
  acc_lim_theta: 20.0 # (double, default: 3.2)
  acc_limit_trans: 1.0 # 

  max_trans_vel: 0.6 # (double, default: 0.55)
  min_trans_vel: 0.02 # (double, default: 0.1)
  trans_stopped_vel: 0.02 # 
  rot_stopped_vel: 0.02 # 

  max_vel_x: 0.6 # (double, default: 0.55)
  min_vel_x: -0.6 # (double, default: 0.0)

  max_vel_y: 0.0 # (double, default: 0.1)
  min_vel_y: 0.0 # (double, default: -0.1)

  max_rot_vel: 1.2 # (double, default: 1.0)
  min_rot_vel: 0.02 # (double, default: 0.4)

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.01 # (double, default: 0.05) 
  xy_goal_tolerance: 0.07 # (double, default: 0.10)
  latch_xy_goal_tolerance: false # (bool, default: false)

  # Forward Simulation Parameters
  sim_time: 0.5
  sim_granularity: 0.05
  angular_sim_granularity: 0.05
  vx_samples: 3 # (integer, default: 3)
  vy_samples: 1 # (integer, default: 10)
  vtheta_samples: 20 # (integer, default: 20)
  # controller_frequency: 10.0 # (double, default: 20.0)

  # Trajectory Scoring Parameters
  path_distance_bias: 32.0 # (double, default: 32.0)
  goal_distance_bias: 24.0 # (double, default: 24.0)
  occdist_scale: 0.01 # (double, default: 0.01)
  # pdist_scale: 0.75  
  # gdist_scale: 0.8 
  forward_point_distance: 0.0 # 2.0 # 0.325 
  stop_time_buffer: 0.2 # (double, default: 0.2)
  scaling_speed: 0.25 # (double, default: 0.25)
  max_scaling_factor: 0.2 # (double, default: 0.2) 
  use_dwa: true

  #Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05 # (double, default: 0.05)
  oscillation_reset_angle: 0.05 # 
  prune_plan: false # (bool, default: true) 

TrajectoryPlannerROS:
  yaw_goal_tolerance: 0.01
  acc_lim_th: 10.0
  max_rotational_vel: 1.2
  min_in_place_rotational_vel: -1.2
  holonomic_robot: false
  meter_scoring: true

[cosmicog]

I found temporal solution when I was poking rqt_reconfigure :)

~planner_frequency (double, default: 0.0)

The rate in Hz at which to run the global planning loop. If the frequency is set to 0.0, the global planner will only run when a new goal is received or the local planner reports that its path is blocked. New in navigation 1.6.0

Changed /move_base/planner_frequency from 0.0 to 10.0.

Now It doesn't stuck with its new plan. But I think It's still an issue.

[hu7241]

Which build is your dwa, 1.13?

[cosmicog]

No, 1.12.7, indigo, but I've tried 1.13.1 too.

[QinZiwen]

My terminal show that DWA planner failes to produce path. What are there possible reason? Thank you very much!

[haochihlin]

Hi,

I also faced the same issue, so I tried to trace related source codes and found something probably are the causes of the problem. But I am not sure, so maybe someone in here could give me some suggestions?

My opinion (or guessing)： the trajectory created by DWA are scoring based on 6 critics, one of it is the distance between current robot pose and local goal. And the local goal is set by the intersection of bounds of "local-cost-map" and global path (could be proven in "map_grid.cpp"). Hence, in the video shown above (also in my case), the local cost map is too big that the local goal is the same as the global goal. Generally, this situation is not a big deal, but when the global path contains "U-turn" shape, it causes problem. When the robot is moving along the U-turn segment and "crossing" the local goal projected point, DWA trajectory generator can not find the best scored result since in this moment, the goal is behind the robot, so it should move backward, but when the robot move a bit back, the goal is now in front of it. This "back and forth" phenomenon can be easily observed by visualizing the DWA-local path. To overcome this issue (if my hypothesis was not wrong), the fast way is to narrow the area of "local cost map". In my case it was working when the local goal does not locate on the U turn segment.

[QinZiwen]

@jim1993 I don't know what is "U-turn" shap?

[RicoJia]

Hi,

I also faced the same issue, so I tried to trace related source codes and found something probably are the causes of the problem. But I am not sure, so maybe someone in here could give me some suggestions?

My opinion (or guessing)： the trajectory created by DWA are scoring based on 6 critics, one of it is the distance between current robot pose and local goal. And the local goal is set by the intersection of bounds of "local-cost-map" and global path (could be proven in "map_grid.cpp"). Hence, in the video shown above (also in my case), the local cost map is too big that the local goal is the same as the global goal. Generally, this situation is not a big deal, but when the global path contains "U-turn" shape, it causes problem. When the robot is moving along the U-turn segment and "crossing" the local goal projected point, DWA trajectory generator can not find the best scored result since in this moment, the goal is behind the robot, so it should move backward, but when the robot move a bit back, the goal is now in front of it. This "back and forth" phenomenon can be easily observed by visualizing the DWA-local path. To overcome this issue (if my hypothesis was not wrong), the fast way is to narrow the area of "local cost map". In my case it was working when the local goal does not locate on the U turn segment.

Thanks to @haochihlin, this is EXACTLY my issue. Awesome explanation!

I tried lowering goal_dist_cost and increasing path_distance_cost, but in short, this do not work. I found out that goal_dist_cost can't be too close to 0 (otherwise the planner might face multiple paths considered "optimal" as they are all close to the global path. Jacking up path_dist_cost alone does not work in my case either.

So, decrease the local costmap size is the best option IMO.

[940222JESUS]

My terminal show that DWA planner failes to produce path. What are there possible reason? Thank you very much!

Hey I have a similar situation. Issue. Could you tell me How did you resolve it?