DWAPlannerROS making spiral trajectory while following global path
Hi all,
I'm trying to configure dwa planner for a custom skid drive platform, but I'm unable to get a good behaviour of the planner. As it tries to follow the global path, the planner always chooses an almost circular trajectory, alternating forward and backward movements. As result, the platform ends following the path in kind of a spiral trajectory. This behaviour can be observed in the following video.
My initial guess was that it's trying to get as fast as possible to the path, so I've tried to reduce path_distance_bias while increasing goal_distance_bias. My hope was that it would tend more towards the goal and less to the path, thus, avoiding turning so hard. However, this change didn't show any improvement.
We need DWA, as TrajectoryPlannerROS is not able to make backwards trajectories and has a hard time achieving goals with our required precision.
Any help will be appreciated.
I'm using ROS Hydro on ubuntu 12.04. Relevant configuration files:
DWA params:
DWAPlannerROS:
acc_lim_x: 2.5
acc_lim_y: 0.0 # Not holonomic == 0
acc_lim_th: 3.2
max_trans_vel: 0.55
min_trans_vel: 0.1
max_vel_x: 0.55
min_vel_x: -0.55
max_vel_y: 0.0 # Not holonomic == 0
min_vel_y: 0.0 # Not holonomic == 0
max_rot_vel: 1.0
min_rot_vel: 0.3
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.05
xy_goal_tolerance: 0.10
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 1.7
sim_granularity: 0.025
vx_samples: 3
vy_samples: 0 # Not holonomic
vtheta_samples: 20
controller_frequency: 20.0
penalize_negative_x: false
# Trajectory Scoring Parameters
path_distance_bias: 0.1 #32.0
goal_distance_bias: 50 #24.0
occdist_scale: 0.1 #0.01
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Global Plan Parameters
prune_plan: true
move_base params
base_global_planner: navfn/NavfnROS
base_local_planner: dwa_local_planner/DWAPlannerROS
recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false
shutdown_costmaps: false
oscillation_timeout: 0.0
oscillation_distance: 0.5
planner_frequency: 0.5
[EDIT]
It seems that my problem was similar to the one reported in this other question. The bypass proposed there (increasing acc_lim to some very high values) also worked for me.
I also noticed that acc_lim_th seems to be ignored. acc_lim_theta is the correct paramether. It looks like the wiki documentation is outdated.
I'm using Theta Acceleration limit as 2x higher of X Acceleration limit. And Decreasing
sim_time
is little bit helping to increasing performance. Also decreasingsim_time
to1.0
helps in some short distance rotational cases.Actually I tested different
sim_time
values ranging from 1.0 up to 3.0 seconds. A longersim_time
seemed to alleviate a bit the reported behaviour, but was still present very frequently. Will test your suggestion for 2x theta acceleration. Thank you!