Clearing costmap to unstuck robot (3.000000m)
I've been banging my head against this navigation problem for days. It occurs under both ROS Indigo and ROS Hydro (Debian installs). And it occurs with both a Kobuki and a Segway RMP 210 both in Gazebo and the real world. At the moment, both robots are using a single planar laser scanner for obstacle detection mounted about 15cm off the ground. Also, I am not using a static map--both the local and global cost maps are using the odom frame for the global frame.
The issue is that when the robot attempts to pass through a narrow gap, such as a doorway, the global planner (NavfnROS) usually plans a reasonable path through the gap. The local planner (TrajectoryPlannerROS) then speeds the robot along the path toward the gap and some times the robot makes it through but at least 50% of the time, the robot stops either just before it enters the gap or even after it has entered the gap. The robot then rotates through small angles left and right and might even back up a bit. After 10 to 15 seconds, I see the warning:
Clearing costmap to unstuck robot (3.000000m)
on the move_base terminal. I've tried a gazillion combination of parameters including increasing the planner frequency, changing the map resolution, reducing the inflation radius to as small as 0.1 and nothing seems to eliminate the problem. In addition, I don't know where the number 3.000000m in the message above is coming from. Looking at the source code for clear_costmap_recovery.cpp this parameter is defined on line 61 as:
private_nh.param("reset_distance", reset_distance_, 3.0);
However, I cannot figure out how to redefine this parameter in my yaml files. For example, neither:
conservative_clear:
reset_distance: 1.0
nor
conservative_reset_dist: 1.0
changes the value of the parameter in the warning message above. Also, the following screen shot shows a typical situation where this stuck behavior occurs:
Notice how the global plan (thin green line) passes very close to the obstacle on the right. And this is after the robot has already been stuck for 10 seconds or so even though the planner frequency in this case was 5 Hz. For some reason, the robot just remains stuck instead of replanning a path to go more centrally through the gap.
So the more general question is this: what parameter tricks are people using to get their robots to move through narrow gaps like doorways?
Thanks!
patrick
You're not going to like this response, but the only thing I found that was effective was to reduce footprint size in the yaml file so it wasn't so worried about hitting the wall. It still shows the behavior, but not as much.
So go with large inflation radius and small footprint.
Thanks for the suggestion. It works great in Gazebo but I can't seem to get it working on a real robot (Kobuki), even if I shrink the footprint (radius) to 1/2 the real size and play with the inflation parameters up and down the scale.
So I can now get the Kobuki (0.35m diameter) to pass through a 0.54m gap using a robot_radius=0.10, inflation_radius=0.5, cost_scaling_factor=5.0, pdist_scale=0.6, gdist_scale=0.8, occdist_scale=0.1. I still get the "Clearing costmap" warning but the robot gets through the gap most of the time.
Just 1 suggestion (might or might not solve ur problem though): I saw the green line is pretty closed to the obstacle, maybe you can increase the inflation_radius of inflation_layer of global_costmap so that Navfn produces a better global plan which is far away from obstacles
(cont.) If the global plan is too closed to some obstacle, local planner will have a hard time finding the valid trajectory.
Thanks for the suggestion. It seems that the parameters I now have (listed above) also solves the global planning problem so I don't need separate inflation parameters for local and global costmaps.
@DavidN, I take that back. It looks like setting the global inflation radius larger (e.g. double) and reversing the pdist and gdist scale parameters (pdist_scale=0.8, gdist_scale=0.6) improves the chances of success.