Cost functions are the heaviest part of the planning process. If the number of candidates is small, the cost of splitting and merging threads is not worth it. But when the number of candidates is above a threshold, a thread split approach could potentially accelerate the overall computation time.

Changes to the Code

In GSServer.py:

...
traffic.stop_all()
dashboard.quit()

print("Tick Count: %d" % sync_global.tick_count)
...

In TickSync.py:

...
def clock_log_avg_delta(label):
    total = 0.0
    count = 0.0

    for log in TickSync._logs:
        if log[0] == label:
            total += float(log[2])
            count += 1.0

    return total / count

def clock_log_min_delta(label):
    min_delta = 100.0

    for log in TickSync._logs:
        if log[0] == label:
            min_delta = min(min_delta, float(log[2]))

    return min_delta

def clock_log_max_delta(label):
    max_delta = 0.0

    for log in TickSync._logs:
        if log[0] == label:
            max_delta = max(max_delta, float(log[2]))

    return max_delta
...

In SDVPlanner.py:

def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
        #TickSync.print_clock_log()

        print("VID: %d, AVG: %f, MIN: %f, MAX: %f" % (
            self.vid, TickSync.clock_log_avg_delta("Cost"),
            TickSync.clock_log_min_delta("Cost"), TickSync.clock_log_max_delta("Cost")
        ))
...

Tests

For each test, I ran scenarios/test_scenarios/gs_ringroad_stress_loop.osm once and scenarios/test_scenarios/gs_ringroad_single.osm once
The timeout for these scenarios was changed to 60 seconds
In each case, the delta time is between TickSync.clock_log("Feas") and TickSync.clock_log("Cost"), which is the time it would take to calculate the maneuver_cost() for all feasible trajectories
In each case, the output was reordered
Note: the reason that MIN is 0.0000 in some cases is probably because there were no feasible trajectories to compute the cost for
Note: for these tests, TRAJECTORY_SPLIT = 10

Test 1: No Thread Splitting

In ManeuverModels.py:

