[fmrico]

Fixes #148

With a NavMap in the NavState, FusionLocalizer places its planar estimate on the map's surface. Before this change it reported z = 0 and a flat attitude everywhere.

Changes

• NavMapTerrain places (x, y, yaw) on a NavMap:
  • the height is the surface under (x, y);
  • on a multi-level map it keeps the level the robot is on; the first time, it takes the level nearest to the initial pose's height, or 0;
  • roll and pitch come from the IMU, or from the surface normal without one;
  • off the map, the last known height is kept.
• FusionLocalizer:
  • applies the placement to robot_pose and, through a new UkfWrapper::setOutputAdjuster() hook, to the filter's published odometry and map -> odom;
  • a new <plugin>.navmap_terrain parameter, true by default, turns it off;
  • an initial pose sets the height to start from.
• easynav_fusion_localizer now depends on navmap_core.

Without a NavMap, or with navmap_terrain: false, the output is unchanged.

Tests

• navmap_terrain_tests:
  • an empty map;
  • a flat surface with an IMU;
  • a ramp without an IMU, heading up it and across it;
  • following the height along a ramp;
  • two overlapping levels, with and without a height hint;
  • leaving the map and coming back;
  • reset.
• fusion_localizer_terrain_tests, the real localizer driving up a ramp:
  • z on the surface with the IMU's tilt;
  • the surface's tilt without an IMU;
  • unchanged without a NavMap;
  • turned off by the parameter.
• colcon test of the package passes, lint included.