Navfn - Potential Field Calculation

edit

I would like to use Navfn to let a robot follow a wall at a close distance. More precisely, I want Navfn to plan a path parallel to the wall at a given distance.

However, with the current implementation, this is not possible because Navfn is planning a path by calculating a potential field based on the global costmap and then using gradient descent on this potential field to find a path. Visualizing this potential field in rviz shows that obstacles have a big influence on the field and as a result, Navfn always plans paths that move first away from the wall and then come back just before reaching the goal.

Is this influence already present in the costmap or is it introduced in the calculation of the potential field? I am aware of the inflation radius in the costmap, but according to section 1.5 Inflation in the costmap_2d wiki page, a cell with a distance from an obstacle cell that is greater than the inflation radius has cost 0. This would mean that the influence of obstacles on the potential field is introduced in the potential field calculation.

How would the Navfn code have to be adjusted so that obstacles affect the potential field only up to a defined distance away from the obstacle. I would guess that this parameter has to be introduced in NavFn::updateCell, but I cannot see how to do that. Also I expect that a new array has to be added which keeps track of the distance to the closest occupied cell for each cell.

I hope somebody has enough insight into the code to help me with these questions or to explain me why it isn't possible.

Thanks in advance.

Update:

The problem I experienced came from the costmap, and to be more precise, from the inflation radius. I got fooled by the topic costmap/inflated_obstacles which, according to the documentation, publishes the cells in the costmap that correspond to the occupied cells inflated by the inscribed radius of the robot. But this is wrong! Playing around with the parameters inflation_radius and robot_radius shows that the topic publishes only occupied cells inflated by the smaller of either inflation_radius or robot_radius. Increasing the inflated_radius over the robot_radius won't affect the cells published by this topic. But the inflation_radius is still taken into account when calculating the potential field.

So in my case, the inflation_radius (0.5) was bigger than the robot_radius (0.25). The robot was approx 0.3 from the wall, which is outside the inflated obstacles, but still inside the inflation_radius, and the goal was set further along the wall at the same distance from the wall. This produced a curved path along the wall.

http://www.dobots.nl/documents/10157/322bd1b7-154f-4e93-b2c5-2b12a81f4be9

Setting the inflation_radius to the same value as robot_radius solved the problem and produced "straighter" paths along the wall.

http://www.dobots.nl/documents/10157/c7071173-4156-44ac-9768-fcf286c39797