Hi, all. I found a bug for dwa local planner. When the goal is in the -x and -y direction with respect to robot's current pose, the robot moves really slowly, even stop.
It outputs "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot". This information is generated from here when the cost of computed path is smaller than zero.
Obviously, we can see that there is no obstacle around the robot at that time. This case can be repeated as long as we want.
The parameters are listed as follows:
DWAPlannerROS:
# Robot Configuration Parameters - Kobuki
max_vel_x: 0.3 # 0.55
min_vel_x: 0.0
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_trans_vel: 0.3 # choose slightly less than the base's capability
min_trans_vel: 0.04 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.1
max_rot_vel: 1.57 # default 5.0 # choose slightly less than the base's capability
min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.4
acc_lim_x: 1.0 # maximum is theoretically 2.0, but we
acc_lim_theta: 2.0
acc_lim_y: 0.0 # diff drive robot
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.05 # 0.05
xy_goal_tolerance: 0.05 # 0.10
# latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 1.7 #1.5 # 1.7
vx_samples: 20 # 3
vy_samples: 1 #1 # diff drive robot, there is only one sample
vtheta_samples: 40 # 20
# Trajectory Scoring Parameters
path_distance_bias: 32.0 # 32.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 12.0 # 24.0 - wighting for how much it should attempt to reach its goal
occdist_scale: 0.02 # 0.01 - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 #0.325 # 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj.
scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed.
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: odom
# Differential-drive robot configuration - necessary?
holonomic_robot: false
Very looking forward anyone can give me any advice to solve this problem! Thank you!
EDIT1:
Thank you! @David Lu I have updated the image with colored cost cloud. The trajectory cloud is not displayed because its topic output nothing suddenly. The topic of trajectory cloud output colored fan-shaped cloud in front of robot normally. However, as the robot cannot move forward, the fan-shaped cloud disappeared. I am not sure that the topic of cost cloud output normally because it has persistence in rviz. It is not updated until a new message arrives.
EDIT2: Thanks again! @David Lu I did some experiments as you advice. According to the source code of dwa_local_planner, there are six cost functions in total. The criteria of these cost functions are pushed into std::vector<base_local_planner::TrajectoryCostFunction*> critics. The summary are as follows:
if the three parameters (path_distance_bias, goal_distance_bias, occdist_scale) are all zero, the robot doesn't move. The debug information from base_local_planner is
[DEBUG] [1518406865.911248289, 682.570000000]: Evaluated 769 trajectories, found 769 valid
[DEBUG] [1518406865.917767600, 682.570000000]: Cost PointCloud published
if the three parameters are kept unchanged, the robot doesn't move. The debug information from base_local_planner is
[DEBUG] [1518396321.802246154, 1539.790000000]: Velocity 0.284, 0.000, 1.000 discarded by cost function 5 with cost: -2.000000
[DEBUG] [1518396321.802506417, 1539.790000000]: Velocity 0.300, 0.000, -1.000 discarded by cost function 5 with cost: -2.00000
Which means the goal cost function(goal_costs_) outputs negative value.
if path_distance_bias is set to be zero only, other two parameters are kept unchanged, the robot doesn't move. The debug information is
[DEBUG] [1518411651.529508502, 662.150000000]: Velocity 0.142, 0.000, 1.000 discarded by cost function 3 with cost: -2.000000
[DEBUG] [1518411651.530015908, 662.150000000]: Velocity 0.158, 0.000, -1.000 discarded by cost function 3 with cost: -2.00000
which means the alignment_costs_ function outputs negative value.
if occdist_scale is set to be zero only, other two parameters are kept unchanged, the robot doesn't move. The debug information is
Velocity 0.1420, 0.000, 1.000 discarded by cost function 4 with cost: -2.000000
Velocity 0.1580, 0.000, -1.000 discarded by cost function 4 with cost: -2.000000
which means the path_costs_ function outputs negative value.
if goal_distance_bias is set to be zero only, other two parameters are kept unchanged, the robot moves normally.
EDIT3
Hi, @David Lu, Following images are goal_cost, occ_cost, path_cost, x, y, z, repectively. The toal cost channel is displayed above.
Originally posted by scopus on ROS Answers with karma: 279 on 2018-01-23
Post score: 0
Original comments
Comment by David Lu on 2018-01-30:
Is the screenshot showing the local or global costmap?
Comment by scopus on 2018-01-30:
Thank you for your attention! It's global costmap. The local costmap is not being shown there. But I don't think this problem is relative to the costmap.
Comment by David Lu on 2018-01-30:
If there was an obstacle, it would be on the local costmap.
Comment by scopus on 2018-01-30:
Thank you! The environment of this experiment is in control, we are sure that there is no obstacle around the robot at that time.
Comment by scopus on 2018-01-31:
We found that the path.cost_ is smaller than zero which makes the dwa planner can't generate controlling velocity .
Comment by David Lu on 2018-02-05:
I would show the local costmap anyway. The most probable reasons for a negative score are obstacles and being off the costmap. It may also help to visualize the cost and trajectory clouds.
