Add search helper for goal angle tolerance

[renan028]

Description

Adding a class to search the space around the input goal and return the closest pose free of obstacles that respect both linear and angular tolerances. During exploration, eight neighboring cells are added in a priority queue. By default, the cells are ordered by euclidean distance wrt to the start pose, but one can add a different strategy in the constructor. The search stops when it finds a cell where the robot can be placed (footprint + padding + safety distance) without colliding with obstacles. For each cell, we test multiple angles.

Example

In the figure below, the navigation goal is A (the blue dot) and the linear tolerance is the circle. The orientation of the goal is the Y-axis. The red polygons are static obstacles (e.g. walls), orange polygons are dynamic obstacles (e.g. chairs, tables, boxes) and gray polygons are unknown space. The problem is to find the closest free space to the goal that the robot can be, i.e., its footprint is not in collision with any obstacle and the distance is equal or above a given clearance distance.

The goal orientation is not a valid solution:

But we can find a solution using the angle tolerance:

[RainerKuemmerle]

@renan028 @corot The tests fail on melodic. Is this something to investigate?

[corot]

@renan028 @corot The tests fail on melodic. Is this something to investigate?

On it

btw, @RainerKuemmerle, can you also review the service interface we are implementing here? PR #316 Thanks!

[corot]

@renan028 @corot The tests fail on melodic. Is this something to investigate?

just realized that melodic EOL was 3 days ago!

But anyway, we investigated the problem. Looks like the test costmap is not getting initialized with the map published on /map topic, but we didn't find out why yet. In any case, it's clearly not an issue with the new service.