Allow pedestrians to walk outside of defined lanelets and areas

[scott-larter]

The purpose of this PR is to allow pedestrians to walk outside of defined lanelets and areas without errors and ending the server. This was done by modifying the get_occupying_lanelet_by_participant() function in LaneletMap.py to return None if the non-vehicle agent is not within a lanelet. This return value is then checked by the pedestrian's planner in SPPlanner.py and handled accordingly.

get_occupying_lanelet_by_participant() was also cleaned up to show the intuitive cases for an agent being within/outside of a lanelet.

To test, run the pedestrian_scenarios/gs_intersection_single_pedestrian.osm scenario which has been modified so that the pedestrian is outside of any defined lanelets.

[scott-larter]

While the modified scenario is good for testing this PR, I would not merge this change: let's revert the scenario to the master version just before we merge this.

I ran all scenarios using the scritpt run_scenarios.bash:

1. `gs_intersection_redlight_follow.osm` the vehicles get stuck on red light and when it changes to green they don't drive

2. interestingly, `gs_intersection_redlight.osm` works as expected

Otherwise, everything works.

I've reverted the single pedestrian scenario back to the master version and pushed the change.

[WAEliasmith]

Note: this change seems to work, but doesn't fix the issues I was having with adding pedestrians to rr_xwalk_intersection_basic, as that scenario gives the same error as before, which is:

"SPPlanner.py", line 72, in plan_route
    closest_entry_dist = np.linalg.norm(list(xwalks_not_in_plan.values())[0] - self.route[-1])
IndexError: list index out of range

[mchlswyr]

gs_intersection_redlight_follow.osm the vehicles get stuck on red light and when it changes to green they don't drive

@mantkiew This is a known issue (but I don't think there is actually an issue for it). Sometimes the vehicles will drive, and other times they get stuck on the stop line.

[scott-larter]

I ran all scenarios using the scritpt run_scenarios.bash:

1. `gs_intersection_redlight_follow.osm` the vehicles get stuck on red light and when it changes to green they don't drive

2. interestingly, `gs_intersection_redlight.osm` works as expected

I tested gs_intersection_redlight_follow.osm 5 or 6 times and got a variety of behaviour. Most of the time, it runs as expected but sometimes neither vehicle starts again after the light turns green and sometimes only the leading vehicle starts and the follower stays behind.

This is strange behaviour and I can't reproduce it in master, but I imagine it is not related to this PR.

[scott-larter]

Note: this change seems to work, but doesn't fix the issues I was having with adding pedestrians to rr_xwalk_intersection_basic, as that scenario gives the same error as before, which is:

"SPPlanner.py", line 72, in plan_route
    closest_entry_dist = np.linalg.norm(list(xwalks_not_in_plan.values())[0] - self.route[-1])
IndexError: list index out of range

Good catch, I think this is an error in my code. This is happening because xwalks_not_in_plan is empty because the pedestrian has already selected all the crosswalks in the map (there's probably only one in this scenario). I'll fix this and push the changes to this PR.

[scott-larter]

Note: this change seems to work, but doesn't fix the issues I was having with adding pedestrians to rr_xwalk_intersection_basic, as that scenario gives the same error as before, which is:

"SPPlanner.py", line 72, in plan_route
    closest_entry_dist = np.linalg.norm(list(xwalks_not_in_plan.values())[0] - self.route[-1])
IndexError: list index out of range

My latest changes should fix this issue. I did some testing and it worked but let me know if it works for you.