rodrigoqueiroz / geoscenarioserver

8 stars 1 forks source link

Split cost functions into threads (when needed) #28

Closed rodrigoqueiroz closed 10 months ago

rodrigoqueiroz commented 3 years ago

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.

mchlswyr commented 3 years ago

Changes to the Code

Tests

Test 1: No Thread Splitting

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

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)
...

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`:

VID| Test 1 | Test 2 | Test 3 | Test 4 1 | AVG: 0.015834, MIN: 0.001800, MAX: 0.057400 | AVG: 0.026596, MIN: 0.003100, MAX: 0.083800 | AVG: 0.015159, MIN: 0.003800, MAX: 0.037000 | AVG: 0.014014, MIN: 0.002400, MAX: 0.036700 2 | AVG: 0.015936, MIN: 0.003200, MAX: 0.049600 | AVG: 0.027520, MIN: 0.004300, MAX: 0.070500 | AVG: 0.014943, MIN: 0.001700, MAX: 0.060800 | AVG: 0.014691, MIN: 0.004500, MAX: 0.039100 3 | AVG: 0.018189, MIN: 0.001500, MAX: 0.058300 | AVG: 0.031093, MIN: 0.001900, MAX: 0.070100 | AVG: 0.017243, MIN: 0.000100, MAX: 0.052100 | AVG: 0.016856, MIN: 0.000100, MAX: 0.050700 4 | AVG: 0.017787, MIN: 0.004800, MAX: 0.091100 | AVG: 0.027002, MIN: 0.000200, MAX: 0.075200 | AVG: 0.016482, MIN: 0.005400, MAX: 0.037400 | AVG: 0.015567, MIN: 0.003000, MAX: 0.036400 5 | AVG: 0.032214, MIN: 0.014200, MAX: 0.086700 | AVG: 0.042351, MIN: 0.015200, MAX: 0.094400 | AVG: 0.030815, MIN: 0.015600, MAX: 0.065500 | AVG: 0.030182, MIN: 0.015900, MAX: 0.061800 6 | AVG: 0.018434, MIN: 0.003600, MAX: 0.050100 | AVG: 0.032170, MIN: 0.005100, MAX: 0.094300 | AVG: 0.017014, MIN: 0.005000, MAX: 0.048100 | AVG: 0.015909, MIN: 0.003000, MAX: 0.043500 7 | AVG: 0.017259, MIN: 0.000000, MAX: 0.070500 | AVG: 0.029945, MIN: 0.000800, MAX: 0.083300 | AVG: 0.015013, MIN: 0.000100, MAX: 0.046200 | AVG: 0.015426, MIN: 0.000100, MAX: 0.045100 8 | AVG: 0.017175, MIN: 0.000000, MAX: 0.054300 | AVG: 0.032017, MIN: 0.000200, MAX: 0.083400 | AVG: 0.016162, MIN: 0.000100, MAX: 0.037500 | AVG: 0.016846, MIN: 0.000100, MAX: 0.053200 9 | AVG: 0.019608, MIN: 0.003300, MAX: 0.060900 | AVG: 0.032877, MIN: 0.005000, MAX: 0.083200 | AVG: 0.015826, MIN: 0.000100, MAX: 0.041000 | AVG: 0.015101, MIN: 0.000100, MAX: 0.043600 10 | AVG: 0.035449, MIN: 0.014500, MAX: 0.085600 | AVG: 0.046807, MIN: 0.018600, MAX: 0.104800 | AVG: 0.033290, MIN: 0.018100, MAX: 0.072200 | AVG: 0.031577, MIN: 0.017200, MAX: 0.061200


- 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

mchlswyr commented 3 years ago

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

mchlswyr commented 3 years ago

Tests

Test 5: No Thread Splitting

Test 6: Thread Splitting With Three Threads

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`:

VID| Test 5 | Test 6 1 | AVG: 0.144635, MIN: 0.077500, MAX: 0.232900 | AVG: 0.120812, MIN: 0.072700, MAX: 0.239500 2 | AVG: 0.138820, MIN: 0.068000, MAX: 0.299900 | AVG: 0.125407, MIN: 0.074400, MAX: 0.281200 3 | AVG: 0.143649, MIN: 0.060500, MAX: 0.291500 | AVG: 0.131270, MIN: 0.061200, MAX: 0.292100 4 | AVG: 0.130966, MIN: 0.062000, MAX: 0.281900 | AVG: 0.121815, MIN: 0.071100, MAX: 0.292900 5 | AVG: 0.251883, MIN: 0.158200, MAX: 0.404400 | AVG: 0.212390, MIN: 0.166200, MAX: 0.400000 6 | AVG: 0.154230, MIN: 0.079800, MAX: 0.278100 | AVG: 0.128022, MIN: 0.077200, MAX: 0.224600 7 | AVG: 0.153476, MIN: 0.070500, MAX: 0.309600 | AVG: 0.123909, MIN: 0.063600, MAX: 0.238400 8 | AVG: 0.152150, MIN: 0.062000, MAX: 0.348000 | AVG: 0.122528, MIN: 0.053000, MAX: 0.324500 9 | AVG: 0.139431, MIN: 0.060100, MAX: 0.304600 | AVG: 0.118923, MIN: 0.064000, MAX: 0.254200 10 | AVG: 0.248448, MIN: 0.166100, MAX: 0.483700 | AVG: 0.219853, MIN: 0.161900, MAX: 0.370600

- 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
mchlswyr commented 3 years ago

Tests

Test 7: Only Main Planning Thread

Test 8: Three Threads Per Planner

Test 9: Two Threads Per Planner

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