DWA local planner can't find a trajectory unless rotate recovery runs
I'm trying to explore a room with my differential drive robot in Gazebo, running ROS Noetic on a Ubuntu 20.04 VM. When I set a 2D nav goal in RViz the global planner finds a path, but the local planner (DWA) cannot find a trajectory. After a while the recovery behaviours run (first the clearing of the cost maps which doesn't help, and then the rotate recovery). After the rotate recovery runs (or even during), the DWA planner generally finds a path and drives the robot to the goal.
The debug log returns the following
[ INFO][1:00:19.385 PM NZST][/move_base]: Got new plan
[DEBUG][1:00:19.385 PM NZST][/move_base]: Nearest waypoint to <-3.054630, 1.505509> is <-3.054631, 1.505509>
[DEBUG][1:00:19.385 PM NZST][/move_base]: Received a transformed plan with 88 points.
[DEBUG][1:00:19.387 PM NZST][/move_base]: Evaluated 0 trajectories, found 0 valid
[DEBUG][1:00:19.388 PM NZST][/move_base]: Cost PointCloud published
[DEBUG][1:00:19.388 PM NZST][/move_base]: 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.
[ WARN][1:00:19.388 PM NZST][/move_base]: DWA planner failed to produce path.
[ WARN][1:00:19.483 PM NZST][/move_base]: Clearing both costmaps to unstuck robot (3.00m).
[ INFO][1:00:19.683 PM NZST][/move_base]: Got new plan
[DEBUG][1:00:19.683 PM NZST][/move_base]: Nearest waypoint to <-3.054630, 1.505561> is <-3.054630, 1.505544>
[DEBUG][1:00:19.683 PM NZST][/move_base]: Received a transformed plan with 88 points.
[DEBUG][1:00:19.683 PM NZST][/move_base]: Evaluated 0 trajectories, found 0 valid
[DEBUG][1:00:19.683 PM NZST][/move_base]: Cost PointCloud published
[DEBUG][1:00:19.683 PM NZST][/move_base]: 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.
[ WARN][1:00:19.683 PM NZST][/move_base]: DWA planner failed to produce path.
[ WARN][1:00:19.783 PM NZST][/move_base]: Rotate recovery behavior started.
[ INFO][1:00:26.883 PM NZST][/move_base]: Got new plan
[ INFO][1:00:26.983 PM NZST][/move_base]: Got new plan
[DEBUG][1:00:26.983 PM NZST][/move_base]: Nearest waypoint to <-3.052998, 1.496237> is <-3.052971, 1.496269>
[DEBUG][1:00:26.983 PM NZST][/move_base]: Received a transformed plan with 88 points.
[DEBUG][1:00:26.994 PM NZST][/move_base]: Evaluated 1008 trajectories, found 1008 valid
[DEBUG][1:00:26.995 PM NZST][/move_base]: Cost PointCloud published
[DEBUG][1:00:26.996 PM NZST][/move_base]: A valid velocity command of (-0.06, 0.00, 0.91) was found for this cycle.
[ INFO][1:00:27.083 PM NZST][/move_base]: Got new plan
[DEBUG][1:00:27.083 PM NZST][/move_base]: Nearest waypoint to <-3.054135, 1.493450> is <-3.054135, 1.493449>
[DEBUG][1:00:27.083 PM NZST][/move_base]: Received a transformed plan with 86 points.
[DEBUG][1:00:27.106 PM NZST][/move_base]: Evaluated 1420 trajectories, found 1420 valid
[DEBUG][1:00:27.106 PM NZST][/move_base]: Cost PointCloud published
[DEBUG][1:00:27.106 PM NZST][/move_base]: A valid velocity command of (-0.11, 0.00, 0.92) was found for this cycle.
[DEBUG][1:00:27.187 PM NZST][/move_base]: Nearest waypoint to <-3.056796, 1.485388> is <-3.054135, 1.493449>
[DEBUG][1:00:27.187 PM NZST][/move_base]: Received a transformed plan with 86 points.
[DEBUG][1:00:27.213 PM NZST][/move_base]: Evaluated 1552 trajectories, found 1552 valid
[DEBUG][1:00:27.214 PM NZST][/move_base]: Cost PointCloud published
[DEBUG][1:00:27.214 PM NZST][/move_base]: A valid velocity command of (-0.18, 0.00, 0.74) was found for this cycle.
Note that the planner indicates it evaluated 0 trajectories, but once the rotate recovery runs it happily finds about 1000 trajectories that are valid. When the robot starts off initially it is in a room that is 4 meters by 4 meters and the robot is placed in the middle of the room. There are no obstacles in the room.
My question: Does anybody know what is going on here, or can somebody tell me what the best way is to debug a situation like this. I've read the navigation guide and the guide here: https://kaiyuzheng.me/documents/navguide.pdf. I'm guessing there is something obvious but I've been staring at this for two weeks now and I'm not making any progress.
Environment and configuration
Output from rosnode list
/foxglovestudio_20824
/gazebo
/gazebo_gui
/joint_state_publisher
/move_base
/robot_state_publisher
/rosout
/rqt_gui_py_node_5311
/rviz
/scuttle_slam_gmapping
Note: NOT running AMCL. There is no static map, just running the robot exploring the map.
My launch file launches gazebo and the navigation stack (see: https://github.com/pvandervelde/scuttle_gazebo/blob/master/launch/scuttle_room_with_walls_1.launch)
The configuration files for navigation are:
costmap common parameters (shared between local and global)
obstacle_range: 3.0
raytrace_range: 3.5
footprint: [[-0.077, -0.220], [0.288, -0.220], [0.288, 0.220], [-0.077, 0.220]]
#robot_radius: 0.105
inflation_radius: 0.35
cost_scaling_factor: 3.0
local costmap
local_costmap:
# The global frame for the costmap to operate in.
global_frame: odom
# The name of the frame for the base link of the robot.
robot_base_frame: base_link
# The frequency in Hz for the map to be updated.
update_frequency: 10.0
# The frequency in Hz for the map to be publish display information.
publish_frequency: 10.0
# Specifies the delay in transform (tf) data that is tolerable in seconds.
# This parameter serves as a safeguard to losing a link in the tf tree while
# still allowing an amount of latency the user is comfortable with to exist in
# the system. For example, a transform being 0.2 seconds out-of-date may be tolerable,
# but a transform being 8 seconds out of date is not. If the tf transform between the
# coordinate frames specified by the global_frame and robot_base_frame parameters is
# transform_tolerance seconds older than ros::Time::now(), then the navigation stack
# will stop the robot.
transform_tolerance: 0.5
static_map: false
# Whether or not to use a rolling window version of the costmap. If the
# static_map parameter is set to true, this parameter must be set to false.
rolling_window: true
# The width of the map in meters.
width: 4
# The height of the map in meters.
height: 4
# The resolution of the map in meters/cell.
resolution: 0.05
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
static_layer:
unknown_cost_value: 0 # default: 0
lethal_cost_threshold: 100 # default: 100
map_topic: map # default: "map"
obstacle_layer:
combination_method: 1 #default 1, meaning we will combine the layers
observation_sources: laser_scan_sensor
track_unknown_space: false # Set to fals because the DWA local planner doesn't like unknown space
map_type: costmap
laser_scan_sensor:
sensor_frame: lidar_1 # default: ""
topic: scan # or /scan?
data_type: LaserScan # default: "PointCloud"
marking: true # default: true
clearing: true # default: false
inflation_layer:
inflation_radius: 0.35 # default: 0.55
Global costmap
global_costmap:
#The global frame for the costmap to operate in.
global_frame: map
# The name of the frame for the base link of the robot.
robot_base_frame: base_link
# The frequency in Hz for the map to be updated.
update_frequency: 10.0
# The frequency in Hz for the map to be publish display information.
publish_frequency: 10.0
# Specifies the delay in transform (tf) data that is tolerable in seconds.
# This parameter serves as a safeguard to losing a link in the tf tree while
# still allowing an amount of latency the user is comfortable with to exist in
# the system. For example, a transform being 0.2 seconds out-of-date may be tolerable,
# but a transform being 8 seconds out of date is not. If the tf transform between the
# coordinate frames specified by the global_frame and robot_base_frame parameters is
# transform_tolerance seconds older than ros::Time::now(), then the navigation stack
# will stop the robot.
transform_tolerance: 0.5
static_map: true
# Whether or not to use a rolling window version of the costmap. If the
# static_map parameter is set to true, this parameter must be set to false.
rolling_window: false
# The width of the map in meters.
width: 20
# The height of the map in meters.
height: 20
# The resolution of the map in meters/cell.
resolution: 0.05
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
static_layer:
unknown_cost_value: 0 # default: 0
lethal_cost_threshold: 100 # default: 100
map_topic: map # default: "map"
obstacle_layer:
combination_method: 1 #default 1, meaning we will combine the layers
observation_sources: laser_scan_sensor
track_unknown_space: true # Set to true because the global planner has 'allow_unknown' set to true
map_type: costmap
laser_scan_sensor:
sensor_frame: lidar_1 # default: ""
topic: scan # or /scan?
data_type: LaserScan # default: "PointCloud"
marking: true # default: true
clearing: true # default: false
inflation_layer:
inflation_radius: 0.35 # default: 0.55
global planner
GlobalPlanner:
# Specifies whether or not to allow the planner to create plans that traverse unknown space.
# NOTE: if you are using a layered costmap_2d costmap with a voxel or obstacle layer, you must also
# set the track_unknown_space param for that layer to be true, or it will convert all your unknown
# space to free space (which planner will then happily go right through).
allow_unknown: true
# A tolerance on the goal point for the planner. The planner will attempt to create a plan that is
# as close to the specified goal as possible but no further than default_tolerance away.
default_tolerance: 0.10
# Specifies whether or not to visualize the potential area computed via a PointCloud2.
visualize_potential: true
# If true, use dijkstra's algorithm. Otherwise, A*.
use_dijkstra: true
# If true, use the quadratic approximation of the potential. Otherwise, use a simpler calculation.
use_quadratic: true
# If true, create a path that follows the grid boundaries. Otherwise, use a gradient descent method.
use_grid_path: false
# If for some reason, you want global_planner to exactly mirror the behavior of navfn,
# set this to true (and use the defaults for the other boolean parameters)
old_navfn_behavior: false
# Lethal Cost (dynamic reconfigure)
lethal_cost: 253
# Neutral Cost (dynamic reconfigure)
neutral_cost: 50
# Factor to multiply each cost from costmap by (dynamic reconfigure)
cost_factor: 0.8
# Publish Potential Costmap (dynamic reconfigure)
publish_potential: true
# How to set the orientation of each point (None=0, Forward=1, Interpolate=2,
# ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6) (dynamic reconfigure)
orientation_mode: 0
# What window to use to determine the orientation based on the position
# derivative specified by the orientation mode (dynamic reconfigure)
orientation_window_size: 1
# Outlines the global costmap with lethal obstacles. For the usage of a
# non static (rolling window) global costmap this needs to be set to false
outline_map: false
dwa planner
DWAPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.45
min_vel_x: -0.45
max_vel_y: 0.0
min_vel_y: 0.0
max_rot_vel : 2.7
max_vel_theta : 2.7
min_rot_vel: 0.6
min_vel_theta: 0.6
# The velocity when robot is moving in a straight line
max_vel_trans: 0.45
min_vel_trans: 0.10
acc_lim_x: 0.50
acc_lim_y: 0.0
acc_lim_trans: 0.50
acc_lim_th: 2.7
acc_lim_theta: 2.7
# Goal Tolerance Parametes
xy_goal_tolerance: 0.10
yaw_goal_tolerance: 0.20
latch_xy_goal_tolerance: true
# Forward Simulation Parameters
sim_time: 2.5
vx_samples: 20
vy_samples: 0
vth_samples: 40
controller_frequency: 10.0
# Trajectory Scoring Parameters
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.3
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
Asked by petrik on 2022-03-19 05:00:39 UTC
Answers
From the circumstances, it appears that the local cost map is failing to be created.
I suggest that you try to run the local cost map in rviz while visualizing it. The recovery operation will clear the local cost map. And I think we can observe that the DWA planner runs after it is cleared.
If the cause is determined by the local cost map, I expect that trying the following will help identify the cause.
- Activate the local_costmap plugins one by one.
- At first, experiment with static_layer only.
- Next, experiment with static_layer and obstacle_layer.
Asked by miura on 2022-03-20 00:15:53 UTC
Comments
When you say experiment with the static_layer only, does that mean I have to provide a static map and then use that to drive-to-goal? Or do I keep the 3 layers in the global planner (which seems to work) and only have a static layer in the local planner?
Asked by petrik on 2022-03-20 23:11:10 UTC
When I display the local costmap in RViz I see the following: https://imgur.com/gallery/12fbYrA. It looks that the local costmap has something on it. Note that the information in the corners is for things that don't exist. They're artefacts of the lidar. The actual obstacles are the walls of the room and they are at least another square further out on all sides of the robot.
Asked by petrik on 2022-03-21 02:27:54 UTC
Additionally if I disable the rotate-recovery behaviour then the robot doesn't move and eventually the navigation stack gives up. So I don't think clearing the costmap actually fixes the problem. I think the rotate-recovery does something that snaps the robot out of its issue
Asked by petrik on 2022-03-21 02:44:20 UTC
When you say experiment with the static_layer only, does that mean I have to provide a static map and then use that to drive-to-goal? Or do I keep the 3 layers in the global planner (which seems to work) and only have a static layer in the local planner?
We suggest that the local planner (local cost map) be only a static layer. The global planner seems to be well done.
Asked by miura on 2022-03-21 07:51:18 UTC
When I display the local costmap in RViz I see the following: https://imgur.com/gallery/12fbYrA. It looks that the local costmap has something on it. Note that the information in the corners is for things that don't exist. They're artefacts of the lidar. The actual obstacles are the walls of the room and they are at least another square further out on all sides of the robot.
The ones out in the corners will be areas that cannot be traveled due to obstructions. For the local planner because it is a local cost map. If something that doesn't exist is being treated as an obstacle, then there could be a problem lurking there.
Asked by miura on 2022-03-21 07:52:03 UTC
Additionally if I disable the rotate-recovery behaviour then the robot doesn't move and eventually the navigation stack gives up. So I don't think clearing the costmap actually fixes the problem. I think the rotate-recovery does something that snaps the robot out of its issue
Perhaps when you stopped the rotational recovery, you also stopped clearing the cost map. navigation's standard recovery is illustrated below. http://wiki.ros.org/move_base#Expected_Robot_Behavior
You may want to try the service clear_costmaps to explicitly clear the cost map.
Asked by miura on 2022-03-21 07:52:32 UTC
Original comment:
Perhaps when you stopped the rotational recovery, you also stopped clearing the cost map. navigation's
standard recovery is illustrated below. http://wiki.ros.org/move_base#Expecte...
You may want to try the service clear_costmaps to explicitly clear the cost map.
I don't think so the log seemed to indicate that it cleared the costmap twice and then immediately failed, I think due to time-out. I did notice that when I did a rotate-recovery, before the rotation the local planner didn't seem to know the global plan, while after the rotation it did. I'll see if I can get some pictures of that later today
Asked by petrik on 2022-03-21 15:04:38 UTC
Comments
Note. I don't have enough rep yet to add pictures. Will upload some once I have enough rep.
Asked by petrik on 2022-03-19 05:01:29 UTC
I've uploaded a video of the behaviour here: https://www.youtube.com/watch?v=Nt9XyJHzfas&list=PLSB3dkFkgyWWY6Wr-5Q7BxVi8YF2wk_C_&index=1. From that video it looks that the DWA planner doesn't "get" the global path until the rotate recovery runs. Not sure why
Asked by petrik on 2022-03-26 05:11:10 UTC
HI,
I have nearly the same issue as you described above, so I would like to know if you solved it and if you did can you please let me know what was the solution. Thank you in advance.
Asked by astef on 2022-06-15 09:12:41 UTC
I haven't figured it out yet. I suspect that somewhere in the DWA planner it tries to work with the global plan but fails or it might not even get a valid path or something. I'd have to start looking at the code for the DWA planner and that hasn't been on my priority list yet. At the moment I'm thinking about switching to ROS2 so if the ROS2 DWA planner has the same issue I will have a look at it in more detail.
Asked by petrik on 2022-06-16 00:26:36 UTC
Okay, thank you for the fast response. I will do some more experimenting with the parameters and in case I find something and solve it I will let you know.
Asked by astef on 2022-06-16 03:54:35 UTC