Adding initial behavior trees for SP pedestrian behavior

[scott-larter]

Overview of changes:

• Initial pedestrian behavior tree integration
• Behaviour model and maneuver model files along with their config and util files added to sp/ folder
• SP pedestrians now have a btree file defined in the scenario osm which controls their behavior. The .btree files are in btrees/sp/
• Behaviour tree maneuvers (MStop, MVelKeep, MUpdateDestination) and conditions (reached_goal, in_crosswalk, etc.) implemented
• When loading lanelet2 map, it automatically adds participant tags (vehicles for road lanelets and pedestrians for crosswalk and walkway lanelets)
• New function (get_occupying_lanelet_by_participant) in mapping/LaneletMap.py to filter overlapping lanelets by the predefined allowed participants

There are current known issues with this btree integration which will be resolved in future smaller pull requests. The purpose of this PR is to merge the basic functionality with master since there are large changes and many new files added. Overall, it is in a working state that can be merged with master with no conflicts.

Current known issues:

• Issue with deceleration in SFM which cases the pedestrians to sometimes stray into area not defined by a lanelet resulting in an index out of range error
• Possible code, functions, and comments that were relevant to vehicles but not pedestrians that are leftover from reusing code. I will be continuously cleaning this up.

[rodrigoqueiroz]

@rodrigoqueiroz I wonder whether we should extract the generic BTreeDSL* files out of sv and sp into geoscenarioserver/btree-dsl level folder so that they can be reused? Also, Do we really have to have separate BTreeInterpreter.py for each agent?

I think we can share the same parser and move to a neutral location to be reused by all actors. Eventually, the pedestrian specific terms can be added to the language without affecting the vehicle parts.

The BehaviorModel and BehaviorTreeLeaves definitely need to be separate, since they are heavily vehicle based.

BTreeInterpreter.py can be adapted for both. But it depends on importing Leaves to instantiate the node objects. It is also the place where we planned to add behavior trees by code for advanced user (similar to loading scenarios from code in ScenarioSetup. So it needs a careful refactoring to break it down into two parts and reorganize dependencies.

[scott-larter]

Please merge master into this branch. There are some conflicts to resolve:

$ git merge master 
Auto-merging scenarios/pedestrian_scenarios/gs_intersection_redlight.osm
CONFLICT (content): Merge conflict in scenarios/pedestrian_scenarios/gs_intersection_redlight.osm
Auto-merging mapping/LaneletMap.py
Auto-merging SimConfig.py
CONFLICT (content): Merge conflict in SimConfig.py
Auto-merging ScenarioSetup.py
Automatic merge failed; fix conflicts and then commit the result.

The latest master has now been merged into this branch and I resolved a couple minor conflicts: config differences in SimConfig.py and updating the btree tag for the vehicle in my gs_intersection_redlight scenario from pedestrian_trees/drive.btree to drive.btree.

[scott-larter]

@rodrigoqueiroz I wonder whether we should extract the generic BTreeDSL* files out of sv and sp into geoscenarioserver/btree-dsl level folder so that they can be reused? Also, Do we really have to have separate BTreeInterpreter.py for each agent?

I think we can share the same parser and move to a neutral location to be reused by all actors. Eventually, the pedestrian specific terms can be added to the language without affecting the vehicle parts.

The BehaviorModel and BehaviorTreeLeaves definitely need to be separate, since they are heavily vehicle based.

BTreeInterpreter.py can be adapted for both. But it depends on importing Leaves to instantiate the node objects. It is also the place where we planned to add behavior trees by code for advanced user (similar to loading scenarios from code in ScenarioSetup. So it needs a careful refactoring to break it down into two parts and reorganize dependencies.

I agree with moving reused files to a central location but I figured this would need a separate PR. We can do this soon so we don't create more work for ourselves in the future but I think there are enough changes to justify a new pull request. However, I can start restructuring if we want to include it in this PR.

[mantkiew]

Actually, let's merge it as it is regarding the BTree DSL so that we have a working system. Then in another PR, we can do the refactoring to see what is common and how we can support BTree parsing for multiple agent types (I expect more agents in the future).

[scott-larter]

Could you please move all btrees from scenarios/trees to btrees/sp?

Yes, I had previously done this so the files in scenarios/trees/pedestrian_trees were old and not in use. I have now deleted these in my latest commit. Should I completely delete the scenarios/trees/pedestrian_trees/ directory since it's not in use anymore?

[mantkiew]

Could you please move all btrees from scenarios/trees to btrees/sp?

Ah, @scott-larter now I see the confusion. I'm sorry, it was ambiguous. I meant "move all pedestrian btrees from scenarios/trees". Please do not remove vehicle ones.

[scott-larter]

Just one small change.

I ran all of the test scenarios and the pedestrian scenarios with no problems. However, I noticed that the pedestrians move very slow -- 20+ seconds to cross the intersection. Also, in scenarios/pedestrian_scenarios/gs_intersection_xwalk_signal.osm, the signal changes multiple times while the pedestrian is crossing.

There are a few known issues with pedestrian motion which likely have to do with the issue with the current SFM calculations. These will be fixed and improved in future pull requests. The purpose of this PR is to get an initial working version of pedestrians using btrees into master so we can improve on it in smaller increments moving forward.