def optimized_trajectory(vehicle_state:VehicleState, target_state_set,
...
TickSync.clock_log("Feas")

for ft in feasible:
    maneuver_cost(ft, mconfig, lane_config, vehicles, pedestrians, static_objects)
TickSync.clock_log("Cost")
...

Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.015834, MIN: 0.001800, MAX: 0.057400
VID:  2, AVG: 0.015936, MIN: 0.003200, MAX: 0.049600
VID:  3, AVG: 0.018189, MIN: 0.001500, MAX: 0.058300
VID:  4, AVG: 0.017787, MIN: 0.004800, MAX: 0.091100
VID:  5, AVG: 0.032214, MIN: 0.014200, MAX: 0.086700
VID:  6, AVG: 0.018434, MIN: 0.003600, MAX: 0.050100
VID:  7, AVG: 0.017259, MIN: 0.000000, MAX: 0.070500
VID:  8, AVG: 0.017175, MIN: 0.000000, MAX: 0.054300
VID:  9, AVG: 0.019608, MIN: 0.003300, MAX: 0.060900
VID: 10, AVG: 0.035449, MIN: 0.014500, MAX: 0.085600
Tick Count: 1011
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID:  1, AVG: 0.015482, MIN: 0.013300, MAX: 0.034400
Tick Count: 1797
...

Test 2: Thread Splitting and Joining (With Two Threads)

In ManeuverModels.py:


def calc_maneuver_cost(feasible, mconfig, lane_config, vehicles, pedestrians, static_objects):
for ft in feasible:
    maneuver_cost(ft, mconfig, lane_config, vehicles, pedestrians, static_objects)

def optimized_trajectory(vehicle_state:VehicleState, target_state_set, ... TickSync.clock_log("Feas")

feasible_len = int(len(feasible) / 2)
feasible_1 = feasible[0:feasible_len]
feasible_2 = feasible[feasible_len:]

thread_g1 = Thread(
    target=calc_maneuver_cost,
    args=(feasible_1, mconfig, lane_config, vehicles, pedestrians, static_objects,)
)
thread_g2 = Thread(
    target=calc_maneuver_cost,
    args=(feasible_2, mconfig, lane_config, vehicles, pedestrians, static_objects,)
)

thread_g1.start()
thread_g2.start()

thread_g1.join()
thread_g2.join()

feasible = feasible_1 + feasible_2

# for ft in feasible:
#     maneuver_cost(ft, mconfig, lane_config, vehicles, pedestrians, static_objects)
TickSync.clock_log("Cost")

...

- Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm ... VID: 1, AVG: 0.026596, MIN: 0.003100, MAX: 0.083800 VID: 2, AVG: 0.027520, MIN: 0.004300, MAX: 0.070500 VID: 3, AVG: 0.031093, MIN: 0.001900, MAX: 0.070100 VID: 4, AVG: 0.027002, MIN: 0.000200, MAX: 0.075200 VID: 5, AVG: 0.042351, MIN: 0.015200, MAX: 0.094400 VID: 6, AVG: 0.032170, MIN: 0.005100, MAX: 0.094300 VID: 7, AVG: 0.029945, MIN: 0.000800, MAX: 0.083300 VID: 8, AVG: 0.032017, MIN: 0.000200, MAX: 0.083400 VID: 9, AVG: 0.032877, MIN: 0.005000, MAX: 0.083200 VID: 10, AVG: 0.046807, MIN: 0.018600, MAX: 0.104800 Tick Count: 1009 ... python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm ... VID: 1, AVG: 0.018007, MIN: 0.013900, MAX: 0.037600 Tick Count: 1797 ...


### Test 3: Spawning Threads Once, Using Semaphores to Join (With Two Threads)
- In `SDVPlanner.py`:
```py
    def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
        start_sems = [Semaphore(0), Semaphore(0)]
        join_sems = [Semaphore(0), Semaphore(0)]

        optimized_trajectory_args = {
            "feasible": [None, None, None], "start_sems": start_sems,
            "join_sems": join_sems, "mconfig" : None,
            "lane_config": None, "vehicles": None,
            "pedestrians": None, "static_objects": None
        }

        thread_g1 = Thread(
            target=calc_maneuver_cost,
            args=(optimized_trajectory_args, 0,)
        )
        thread_g2 = Thread(
            target=calc_maneuver_cost,
            args=(optimized_trajectory_args, 1,)
        )

        thread_g1.start()
        thread_g2.start()

        while sync_planner.tick():
...
                frenet_traj, cand = plan_maneuver(self.vid, 
                                            mconfig,
                                            planner_state.vehicle_state,
                                            planner_state.lane_config,
                                            planner_state.traffic_vehicles,
                                            planner_state.pedestrians,
                                            planner_state.static_objects,
                                            optimized_trajectory_args)
...

In ManeuverModels.py:


def calc_maneuver_cost(optimized_trajectory_args, thread_num):
while True:
    optimized_trajectory_args["start_sems"][thread_num].acquire()

    for ft in optimized_trajectory_args["feasible"][thread_num]:
        maneuver_cost(
            ft, optimized_trajectory_args["mconfig"],
            optimized_trajectory_args["lane_config"], optimized_trajectory_args["vehicles"],
            optimized_trajectory_args["pedestrians"], optimized_trajectory_args["static_objects"]
        )

    optimized_trajectory_args["join_sems"][thread_num].release()

def optimized_trajectory(vehicle_state:VehicleState, target_state_set, mconfig, lane_config:LaneConfig, vehicles, pedestrians, static_objects, optimized_trajectory_args, s_solver): ... TickSync.clock_log("Feas")

feasible_len = int(len(feasible) / 2)
optimized_trajectory_args["feasible"][0] = feasible[0:feasible_len]
optimized_trajectory_args["feasible"][1] = feasible[feasible_len:]

optimized_trajectory_args["mconfig"] = mconfig
optimized_trajectory_args["lane_config"] = lane_config
optimized_trajectory_args["vehicles"] = vehicles
optimized_trajectory_args["pedestrians"] = pedestrians
optimized_trajectory_args["static_objects"] = static_objects

for start_sem in optimized_trajectory_args["start_sems"]:
    start_sem.release()

for join_sem in optimized_trajectory_args["join_sems"]:
    join_sem.acquire()

feasible = optimized_trajectory_args["feasible"][0] + optimized_trajectory_args["feasible"][1]

# for ft in feasible:
#     maneuver_cost(ft, mconfig, lane_config, vehicles, pedestrians, static_objects)
TickSync.clock_log("Cost")

...

- Note: `optimized_trajectory_args` is passed from `plan_maneuver()` to each of the `plan_` functions, and it is then passed to `optimized_trajectory()`
- Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm ... VID: 1, AVG: 0.015159, MIN: 0.003800, MAX: 0.037000 VID: 2, AVG: 0.014943, MIN: 0.001700, MAX: 0.060800 VID: 3, AVG: 0.017243, MIN: 0.000100, MAX: 0.052100 VID: 4, AVG: 0.016482, MIN: 0.005400, MAX: 0.037400 VID: 5, AVG: 0.030815, MIN: 0.015600, MAX: 0.065500 VID: 6, AVG: 0.017014, MIN: 0.005000, MAX: 0.048100 VID: 7, AVG: 0.015013, MIN: 0.000100, MAX: 0.046200 VID: 8, AVG: 0.016162, MIN: 0.000100, MAX: 0.037500 VID: 9, AVG: 0.015826, MIN: 0.000100, MAX: 0.041000 VID: 10, AVG: 0.033290, MIN: 0.018100, MAX: 0.072200 Tick Count: 1001 ... python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm ... VID: 1, AVG: 0.019804, MIN: 0.013800, MAX: 0.035500 Tick Count: 1797 ...


### Test 4: Spawning Threads Once, Using Semaphores to Join (With Three Threads)
- Same as **Test 3**, but using three threads instead of two
- Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm ... VID: 1, AVG: 0.014014, MIN: 0.002400, MAX: 0.036700 VID: 2, AVG: 0.014691, MIN: 0.004500, MAX: 0.039100 VID: 3, AVG: 0.016856, MIN: 0.000100, MAX: 0.050700 VID: 4, AVG: 0.015567, MIN: 0.003000, MAX: 0.036400 VID: 5, AVG: 0.030182, MIN: 0.015900, MAX: 0.061800 VID: 6, AVG: 0.015909, MIN: 0.003000, MAX: 0.043500 VID: 7, AVG: 0.015426, MIN: 0.000100, MAX: 0.045100 VID: 8, AVG: 0.016846, MIN: 0.000100, MAX: 0.053200 VID: 9, AVG: 0.015101, MIN: 0.000100, MAX: 0.043600 VID: 10, AVG: 0.031577, MIN: 0.017200, MAX: 0.061200 Tick Count: 1019 ... python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm ... VID: 1, AVG: 0.020634, MIN: 0.014400, MAX: 0.039100 Tick Count: 1797 ...


## Results
- For `scenarios/test_scenarios/gs_ringroad_stress_loop.osm`:


- For `scenarios/test_scenarios/gs_ringroad_single.osm`:

VID 1 Test 1|AVG: 0.015482, MIN: 0.013300, MAX: 0.034400 Test 2|AVG: 0.018007, MIN: 0.013900, MAX: 0.037600 Test 3|AVG: 0.019804, MIN: 0.013800, MAX: 0.035500 Test 4|AVG: 0.020634, MIN: 0.014400, MAX: 0.039100

The following tests are the same as Tests, but with TRAJECTORY_SPLIT = 100

Tests

Test 1: No Thread Splitting

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.037097, MIN: 0.006700, MAX: 0.102800
VID:  2, AVG: 0.034364, MIN: 0.000000, MAX: 0.087900
VID:  3, AVG: 0.032423, MIN: 0.000000, MAX: 0.148900
VID:  4, AVG: 0.022147, MIN: 0.000000, MAX: 0.115400
VID:  5, AVG: 0.055380, MIN: 0.034100, MAX: 0.121300
VID:  6, AVG: 0.039916, MIN: 0.008300, MAX: 0.106000
VID:  7, AVG: 0.033899, MIN: 0.000000, MAX: 0.104700
VID:  8, AVG: 0.034224, MIN: 0.000000, MAX: 0.127000
VID:  9, AVG: 0.029630, MIN: 0.000000, MAX: 0.083200
VID: 10, AVG: 0.055129, MIN: 0.029700, MAX: 0.100900
Tick Count: 816
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID: 1, AVG: 0.020121, MIN: 0.015900, MAX: 0.045600
Tick Count: 1796
...

Test 2: Thread Splitting and Joining (With Two Threads)

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.051258, MIN: 0.011000, MAX: 0.112100
VID:  2, AVG: 0.044626, MIN: 0.007600, MAX: 0.095200
VID:  3, AVG: 0.049083, MIN: 0.004400, MAX: 0.133900
VID:  4, AVG: 0.039131, MIN: 0.000200, MAX: 0.122300
VID:  5, AVG: 0.066083, MIN: 0.031400, MAX: 0.113200
VID:  6, AVG: 0.053194, MIN: 0.013400, MAX: 0.127200
VID:  7, AVG: 0.048594, MIN: 0.002000, MAX: 0.097100
VID:  8, AVG: 0.050905, MIN: 0.000200, MAX: 0.141900
VID:  9, AVG: 0.040758, MIN: 0.000200, MAX: 0.118500
VID: 10, AVG: 0.063171, MIN: 0.032100, MAX: 0.110600
Tick Count: 859
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID:  1, AVG: 0.024937, MIN: 0.018800, MAX: 0.050500
Tick Count: 1796
...

Test 3: Spawning Threads Once, Using Semaphores to Join (With Two Threads)

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.030389, MIN: 0.000100, MAX: 0.074600
VID:  2, AVG: 0.027098, MIN: 0.005700, MAX: 0.103900
VID:  3, AVG: 0.028612, MIN: 0.000100, MAX: 0.118900
VID:  4, AVG: 0.023697, MIN: 0.000100, MAX: 0.052800
VID:  5, AVG: 0.045559, MIN: 0.034900, MAX: 0.092100
VID:  6, AVG: 0.031750, MIN: 0.006200, MAX: 0.091000
VID:  7, AVG: 0.028844, MIN: 0.003100, MAX: 0.085500
VID:  8, AVG: 0.028329, MIN: 0.000100, MAX: 0.096900
VID:  9, AVG: 0.023258, MIN: 0.000100, MAX: 0.070100
VID: 10, AVG: 0.047359, MIN: 0.031800, MAX: 0.098800
Tick Count: 784
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID:  1, AVG: 0.026345, MIN: 0.019300, MAX: 0.051200
Tick Count: 1796
...

Test 4: Spawning Threads Once, Using Semaphores to Join (With Three Threads)

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.028407, MIN: 0.000100, MAX: 0.061900
VID:  2, AVG: 0.026510, MIN: 0.007200, MAX: 0.061200
VID:  3, AVG: 0.028310, MIN: 0.000100, MAX: 0.101200
VID:  4, AVG: 0.020652, MIN: 0.000100, MAX: 0.068000
VID:  5, AVG: 0.043454, MIN: 0.033900, MAX: 0.086400
VID:  6, AVG: 0.030124, MIN: 0.008700, MAX: 0.084100
VID:  7, AVG: 0.027326, MIN: 0.000100, MAX: 0.086900
VID:  8, AVG: 0.027077, MIN: 0.000100, MAX: 0.097500
VID:  9, AVG: 0.023071, MIN: 0.000100, MAX: 0.054900
VID: 10, AVG: 0.044071, MIN: 0.030300, MAX: 0.118900
Tick Count: 773
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID:  1, AVG: 0.027017, MIN: 0.019600, MAX: 0.044300
Tick Count: 1797
...

Results

For scenarios/test_scenarios/gs_ringroad_stress_loop.osm:

VID| Test 1                                      | Test 2                                      | Test 3                                      | Test 4
1 | AVG: 0.037097, MIN: 0.006700, MAX: 0.102800 | AVG: 0.051258, MIN: 0.011000, MAX: 0.112100 | AVG: 0.030389, MIN: 0.000100, MAX: 0.074600 | AVG: 0.028407, MIN: 0.000100, MAX: 0.061900
2 | AVG: 0.034364, MIN: 0.000000, MAX: 0.087900 | AVG: 0.044626, MIN: 0.007600, MAX: 0.095200 | AVG: 0.027098, MIN: 0.005700, MAX: 0.103900 | AVG: 0.026510, MIN: 0.007200, MAX: 0.061200
3 | AVG: 0.032423, MIN: 0.000000, MAX: 0.148900 | AVG: 0.049083, MIN: 0.004400, MAX: 0.133900 | AVG: 0.028612, MIN: 0.000100, MAX: 0.118900 | AVG: 0.028310, MIN: 0.000100, MAX: 0.101200
4 | AVG: 0.022147, MIN: 0.000000, MAX: 0.115400 | AVG: 0.039131, MIN: 0.000200, MAX: 0.122300 | AVG: 0.023697, MIN: 0.000100, MAX: 0.052800 | AVG: 0.020652, MIN: 0.000100, MAX: 0.068000
5 | AVG: 0.055380, MIN: 0.034100, MAX: 0.121300 | AVG: 0.066083, MIN: 0.031400, MAX: 0.113200 | AVG: 0.045559, MIN: 0.034900, MAX: 0.092100 | AVG: 0.043454, MIN: 0.033900, MAX: 0.086400
6 | AVG: 0.039916, MIN: 0.008300, MAX: 0.106000 | AVG: 0.053194, MIN: 0.013400, MAX: 0.127200 | AVG: 0.031750, MIN: 0.006200, MAX: 0.091000 | AVG: 0.030124, MIN: 0.008700, MAX: 0.084100
7 | AVG: 0.033899, MIN: 0.000000, MAX: 0.104700 | AVG: 0.048594, MIN: 0.002000, MAX: 0.097100 | AVG: 0.028844, MIN: 0.003100, MAX: 0.085500 | AVG: 0.027326, MIN: 0.000100, MAX: 0.086900
8 | AVG: 0.034224, MIN: 0.000000, MAX: 0.127000 | AVG: 0.050905, MIN: 0.000200, MAX: 0.141900 | AVG: 0.028329, MIN: 0.000100, MAX: 0.096900 | AVG: 0.027077, MIN: 0.000100, MAX: 0.097500
9 | AVG: 0.029630, MIN: 0.000000, MAX: 0.083200 | AVG: 0.040758, MIN: 0.000200, MAX: 0.118500 | AVG: 0.023258, MIN: 0.000100, MAX: 0.070100 | AVG: 0.023071, MIN: 0.000100, MAX: 0.054900
10 | AVG: 0.055129, MIN: 0.029700, MAX: 0.100900 | AVG: 0.063171, MIN: 0.032100, MAX: 0.110600 | AVG: 0.047359, MIN: 0.031800, MAX: 0.098800 | AVG: 0.044071, MIN: 0.030300, MAX: 0.118900

For scenarios/test_scenarios/gs_ringroad_single.osm:

VID 1
Test 1|AVG: 0.020121, MIN: 0.015900, MAX: 0.045600
Test 2|AVG: 0.024937, MIN: 0.018800, MAX: 0.050500
Test 3|AVG: 0.026345, MIN: 0.019300, MAX: 0.051200
Test 4|AVG: 0.027017, MIN: 0.019600, MAX: 0.044300

This test is for computing cost and feasibility in different threads
Both tests include the same changes as Changes to the Code

Tests

For each test, I ran scenarios/test_scenarios/gs_ringroad_stress_loop.osm once and scenarios/test_scenarios/gs_ringroad_single.osm once
The timeout for these scenarios was changed to 60 seconds
In SimConfig.py, TRAJECTORY_SPLIT = 100

Test 5: No Thread Splitting

In sv/ManeuverModels.py:

def optimized_trajectory(vehicle_state:VehicleState, target_state_set,
...
TickSync.clock_log("PolynFit")

#evaluate feasibility
#New: holding a data object for trajectories to hold and share partial costs across multiple cost functions 
frenet_trajectories = []
feasible = []
for traj, target_state in zip(trajectories, target_state_set):
    ft = FrenetTrajectory()
    ft.set_trajectory(traj[0],traj[1],target_state[2] )
    ft.start_state = start_state
    ft.target_state = np.concatenate([target_state[0],target_state[1]])
    frenet_trajectories.append(ft)
    if maneuver_feasibility(ft, mconfig, lane_config, vehicles, pedestrians, static_objects):
        ft.feasible = True
        feasible.append(ft)
#TickSync.clock_log("Feas")

for ft in feasible:
    maneuver_cost(ft, mconfig, lane_config, vehicles, pedestrians, static_objects)
TickSync.clock_log("Cost")
...

Notice that the delta time for "Cost" is now between "PolynFit" and "Cost"

Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID:  1, AVG: 0.144635, MIN: 0.077500, MAX: 0.232900
VID:  2, AVG: 0.138820, MIN: 0.068000, MAX: 0.299900
VID:  3, AVG: 0.143649, MIN: 0.060500, MAX: 0.291500
VID:  4, AVG: 0.130966, MIN: 0.062000, MAX: 0.281900
VID:  5, AVG: 0.251883, MIN: 0.158200, MAX: 0.404400
VID:  6, AVG: 0.154230, MIN: 0.079800, MAX: 0.278100
VID:  7, AVG: 0.153476, MIN: 0.070500, MAX: 0.309600
VID:  8, AVG: 0.152150, MIN: 0.062000, MAX: 0.348000
VID:  9, AVG: 0.139431, MIN: 0.060100, MAX: 0.304600
VID: 10, AVG: 0.248448, MIN: 0.166100, MAX: 0.483700
Tick Count: 827
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm
...
VID:  1, AVG: 0.095507, MIN: 0.082500, MAX: 0.145600
Tick Count: 1797
...

Test 6: Thread Splitting With Three Threads

In sv/SDVPlanner.py:

def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
    start_sems = [Semaphore(0), Semaphore(0), Semaphore(0)]
    join_sems = [Semaphore(0), Semaphore(0), Semaphore(0)]

    evaluate_trajectories_args = {
        "start_state": None, "target_state_set": [None, None, None],
        "mconfig" : None, "lane_config": None,
        "vehicles": None, "pedestrians": None,
        "static_objects": None, "trajectories": [None, None, None],
        "frenet_trajectories": [None, None, None], "feasible": [None, None, None],
        "start_sems": start_sems, "join_sems": join_sems
    }

    thread_g1 = Thread(
        target=evaluate_trajectories,
        args=(evaluate_trajectories_args, 0,)
    )
    thread_g2 = Thread(
        target=evaluate_trajectories,
        args=(evaluate_trajectories_args, 1,)
    )
    thread_g3 = Thread(
        target=evaluate_trajectories,
        args=(evaluate_trajectories_args, 2,)
    )

    thread_g1.start()
    thread_g2.start()
    thread_g3.start()
...

In sv/ManeuverModels.py:


def evaluate_trajectories(args, thread_num):
while True:
    args["start_sems"][thread_num].acquire()

    frenet_trajectories = []
    feasible = []
    for traj, target_state in zip(args["trajectories"][thread_num], args["target_state_set"][thread_num]):
        ft = FrenetTrajectory()
        ft.set_trajectory(traj[0],traj[1],target_state[2] )
        ft.start_state = args["start_state"]
        ft.target_state = np.concatenate([target_state[0],target_state[1]])
        frenet_trajectories.append(ft)
        if maneuver_feasibility(
            ft, args["mconfig"],
            args["lane_config"], args["vehicles"],
            args["pedestrians"], args["static_objects"]
        ):
            ft.feasible = True
            feasible.append(ft)
    # TickSync.clock_log("Feas")

    for ft in feasible:
        maneuver_cost(
            ft, args["mconfig"],
            args["lane_config"], args["vehicles"],
            args["pedestrians"], args["static_objects"]
        )

    args["feasible"][thread_num] = feasible
    args["frenet_trajectories"][thread_num] = frenet_trajectories

    args["join_sems"][thread_num].release()

def optimized_trajectory(vehicle_state:VehicleState, target_state_set, ... TickSync.clock_log("Opt Traj Start")

find trajectories

trajectories = []
traj_solver = lambda x: find_trajectory(x, s_solver)
trajectories = list(map(traj_solver, zip(itertools.repeat(start_state), target_state_set)))
TickSync.clock_log("PolynFit")

evaluate_trajectories_args["mconfig"] = mconfig
evaluate_trajectories_args["lane_config"] = lane_config
evaluate_trajectories_args["vehicles"] = vehicles
evaluate_trajectories_args["pedestrians"] = pedestrians
evaluate_trajectories_args["static_objects"] = static_objects
evaluate_trajectories_args["start_state"] = start_state

trajectories_len = int(len(trajectories) / 3)
evaluate_trajectories_args["trajectories"][0] = trajectories[0:trajectories_len]
evaluate_trajectories_args["trajectories"][1] = trajectories[trajectories_len:2*trajectories_len]
evaluate_trajectories_args["trajectories"][2] = trajectories[2*trajectories_len:]

evaluate_trajectories_args["target_state_set"][0] = target_state_set[0:trajectories_len]
evaluate_trajectories_args["target_state_set"][1] = target_state_set[trajectories_len:2*trajectories_len]
evaluate_trajectories_args["target_state_set"][2] = target_state_set[2*trajectories_len:]

for start_sem in evaluate_trajectories_args["start_sems"]:
    start_sem.release()

for join_sem in evaluate_trajectories_args["join_sems"]:
    join_sem.acquire()

feasible = evaluate_trajectories_args["feasible"][0] + evaluate_trajectories_args["feasible"][1] + evaluate_trajectories_args["feasible"][2]
frenet_trajectories = evaluate_trajectories_args["frenet_trajectories"][0] + evaluate_trajectories_args["frenet_trajectories"][1] + evaluate_trajectories_args["frenet_trajectories"][2]

TickSync.clock_log("Cost")

...

- Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm ... VID: 1, AVG: 0.120812, MIN: 0.072700, MAX: 0.239500 VID: 2, AVG: 0.125407, MIN: 0.074400, MAX: 0.281200 VID: 3, AVG: 0.131270, MIN: 0.061200, MAX: 0.292100 VID: 4, AVG: 0.121815, MIN: 0.071100, MAX: 0.292900 VID: 5, AVG: 0.212390, MIN: 0.166200, MAX: 0.400000 VID: 6, AVG: 0.128022, MIN: 0.077200, MAX: 0.224600 VID: 7, AVG: 0.123909, MIN: 0.063600, MAX: 0.238400 VID: 8, AVG: 0.122528, MIN: 0.053000, MAX: 0.324500 VID: 9, AVG: 0.118923, MIN: 0.064000, MAX: 0.254200 VID: 10, AVG: 0.219853, MIN: 0.161900, MAX: 0.370600 Tick Count: 680 ... python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_single.osm ... VID: 1, AVG: 0.108793, MIN: 0.085900, MAX: 0.162400 Tick Count: 1797 ...


## Results
- For `scenarios/test_scenarios/gs_ringroad_stress_loop.osm`:

- The problem in this case is that in **Test 5**, the tick count is 827, but in **Test 6** it is only 680

- For `scenarios/test_scenarios/gs_ringroad_single.osm`:

VID 1 Test 5|AVG: 0.095507, MIN: 0.082500, MAX: 0.145600 Test 6|AVG: 0.108793, MIN: 0.085900, MAX: 0.162400


- In this case the tick count isn't affected

In Test 5 and Test 6, there was a problem where the total tick count of the main process (GSServer.py) was over 100 ticks higher in Test 5 than in Test 6
This only occured when running scenarios/test_scenarios/gs_ringroad_stress_loop.osm; when running scenarios/test_scenarios/gs_ringroad_single.osm, the total tick count was the same in both tests
The following tests were to see how long it takes to copy lists to and from evaluate_trajectories_args, and to see how long it takes all of the threads to run

Tests

Each test was only run on scenarios/test_scenarios/gs_ringroad_stress_loop.osm (with the timeout set to 60 seconds)
In SimConfig.py, TRAJECTORY_SPLIT = 100

Test 7: Only Main Planning Thread

In GSServer.py:

def start_server(args, m=MVelKeepConfig()):
...
max_delta_time = 0.0

while sync_global.tick():
    max_delta_time = max(max_delta_time, sync_global.delta_time)
...
print("Tick Count: %d" % sync_global.tick_count)
print("Max Delta Time: %f" % max_delta_time)
...

Test 8: Three Threads Per Planner

Includes the changes from Test 6
Includes the changes from Test 7

In sv.ManeuverModels.py:

def optimized_trajectory(vehicle_state:VehicleState, target_state_set,
...
evaluate_trajectories_args["target_state_set"][2] = target_state_set[2*trajectories_len:]

TickSync.clock_log("Copy Before Release")

for start_sem in evaluate_trajectories_args["start_sems"]:
    start_sem.release()

for join_sem in evaluate_trajectories_args["join_sems"]:
    join_sem.acquire()

TickSync.clock_log("Cost")

feasible = evaluate_trajectories_args["feasible"][0] + evaluate_trajectories_args["feasible"][1] + evaluate_trajectories_args["feasible"][2]
frenet_trajectories = evaluate_trajectories_args["frenet_trajectories"][0] + evaluate_trajectories_args["frenet_trajectories"][1] + evaluate_trajectories_args["frenet_trajectories"][2]

TickSync.clock_log("Copy After Release")
...

"Copy Before Release" is the time it takes to copy the arguments before the threads are released
"Cost" is the time it takes to run the threads
"Copy After Release" is the time it takes to copy the arguments after the threads have finished

In sv/SDVPlanner.py:

def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
        print("VID: %d, AVG Copy Before: %f, AVG Cost: %f, AVG Copy After: %f" % (
            self.vid, TickSync.clock_log_avg_delta("Copy Before Release"),
            TickSync.clock_log_avg_delta("Cost"), TickSync.clock_log_avg_delta("Copy After Release"),
        ))
...

Test 9: Two Threads Per Planner

Same changes as Test 8, but using two threads

Results

VID| Test 7        | Test 8                                                                  | Test 9
 1 | AVG: 0.148414 | AVG Copy Before: 0.000003, AVG Cost: 0.130650, AVG Copy After: 0.000000 | AVG Copy Before: 0.000004, AVG Cost: 0.132641, AVG Copy After: 0.000001
 2 | AVG: 0.144874 | AVG Copy Before: 0.000013, AVG Cost: 0.127374, AVG Copy After: 0.000005 | AVG Copy Before: 0.000050, AVG Cost: 0.135226, AVG Copy After: 0.000000
 3 | AVG: 0.150666 | AVG Copy Before: 0.000010, AVG Cost: 0.131672, AVG Copy After: 0.000000 | AVG Copy Before: 0.000089, AVG Cost: 0.141390, AVG Copy After: 0.000000
 4 | AVG: 0.138692 | AVG Copy Before: 0.000071, AVG Cost: 0.124329, AVG Copy After: 0.000005 | AVG Copy Before: 0.000005, AVG Cost: 0.123230, AVG Copy After: 0.000000
 5 | AVG: 0.257542 | AVG Copy Before: 0.000087, AVG Cost: 0.218434, AVG Copy After: 0.000000 | AVG Copy Before: 0.000009, AVG Cost: 0.233796, AVG Copy After: 0.000000
 6 | AVG: 0.158387 | AVG Copy Before: 0.000022, AVG Cost: 0.135455, AVG Copy After: 0.000001 | AVG Copy Before: 0.000002, AVG Cost: 0.139093, AVG Copy After: 0.000003
 7 | AVG: 0.154253 | AVG Copy Before: 0.000003, AVG Cost: 0.128031, AVG Copy After: 0.000000 | AVG Copy Before: 0.000003, AVG Cost: 0.135297, AVG Copy After: 0.000000
 8 | AVG: 0.156144 | AVG Copy Before: 0.000044, AVG Cost: 0.118554, AVG Copy After: 0.000000 | AVG Copy Before: 0.000027, AVG Cost: 0.138153, AVG Copy After: 0.000000
 9 | AVG: 0.152712 | AVG Copy Before: 0.000026, AVG Cost: 0.129708, AVG Copy After: 0.000003 | AVG Copy Before: 0.000001, AVG Cost: 0.129821, AVG Copy After: 0.000010
10 | AVG: 0.255017 | AVG Copy Before: 0.000030, AVG Cost: 0.237093, AVG Copy After: 0.000002 | AVG Copy Before: 0.000003, AVG Cost: 0.228959, AVG Copy After: 0.000014

Test 7 | Tick Count: 812, Max Delta Time: 0.261853
Test 8 | Tick Count: 661, Max Delta Time: 0.390551
Test 9 | Tick Count: 719, Max Delta Time: 0.336382

Conclusion

Most of the time is spent in the threads, as can be seen in "AVG Cost"
Copying does not seem to be the issue
The "Max Delta Time" increases quite a bit as the number of threads are increased
When viewing htop while GSServer.py was running these test, we noticed that all 8 cores were sitting around 100%
This might explain why the main thread is affected when we are running multiple threads per planner process: the cores are already maxed out
Also, as seen in Test 6, for example, the total tick count is not affected when running scenarios/test_scenarios/gs_ringroad_single.osm
This is probably because there is only one planner process (instead of ten, when running scenarios/test_scenarios/gs_ringroad_stress_loop.osm), and so running multiple threads from that process does not affect the main process
Since the decrease in time is only on the order of tens of milliseconds, and because multiple threads affect the main process, we've decided not to split the cost functions into threads for now

rodrigoqueiroz / geoscenarioserver

Split cost functions into threads (when needed) #28

Changes to the Code

Tests

Test 1: No Thread Splitting

Test 2: Thread Splitting and Joining (With Two Threads)

Tests

Test 1: No Thread Splitting

Test 2: Thread Splitting and Joining (With Two Threads)

Test 3: Spawning Threads Once, Using Semaphores to Join (With Two Threads)

Test 4: Spawning Threads Once, Using Semaphores to Join (With Three Threads)

Results

Tests

Test 5: No Thread Splitting

Test 6: Thread Splitting With Three Threads

find trajectories

Tests

Test 7: Only Main Planning Thread

Test 8: Three Threads Per Planner

Test 9: Two Threads Per Planner

Results

Conclusion