Add nonlinear part to obstacle cost to improve narrow gap

[howardcochran]

This solves a major problem when the robot is faced with a narrow gap
that the global planner wants to traverse. The previous straight-line
obstacle cost function does not work well when there is a sufficiently
close obstacle on the left and right that the straight lines overlap.

The sum of two straight lines with opposite slope is a horizontal line,
which has zero derivative. Thus, there is no net force from obstacles
pushing poses to the center of the gap. So other slopes in the
optimization graph tend to pull these poses onto an obstacle, causing the
planned trajectory to be rejected, resetting the planner from the global
plan. We can stay in an endless loop in this case.

Reducing the obstacle weight helps a little bit in this case because the
solver becomes more stable. However, this makes it hard to avoid turning
a corner of a non-round robot into a nearby obstacle because the
gentle straight line slope of the obstacle cost is insufficient to
overcome other forces.

The new solution is to add a nonlinear cost component that will:

1. Make the minimum cost point between left & right obstacles be
   in the middle.

2. Drive the cost very high as distance to obstacle aproaches zero. This
   helps prevent us from turning a corner into an obstacle. Instead,
   the solver will typically pull forward or backward in a slight
   turn until clear enough to turn the desired direction, much like a
   human driver would.

The new parameter, obstacle_cost_exponent, controls how nonlinear
the obstacle error function will be. The default is 1.0 which preserves
existing behavior. I have found that setting the exponent to 4.0 and
increasing weight_obstacle to 100 (from 10) leads to excellent
narrow gap performance on our robot.

This solves the following open issues:
#28
#16

[croesmann]

Thanks a lot for all the PRs :). I'll review them in a few days.

[croesmann]

Thanks for this PR. I reviewed the code and did some tests in simulation. We can merge this now and it's good to be deactivated by default in kinetic to not break any existing configurations.