Fix matcWaypointsAndLanelets function

bug

Fixed bug

This MR fixes bug in matchWaypointAndLanelet function in lanelet2_extension package.

Required information:

• Operating system and version:
  • Ubuntu 16.04
• Autoware installation type:
  • From Source
• Autoware version or commit hash
  • master(2019/11/18)
• ROS distribution and version:
  • ROS Kinetic
• ROS installation type:
  • From Binary
• Package or library, if applicable:
  • lanelet2_extension

Description of the bug

The function matches waypoints and lanelets from geometry information and road connection information. First, it provides possible lanelet candidates for each waypoint from geometry information. At this point, waypoints at an intersection would have multiple candidates(since lanes are overlapping at intersections). Then, the function uses road connection information to eliminate impossible candidates by comparing lanelet candidates of previous and following waypoints.

However, current function has bug when comparing against lanelet candidates in following waypoints. This MR fix that bug.

Steps to reproduce the bug

1. Download following files: waypoints.csv autoware_mapping_example2.osm lanelet_tf.launch
2. Run roslaunch runtime_manager runtime_manager.launch
3. Go to "Map" tab and load autoware_mapping_example.osm and lanelet_tf.launch.
   
4. Go to Computing Tab and wf_simulator with default option.
   
5. Click [app] for vel_pose_connect, turn on "Simulation Mode", and launch vel_pose_connect.
   
6. start rviz
7. Set simulated position for wf_simulator using 2D Pose Estimate.
   

Start decision_maker

1. Launch decision_maker, run roslaunch decision_maker decision_maker.launch use_ll2:=true sim_mode:=true
2. Load waypoints by rosrun waypoint_maker waypoint_loader _multi_lane_csv:=/path/to/waypoints.csv based/lane_waypoints_raw:=based/lane_waypoints_array

Expected behavior

There is no warning message in output.

Actual behavior

decision_maker outputs following error:

[ WARN] [1574052378.515902830]: No lanelet was matched for waypoint with gid: 89