BACKEND: Improve planning code

[TomCC7]

Task Description

The current vehicle planning & control code is a bit messy and hard to extend. As we are going to add new features and more vehicle behaviors, it's time to refactor and improve the planning code.

Subtasks:

• IMPORTANT: investigate design patterns and structures to enable flexibility
• check vehicle control part in scenario runner #69
• better trajectory generation from waypoints
• optimize steering_control #60
• change throttle control to state machine style
• investigate py_trees #73

[TomCC7]

Current control process

1. vehicle is following trajectory generated from waypoints
2. get steer angle from steering_control
3. determine if is turning, which will set different speed for the vehicle
4. get throttle based on
   • the target speed
   • vehicle in front of the following vehicle
   • the distance to a intersection

[TomCC7]

Possible refactor plan: decision tree for each vehicle

• following waypoint (vehicle in front too close?)
  • YES: want to change line(?)
    • YES: change line
    • NO: decrease speed
  • NO: approaching intersection?
    • YES: ignore light?
      • YES: keep move on
      • NO: slow down and stop at sign
    • NO: keep move on

[TomCC7]

Scenario runner's Control Procss

1. Parser load everything from file into a config object
2. config passed to Scenario object and loaded into behavior tree
3. each behavior is linked to some Atomic Behavior which executes corresponding command
4. tick everything (most importantly the behavior tree)

Parser

The parser is separated into several stages:

1. check version - check if the version is 1.0.0
2. load catalog - don't understand
3. set scenario name
4. set carla town
5. set actor information
6. validate everything necessary

Behaviors

It uses Behavior Tree and a corresponding lib called py-trees to compose scenarios. An example from scenario runner is displayed below:

[-] behavior
    (o) behavior
        (-) ParametersInit
            --> InitialParameterSettings Done?
            [-] OneShot
                (o) ParametersInit
                --> Mark Done
        (-) EnvironmentBehavior
            --> InitialEnvironmentSettings Done?
            [-] OneShot
                (o) EnvironmentBehavior
                    (-) ChangeWeather
                        --> InitialWeather Done?
                        [-] OneShot
                            --> ChangeWeather
                            --> Mark Done
                    (-) ChangeRoadFriction
                        --> InitRoadFriction Done?
                        [-] OneShot
                            --> ChangeRoadFriction
                            --> Mark Done
                --> Mark Done
        (-) InitBehaviour
            --> InitialActorSettings Done?
            [-] OneShot
                (o) InitBehaviour
                    [-] InitActoradversary
                        --> ChangeActorControl
                    [-] InitActorstanding
                        --> ChangeActorControl
                    [-] InitActorhero
                        --> ChangeActorControl
                --> Mark Done
        (o) OSCStories
            (-) MyStory
                --> >MyStory Done?
                [-] OneShot
                    (o) MyStory
                        [-] Act StartConditions and behaviours
                            (o) StartConditions Group
                                (o) StartConditions
                                    (o) Condition Group
                                        (-) OverallStartCondition
                                            --> Behavior>None>None>OverallStartCondition Done?
                                            [-] OneShot
                                                --> OverallStartCondition
                                                --> Mark Done
                                        (-) SimulationTimeCondition
                                            --> Behavior>None>None>StartTime Done?
                                            [-] OneShot
                                                --> SimulationTimeCondition
                                                --> Mark Done
                            (o) Maneuver + EndConditions Group
                                --> Maneuvers
                                    (o) Maneuvers
                                        (-) ManeuverSequence
                                            --> Behavior>ManeuverSequence Done?
                                            [-] ManeuverSequence
                                                [-] RepeatableBehaviour of Behavior>ManeuverSequence
                                                    --> ManeuverSequence
                                                        (o) ManeuverSequence
                                                            (-) Maneuver LaneChangeSimpleManeuver
                                                                --> Behavior>ManeuverSequence>LaneChangeSimpleManeuver Done?
                                                                [-] OneShot
                                                                    --> Maneuver LaneChangeSimpleManeuver
                                                                        (o) Maneuver LaneChangeSimpleManeuver
                                                                            (-) Event AdversaryAccelerates
                                                                                --> Behavior>ManeuverSequence>AdversaryAccelerates Done?
                                                                                [-] Event AdversaryAccelerates
                                                                                    (o) Parallel Condition Groups
                                                                                        (o) Condition Group
                                                                                            (-) StartCondition
                                                                                                --> Behavior>ManeuverSequence>AdversaryAccelerates>None>None>StartCondition Done?
                                                                                                [-] OneShot
                                                                                                    --> StartCondition
                                                                                                    --> Mark Done
                                                                                    --> Actions
                                                                                        (o) Actions
                                                                                            (-) AdversaryAccelerates
                                                                                                --> Behavior>ManeuverSequence>AdversaryAccelerates>AdversaryAccelerates>0 Done?
                                                                                                [-] OneShot
                                                                                                    --> AdversaryAccelerates
                                                                                                        --> AdversaryAccelerates
                                                                                                    --> Mark Done
                                                                                    --> Mark Done
                                                                            (-) Event AdversaryChangesLane
                                                                                --> Behavior>ManeuverSequence>AdversaryChangesLane Done?
                                                                                [-] Event AdversaryChangesLane
                                                                                    (o) Parallel Condition Groups
                                                                                        (o) Condition Group
                                                                                            (-) completeStateCondition
                                                                                                --> Behavior>ManeuverSequence>AdversaryChangesLane>None>None>AfterAdversaryAccelerates Done?
                                                                                                [-] OneShot
                                                                                                    --> completeStateCondition
                                                                                                    --> Mark Done
                                                                                    --> Actions
                                                                                        (o) Actions
                                                                                            (-) AdversaryChangesLane
                                                                                                --> Behavior>ManeuverSequence>AdversaryChangesLane>AdversaryChangesLane>0 Done?
                                                                                                [-] OneShot
                                                                                                    --> AdversaryChangesLane
                                                                                                        --> AdversaryChangesLane
                                                                                                    --> Mark Done
                                                                                    --> Mark Done
                                                                    --> Mark Done
                                                    --> Clear Descendant Variables of Behavior>ManeuverSequence
                                                --> Mark Done
                                (o) EndConditions
                                    (o) Condition Group
                                        (-) EndCondition
                                            --> Behavior>None>None>EndCondition Done?
                                            [-] OneShot
                                                --> EndCondition
                                                --> Mark Done
                    --> Mark Done
--> TimeOut
--> WeatherBehavior
--> UpdateAllActorControls
(o) EndConditions (Criteria Group)
    (o) Evaluation Criteria for multiple ego vehicles
        --> RunningStopTest
    (o) Evaluation Criteria for multiple ego vehicles
        --> RunningRedLightTest
    (o) Evaluation Criteria for multiple ego vehicles
        --> WrongLaneTest
    (o) Evaluation Criteria for multiple ego vehicles
        --> OnSidewalkTest
    (o) Evaluation Criteria for multiple ego vehicles
        --> CheckKeepLane
    (o) Evaluation Criteria for multiple ego vehicles
        --> CollisionTest
    (o) Evaluation Criteria for multiple ego vehicles
        --> CheckDrivenDistance