weight the `map_grid` cost functions near the goal pose

[rayvburn]

MapGridCostFunction scores trajectories using the following pattern:

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);

    // translate point forward if specified
    if (xshift_ != 0.0) {
      px = px + xshift_ * cos(pth);
      py = py + xshift_ * sin(pth);
    }
    // translate point sideways if specified
    if (yshift_ != 0.0) {
      px = px + yshift_ * cos(pth + M_PI_2);
      py = py + yshift_ * sin(pth + M_PI_2);
    }

which translated each point of the trajectory according to the "shifts" specified. This can be problematic when approaching a goal near in front of the obstacle. Consider weighting those cost functions near the goal (N meters from the goal pose).

[rayvburn]

In fact, the X shift should be the set according to the radius of the mobile base.

[rayvburn]

Implemented the solution as in dwa_local_planner