rodrigoqueiroz / geoscenarioserver

8 stars 1 forks source link

The "Jumping Back" of Vehicles in Simulation #41

Closed mchlswyr closed 8 months ago

mchlswyr commented 3 years ago

Tests

The following script (run from anm_unreal_test_suite) was used for running the tests:
```
#!/bin/bash
```

GSS="${HOME}/anm_unreal_sim/submodules/geoscenarioserver" SIM="${HOME}/anm_unreal_sim/submodules/anm_sim_platform"

cleanup_and_exit() { killall python3.8 killall roslaunch

# Cleanup Shared Memory
ipcrm -M 0x0001e240 # 123456
ipcrm -M 0x00051877 # 333943

# Cleanup Semaphores
ipcrm -S 0x000549c5 # 346565
ipcrm -S 0x000e3e39 # 933433

exit 0

}

keyboard_int() { echo "" echo "Press ENTER to exit." }

trap keyboard_int SIGINT

source "${GSS}/catkin_ws/install/opt/ros/lanelet2/setup.bash" --extend python3.8 "${GSS}/GSServer.py" -s 'scenarios/test_scenarios/gs_ringroad_stress_loop.osm' & bash "${SIM}/scripts/launch.bash" 'ring_road_ccw' > simlog.log &

read -p "Press ENTER to exit." cleanup_and_exit



- In each test, some vehicles were removed from `scenarios/test_scenarios/gs_ringroad_stress_loop.osm`
- Each test was only run once, so they only give a feel for the performance
### Test 1: No Lag
- Removed all vehicles except for `vid: 8`
- Average gss delta time was 33.452395/1000 s (33.452 ms)
- Average gsclient delta time was 30.636478/1000 s (30.636 ms)
### Test 2: Slight Lag
- Removed all vehicles except for `vids: 6, 7, 8, 9, 10`
- Average gss delta time was 33.954446/1000 s (33.954 ms)
- Average gsclient delta time was 36.989265/1000 s (36.989 ms)
### Test 3: Noticable Lag
- Removed all vehicles except for `vids: 4, 5, 6, 7, 8, 9, 10`
- Average gss delta time was 42.989064/1000 s (42.989 ms)
- Average gsclient delta time was 56.858391/1000 s (56.858 ms)
### Test 4: Very Laggy
- Didn't remove any vehicles
- Average gss delta time was 67.004617/800 s (83.756 ms)
- Average gsclient delta time was 54.260967/800 s (67.826 ms)

## Issues
- The issue that we would like to focus on is the "jumping back" of the vehicles that is somewhat noticable in **Test 3**, and much more noticable in **Test 4**

mchlswyr commented 3 years ago

Notes

Note 1

After adding time.sleep(2) (block for 2 seconds) to run _planner_process() in sv/SDVPlanner.py:
```
while sync_planner.tick():
time.sleep(2)
```
the "jumping back" effect seemed to stop
This suggests that the planner is ticking too frequently (note that during these tests, PLANNER_RATE = 5 and TRAFFIC_RATE = 30 in SimConfig.py)

Note 2

Setting block=False in run _planner_process() in sv/SDVPlanner.py


...
log.info('PLANNER PROCESS START for Vehicle {}'.format(self.vid))

sync_planner = TickSync(rate=PLANNER_RATE, realtime=True, block=False, verbose=False, label="PP") ...

and removing `return False` in `tick()` in `TickSync.py`:
```py
...
if (drift<0):
    #Too fast. Need to chill.
    if (self.block):
        time.sleep(-drift)      #blocks diff if negative drift
        #self.print('sleep {:.3}'.format(drift))
    else:
        #self.print('skip {:.3}'.format(drift))
        pass#return False            #return false to skip
...

it is easy to notice the "jumping back" effect

Undoing those changes, and setting PLANNER_RATE = 0.5 in SimConfig.py, seems to remove the "jumping back" effect
Again, this suggests that the "jumping back" effect is caused by the planner running at too high of a frequency

Note 3

Removing interpolation from ReadServerState() in GSClient.cpp

...
if (server_framestat.tick_count == server_tick_count) 
{
//same tick, no new state
//Predict new state based on Unreal tick time
//gsvptr->vehicle_state.x  = gsvptr->vehicle_state.x + (gsvptr->vehicle_state.x_vel * deltaTime);
//gsvptr->vehicle_state.y  = gsvptr->vehicle_state.y + (gsvptr->vehicle_state.y_vel * deltaTime);
}
...

did not remove the "jumping back" effect

This means that the "jumping back" effect is not caused by the client reading the server faster than the server can update, interpolating too far ahead, and then getting a position further back (causing the "jumping back" effect) when the server finally does update

Note 4

The vehicle that is "jumping back" always seem to be the leading vehicle

mchlswyr commented 3 years ago

The first three notes suggest that the "jump back" effect happens on the server side instead of the client side
Here, I'll explain what I did to measure the "jump back" effect on the server side in order to confirm what is described in Notes

Measuring "Jump Back"

"Jump back" is when a gss vehicle, observed in the gsclient simulation, is driving in some direction, and then "teleports" to a position in the opposite direction
In other words, just before the vehicle teleports backwards, it is at old_pos, and then it teleports to new_pos, which is "behind" old_pos
"new_pos behind old_pos" means that the vehicle was headed in the direction described by yaw, and new_pos should have been "in front" of old_pos in this direction, but, instead, the vehicle headed in the opposite direction, described by new_yaw
If yaw and new_yaw are in the range [0, 360), then when "jump back" occurs, abs(yaw - new_yaw) is roughly 180 degrees
So, if abs(yaw - new_yaw) ~= 180, then "jump back" has occured; otherwise, new_yaw and yaw are assumed to be roughly the same (i.e., the vehicle kept heading in a similar direction as expected)

Measuring "Jump Back" in the Python Code

In the function compute_vehicle_state() in the file Vehicle.py, I made the following changes:
```
...
old_pos = np.array([self.state.x, self.state.y])
```

update sim state

self.state.x = x_vector[0] ... if self.reversing: heading *= -1 self.state.yaw = math.degrees(math.atan2(heading[1], heading[0])) % 360

new_pos = np.array([self.state.x, self.state.y])
delta_d = new_pos - old_pos
new_yaw = math.degrees(math.atan2(delta_d[0], delta_d[1])) % 360

yaw_diff = abs(self.state.yaw - new_yaw)
if yaw_diff > 178.2 and yaw_diff < 181.8: # if yaw_diff within 1% of 180
    self.freq += 1

sanity check

...

- `old_pos` is the position just before it is updated
- `self.state.yaw` is placed in the range [0, 360), and is the direction the vehicle is supposed to head in
- `new_pos` is the position after it was updated (after the comment `#update sim state`)
- `delta_d` can be thought of as the "delta distance" in the equation v = d/t, where v is a vector containing the speed in the x- and y-directions
- `new_yaw`, then, is calculated in the exact same way as `self.state.yaw`, but using the change in position (`delta_d`) instead of the velocity
- If the difference between the two yaws in within 1% of 180, then this is assumed to be the "jump back" effect
- A variable `self.freq` is added to each vehicle to keep track of how many times it "jumps back" during the simulation
- In the function `stop_all()` in the file `SimTraffic.py`, I added:
```py
...
for vid in self.vehicles:
    print("VID: %d, FREQ: %d" % (vid, self.vehicles[vid].freq))
    self.vehicles[vid].stop()
...

When the simulation ends, each vehicle's self.freq is printed, so the results can be seen at the end

Results

I used the following command to run each test:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm

The timeout was changed to 60 seconds
The scenario was run until it timed out
Each test was done once

Test 1

This test is to see the frequency of the "jump back" effect with the same conditions as Test 4

Results:

VID: 1, FREQ: 3
VID: 2, FREQ: 0
VID: 3, FREQ: 4
VID: 4, FREQ: 1
VID: 5, FREQ: 18
VID: 6, FREQ: 1
VID: 7, FREQ: 0
VID: 8, FREQ: 2
VID: 9, FREQ: 1
VID: 10, FREQ: 30

Notice that the leading vehicles (vid 5 and vid 10) have the highest frequency
This matches what was observed in Note 4

Test 2

The test was to see the frequency of the "jump back" effect with the same conditions as Note 1

Results:

VID: 1, FREQ: 0
VID: 2, FREQ: 0
VID: 3, FREQ: 0
VID: 4, FREQ: 0
VID: 5, FREQ: 0
VID: 6, FREQ: 0
VID: 7, FREQ: 0
VID: 8, FREQ: 0
VID: 9, FREQ: 0
VID: 10, FREQ: 0

There was not a single case of "jump back", which matches what was observed in Note 1

Test 3

This test was to see the frequency of the "jump back" effect with the same conditions as Note 2

Results:

VID: 1, FREQ: 5
VID: 2, FREQ: 3
VID: 3, FREQ: 2
VID: 4, FREQ: 10
VID: 5, FREQ: 51
VID: 6, FREQ: 3
VID: 7, FREQ: 1
VID: 8, FREQ: 9
VID: 9, FREQ: 7
VID: 10, FREQ: 26

There was a lot of "jump back" in leading vehicles (such as vid 5, and vid 10)
This matches what was observed in Note 2 and Note 4

Test 4

Setting PLANNER_RATE = 0.5, as was done in Note 2

Results:

VID: 1, FREQ: 0
VID: 2, FREQ: 0
VID: 3, FREQ: 0
VID: 4, FREQ: 0
VID: 5, FREQ: 0
VID: 6, FREQ: 0
VID: 7, FREQ: 0
VID: 8, FREQ: 0
VID: 9, FREQ: 0
VID: 10, FREQ: 0

This matches what was observed in Note 2

Test 5

Running test_scenarios/gs_ringroad_single.osm with the same changes as Note 2 and with a timeout of 60 seconds:
Results:
```
VID: 1, FREQ: 211
```

Test 6

Running test_scenarios/gs_ringroad_single.osm with PLANNER_RATE = 0.5, or with PLANNER_RATE = 5, and with a timeout of 60 seconds:
Results:
```
VID: 1, FREQ: 0
```

Conclusion

It looks like there are cases where the next position of the vehicle is "behind" its current one ('jump back"), and these cases occur more frequently when the PLANNER_RATE is increased, the performance decreased (i.e., lots of vehicles), and more frequently in leading vehicles

mchlswyr commented 3 years ago

The "jump back" effect occurs only when the variable plan in tick() (in Vehicle.py) is not None
In other words, if "jump back" occurs, then there is a new plan
The converse isn't true: there are many cases where the plan is changed and "jump back" doesn't occur

In Python

In addition to the changes described in Measuring "Jump Back" in the Python Code, I added the following, starting with Vehicle.py:

def tick(self, tick_count, delta_time, sim_time):
Vehicle.tick(self, tick_count, delta_time, sim_time)
self.plan_change = False
#Read planner
if self.sv_planner:
    plan = self.sv_planner.get_plan()
    if plan:
        self.plan_change = True
         if plan.t != 0:
...

and in compute_vehicle_state():

...
yaw_diff = abs(self.state.yaw - new_yaw)
if yaw_diff > 178.2 and yaw_diff < 181.8: # if yaw_diff within 1% of 180
self.freq += 1

if self.plan_change:
    self.count += 1
...

and in SimTraffic.py:

for vid in self.vehicles:
print("VID: %d, FREQ: %d, COUNT: %d" % (vid, self.vehicles[vid].freq, self.vehicles[vid].count))
self.vehicles[vid].stop()

For example:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID: 4, FREQ: 7, COUNT: 7
I0224 15:45:59.312332 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 4
VID: 3, FREQ: 4, COUNT: 4
I0224 15:45:59.312481 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 3
VID: 5, FREQ: 47, COUNT: 47
I0224 15:45:59.312552 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 5
VID: 10, FREQ: 42, COUNT: 42
I0224 15:45:59.312607 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 10
VID: 9, FREQ: 2, COUNT: 2
I0224 15:45:59.312659 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 9
VID: 8, FREQ: 3, COUNT: 3
I0224 15:45:59.312705 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 8
VID: 7, FREQ: 3, COUNT: 3
I0224 15:45:59.312748 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 7
VID: 6, FREQ: 15, COUNT: 15
I0224 15:45:59.312796 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 6
VID: 2, FREQ: 8, COUNT: 8
I0224 15:45:59.312844 10008 SDVPlanner.py:66] Terminate Planner Process - vehicle 2
VID: 1, FREQ: 4, COUNT: 4
...

In all cases of "jump back", the plan had also changed
P.S. "jump back" does not occur for only one type of maneuver: by passing mconfig.mkey.value along to the Vehicle() object, and printing it when there is "jump back", you will see that it can happen for 1 (M_VELKEEP) or 2 (M_FOLLOW)

mchlswyr commented 3 years ago

With the following changes to compute_vehicle_state() in Vehicle.py:
```
...
old_s = self.state.s
```

update frenet state

self.state.s = self.s_eq(time) ... if yaw_diff > 178.2 and yaw_diff < 181.8: self.freq += 1

if self.plan_change:
    self.count += 1

if old_s > self.state.s:
    self.s += 1

...

- You will find that `self.s == self.freq`
- In other words, if there is "jump back", then `old_s > self.state.s`; the converse isn't true
- `old_s > self.state.s` is what `direction_cost()` does in `CostFunctions.py`
- I've tried to run `direction_cost()` for every trajectory (in `maneuver_feasibility()`):
```py
...
elif c == 'max_long_jerk':
    if max_long_jerk_cost(trajectory, mconfig.max_long_jerk) > 0:
        return False
    #elif c == 'direction':
    #     if direction_cost(trajectory, start_state, target_state) > 0:
    #         return False
    else:
        raise NotImplementedError("Feasibility function {} not implemented".format(c))

if direction_cost(trajectory, start_state, target_state) > 0:
    return False

return True

But this doesn't decrease "jump back" at all: direction_cost() isn't catching what is being caught in compute_vehicle_state()
The feasibility of all the trajectories is determined using the state of the vehicle as it was when it was read in run_planner_process() in SDVPlanner.py, but some of these trajectories are unfeasible for the vehicle as it is when the trajectory is actually used in compute_vehicle_state()

mchlswyr commented 3 years ago

In SimTraffic.py, tick() first updates the state of all of the vehicles, then it writes this state to the shared memory, self.traffic_state_sharr
In many separate processes (one for each SDV type vehicle), run_planner_process() (in SDVPlanner.py) is reading the planner's corresponding vehicle's state in this shared memory and planning the next trajectory based on this state
Back in SimTraffic.py, when tick() ticks each of the vehicles, the vehicles get the new trajectory (if a new one is available) from their corresponding planner process
Ideally, the planner process gives a new plan to a given vehicle based on the vehicle's last state
For example, if SimTraffic.py updates the shared memory with the vehicles' new state when tick_count is 200, and the planner process for vid: 10 reads this new state, then in the next tick, when tick_count is 201, the vehicle with vid: 10 should have the plan that was just created based on the state from tick_count: 200
Otherwise, if the vehicle doesn't receive this new plan at tick_count: 201, but instead receives it at tick_count: 205, then it will be using a new trajectory based on state from 5 ticks ago, where, in those 5 ticks, the vehicle's state will likely have changed quite a bit when compared with the state at tick_count: 200
This is likely why direction_cost() isn't seeing "jump back", but compute_vehicle_state() is
direction_cost() is calculating feasible new trajectories (i.e., none with "jump back"), but by the time compute_vehicle_state() sees these trajectories, the vehicle has driven way beyond the start of the new trajectory

Changes to Python Code

Here are some changes I made to see how many ticks had passed between the time when compute_vehicle_state() used the new trajectory and the time when the vehicle state was read in run_planner_process() for the generation of the new trajectory

In Vehicle.py:


def __init__(self, vid, name, btree_root, start_state,  lanelet_map:LaneletMap, lanelet_route, start_state_in_frenet=False):
...
self.reversing = False

self.jump_back_count = 0 self.plan_change = False self.plan_change_count = 0 self.tick_diff_one_count = 0 self.min_tick_diff = 100 ... def tick(self, tick_count, delta_time, sim_time): Vehicle.tick(self, tick_count, delta_time, sim_time) self.plan_change = False ... if plan: self.plan_change = True ...

Compute new state

    self.compute_vehicle_state(delta_time, tick_count)

def compute_vehicle_state(self, delta_time, tick_count): ... old_s = self.state.s

    # update frenet state
    self.state.s = self.s_eq(time)

... heading = np.array([y_vector[1], x_vector[1]])

if self.reversing:

    #     heading *= -1
    self.state.yaw = math.degrees(math.atan2(heading[1], heading[0]))

     if old_s > self.state.s:
         self.jump_back_count += 1

         if self.plan_change:
             self.plan_change_count += 1

              if tick_count == (self.reversing + 1):
                  self.tick_diff_one_count += 1

                  self.min_tick_diff = min(self.min_tick_diff, tick_count - self.reversing)

    #sanity check

- In `SimTraffic.py`:
```py
def stop_all(self):
...
    for vid in self.vehicles:
        print(
            "VID: %d, FREQ: %d, COUNT: %d, S: %d, GOOD: %d, TICK_DIFF: %d" %
            (vid, self.vehicles[vid].freq, self.vehicles[vid].count,
            self.vehicles[vid].s, self.vehicles[vid].good_tick_count,
            self.vehicles[vid].min_tick_diff)
        )

    for vid in self.vehicles:
...

In SDVPlanner.py:

def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
    header, traffic_vehicles, traffic_pedestrians,traffic_light_states, static_objects = self.sim_traffic.read_traffic_state(traffic_state_sharr,False)
    state_time = header[2]
    tick_count = header[0]
...
        if traj is None:
           # log.warn("plan_maneuver return invalid trajectory.")
           pass
        else:
            self.write_motion_plan(mplan_sharr, traj, state_time, ref_path_changed, tick_count)
...

Results

With the timeout for scenarios/test_scenarios/gs_ringroad_stress_loop.osm set to 60 seconds:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
VID: 4, JBC: 21, PLC: 21, TDOC: 0, MTD: 3
VID: 3, JBC: 6, PLC: 6, TDOC: 0, MTD: 5
VID: 5, JBC: 60, PLC: 60, TDOC: 0, MTD: 3
VID: 10, JBC: 36, PLC: 36, TDOC: 0, MTD: 3
VID: 9, JBC: 7, PLC: 7, TDOC: 0, MTD: 3
VID: 8, JBC: 5, PLC: 5, TDOC: 0, MTD: 3
VID: 7, JBC: 1, PLC: 1, TDOC: 0, MTD: 5
VID: 6, JBC: 8, PLC: 8, TDOC: 0, MTD: 2
VID: 2, JBC: 5, PLC: 5, TDOC: 0, MTD: 3
VID: 1, JBC: 33, PLC: 33, TDOC: 0, MTD: 3
...

Again, JBC == PLC means that if "jump back" occurs, then plan has changed (i.e., there is a new trajectory); the converse isn't necessarily true
TDOC == 0 means that there wasn't a single cases of "jump back" where the tick difference, between the current tick_count and the tick_count at which the new trajectory was generated, was equal to 1
The minimum tick difference (MTD) was always greater than 1, which means the vehicles are not getting the new trajectory at the ideal time (i.e., one tick after the tick where the vehicle state was read to generate the new trajectory)
P.S. there aren't many cases where the tick difference is equal to 1
So, just because the tick difference isn't equal to 1, doesn't mean that "jump back" will occur
But when the tick difference is equal to 1, then "jump back" will not occur

mchlswyr commented 3 years ago

Solution 1

This solution ensures that, once any planner reads the simulation state to generate a new trajectory, the simulation state is not updated until the new trajectory is generated
The downside to this simple solution is the BIG hit to performance, due to the fact that the simulation is much more serialized

Changes to Python Code

The changes made in Changes to Python Code plus the following changes:

In GSServer.py:

def start_server(args, m=MVelKeepConfig()):
...
traffic.stop_all()
log.info("TICK_COUNT: %d" % sync_global.tick_count)
dashboard.quit()
...

In SimTraffic.py:

def __init__(self, laneletmap, sim_config):
self.log_file = ''
self.vehicles_log = {}

self.plan_sem = Lock()
...
def tick(self, tick_count, delta_time, sim_time):
...
self.plan_sem.acquire()
#tick vehicles (all types)
...
self.write_traffic_state(tick_count, delta_time, sim_time)
self.plan_sem.release()
...

In SDVPlanner.py:

def __init__(self, sdv, sim_traffic):
...
self.mconfig = None

self.plan_sem = sim_traffic.plan_sem
...
def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
    self.plan_sem.acquire()
    header, traffic_vehicles, traffic_pedestrians,traffic_light_states, static_objects = self.sim_traffic.read_traffic_state(traffic_state_sharr,False)
...
        #vehicle state not available. Vehicle can be inactive.
        self.plan_sem.release()
        continue
...
        log.warn("Invalid planner state, skipping planning step...")
        self.plan_sem.release()
        continue
...
            log.warn("Invalid planner state, skipping planning step...")
            self.plan_sem.release()
            continue
...
    else:
        traj, cand, unf = None, None

    self.plan_sem.release()
...

In CostFunctions.py, when enabling/disabling direction_cost():

def maneuver_feasibility(start_state, trajectory, target_state, mconfig, lane_config:LaneConfig, vehicles, pedestrians, static_objects):
...
# Uncomment to enable
# if direction_cost(trajectory, start_state, target_state) > 0:
#     return False  
return True
...

Results:

In each case, python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm was run, with the timeout set to 60 seconds

Before implementing the solution and with direction_cost() disabled:

VID: 4, JBC: 10, PLC: 10, TDOC: 0, MTD: 3
VID: 3, JBC: 4, PLC: 4, TDOC: 0, MTD: 4
VID: 5, JBC: 45, PLC: 45, TDOC: 0, MTD: 3
VID: 10, JBC: 33, PLC: 33, TDOC: 0, MTD: 3
VID: 9, JBC: 8, PLC: 8, TDOC: 0, MTD: 3
VID: 8, JBC: 5, PLC: 5, TDOC: 0, MTD: 3
VID: 7, JBC: 12, PLC: 12, TDOC: 0, MTD: 3
VID: 6, JBC: 4, PLC: 4, TDOC: 0, MTD: 2
VID: 2, JBC: 6, PLC: 6, TDOC: 0, MTD: 2
VID: 1, JBC: 6, PLC: 6, TDOC: 0, MTD: 3
...
I0226 11:32:10.399209 25717 GSServer.py:77] TICK_COUNT: 950
I0226 11:32:10.399298 25717 GSServer.py:81] SIMULATION END

Many cases of "jump back"
A total tick count of 950

With solution and direction_cost() disabled:

VID: 4, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 3, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 5, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 10, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 9, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 8, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 7, JBC: 6, PLC: 0, TDOC: 0, MTD: 100
VID: 6, JBC: 4, PLC: 1, TDOC: 1, MTD: 1
VID: 2, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 1, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
...
I0226 11:25:31.264000 24771 GSServer.py:77] TICK_COUNT: 73
I0226 11:25:31.264088 24771 GSServer.py:81] SIMULATION END

Two cases of "jump back"
Total tick count of 73: way too low when compared with 950

With solution and direction_cost() enabled:

VID: 4, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 3, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 5, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 10, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 9, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 8, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 7, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 6, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 2, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 1, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
...
I0226 11:28:45.875734 25128 GSServer.py:77] TICK_COUNT: 74
I0226 11:28:45.875855 25128 GSServer.py:81] SIMULATION END

One case of "jump back"
This must be due to the fact that direction_cost() computed state.s using a different time than the time used in compute_vehicle_state() because the tick difference (MTD) was 1 (i.e., direction_cost() and compute_vehicle_state() were both looking at the same last state)
This slight difference in time should mean that the "jump back" distance is not very far (it may not even be noticable in the simulation)
Again, a total tick count of 74 is way too low compared to 950

mchlswyr commented 3 years ago

Solution 2

The reason that Solution 1 performs so poorly is because the planner processes don't run concurrently (they are all competing to acquire the one Lock)
This solution runs all of the planner processes, then runs the tick functions for vehicles and pedestrians for some number of ticks, and then runs the planner processes, etc.
This ensures that the state that each planner process sees is the current state of the simulation; this prevents "jump back"
There are two types of semaphores for each planner process: one that is acquired by the SimTraffic process, called t_sem, and one that is acquired by the planner process, called p_sem
All semaphores are initialized to 0
The SimTraffic process will release each p_sem, which will allow the planners to run; this can be thought of as spawning a thread
The SimTraffic process will then acquire each t_sem; this can be thought of as joining the threads
The reason threads aren't used is because of the cost of starting up a new thread for each planner every number of ticks

Changes to Python Code

All of the changes made in Changes to Python Code plus the following changes:

In SimTraffic.py:

def __init__(self, laneletmap, sim_config):
...
self.t_sems = {}
self.p_sems = {}
...
def start(self):
...
    if vehicle.type == Vehicle.SDV_TYPE:
        self.t_sems[vid] = Semaphore(0)
        self.p_sems[vid] = Semaphore(0)
        #vehicle.start_planner(
...
def tick(self, tick_count, delta_time, sim_time):
nv = len(self.vehicles)
np = len(self.pedestrians)

# Run on first tick
# E.g., If TRAFFIC_RATE is 30 ticks/s, and PLANNER_RATE is 5 ticks/s
# Then run the planners every 6 ticks
if tick_count % int(TRAFFIC_RATE/PLANNER_RATE) == 1:
    for vid in self.vehicles:
        self.p_sems[vid].release()

    for vid in self.vehicles:
        self.t_sems[vid].acquire()
...

In SDVPlanner.py:

def start(self):
...
#Process based
self._process = Process(target=self.run_planner_process, args=(
    self.traffic_state_sharr, 
    self._mplan_sharr, 
    self._debug_shdata,
    self.sim_traffic.p_sems[self.vid],
    self.sim_traffic.t_sems[self.vid]), daemon=True)
self._process.start()
...
def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata, p_sem, t_sem):
...
    p_sem.acquire()
    header, traffic_vehicles, traffic_pedestrians,traffic_light_states, static_objects = self.sim_traffic.read_traffic_state(traffic_state_sharr,False)
...
        #vehicle state not available. Vehicle can be inactive.
        t_sem.release()
        continue
...
        log.warn("Invalid planner state, skipping planning step...")
        t_sem.release()
        continue
...
            log.warn("Invalid planner state, skipping planning step...")
            t_sem.release()
            continue
...
    else:
        traj, cand, unf = None, None

    t_sem.release()

In Vehicle.py:

def compute_vehicle_state(self, delta_time, tick_count):
...
    old_pos = np.array([self.state.x, self.state.y])

    # update sim state
    self.state.x = x_vector[0]
...
        self.jump_back_count += 1

        new_pos = np.array([self.state.x, self.state.y])
        del_d = new_pos - old_pos
        dist = math.sqrt(del_d[0]*del_d[0] + del_d[1]*del_d[1])
        log.info("++++++++++VID: %d, DIST: %f++++++++++" % (self.id, dist))
...

Results:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
I0226 16:30:51.833491 32195 SDVPlanner.py:93] PLANNER PROCESS START for Vehicle 2
10 is leading. Distance 5.705784213721145, time gap inf
9 is leading. Distance 25.543431419976557, time gap inf
8 is leading. Distance 5.6204556900652065, time gap inf
E0226 16:30:51.853212 32190 ManeuverModels.py:395] No feasible trajectory to select
I0226 16:30:51.858024 32194 SDVPlanner.py:93] PLANNER PROCESS START for Vehicle 6
I0226 16:30:51.887776 32196 SDVPlanner.py:93] PLANNER PROCESS START for Vehicle 1
3 is leading. Distance 11.11835434773866, time gap inf
7 is leading. Distance 5.197694971518814, time gap inf
5 is leading. Distance 6.298504413839746, time gap inf
2 is leading. Distance 11.19746881928128, time gap inf
I0226 16:30:52.038504 32159 Vehicle.py:190] ++++++++++VID: 3, DIST: 0.000012++++++++++
I0226 16:30:52.039570 32159 Vehicle.py:190] ++++++++++VID: 10, DIST: 0.000016++++++++++
I0226 16:30:52.040146 32159 Vehicle.py:190] ++++++++++VID: 9, DIST: 0.000027++++++++++
I0226 16:30:52.041012 32159 Vehicle.py:190] ++++++++++VID: 6, DIST: 0.000025++++++++++
I0226 16:30:52.041642 32159 Vehicle.py:190] ++++++++++VID: 2, DIST: 0.000004++++++++++
...
VID: 4, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 3, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 5, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 10, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 9, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 8, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 7, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
VID: 6, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 2, JBC: 1, PLC: 1, TDOC: 1, MTD: 1
VID: 1, JBC: 0, PLC: 0, TDOC: 0, MTD: 100
...
I0226 16:31:51.837915 32159 GSServer.py:77] TICK_COUNT: 758
I0226 16:31:51.838017 32159 GSServer.py:81] SIMULATION END
...

There were 5 cases of "jump back"
Each case happened at the start of the simulation, and the farthest distance that a vehicle jumped back was 27 micrometers, which is insignificant in terms of watching the simulation, and in terms of collisions
The tick count was 758, which is acceptable when compared with 950 (especially since the "jump back" issue is solved)

rodrigoqueiroz commented 3 years ago

Great detailed findings. Question about Solution2: Is the main process (SimTraffic) being blocked while the planners are released to run?

mchlswyr commented 3 years ago

Question about Solution2: Is the main process (SimTraffic) being blocked while the planners are released to run?

Yes, it releases them all at once and is blocked until they all finish.

rodrigoqueiroz commented 3 years ago

We can't block the main process. Simulation will stutter until all planners finish their job. The co-simulator (in this case, unreal) or the subject system (the ADS) will continue running and reading last states as the vehicles, pedestrians, tlight state transitions are paused for a brief moment.

With my recent optimizations over the cost functions and the planning process I believe most of the "jump back" is gone (or significantly reduced). But there is an inherit problem with the planning task that makes it impossible to completely remove it. The more vehicles we add, the more it becomes noticeable.

There is an alternative I would like to discuss in the meeting. There are trade offs, so we will have it as an option. For now. Please, update your tests with the most recent version in the master branch so we have a new baseline.

mchlswyr commented 3 years ago

This test is to be used as a new baseline to compare new solutions with

Code Changes

In GSServer.py:

def start_server(args, m=MVelKeepConfig()):
...
traffic.stop_all()
print("TICK_COUNT: %d" % int(sync_global.tick_count))
dashboard.quit()
...

In SimTraffic.py:

def stop_all(self):
...
for pid in self.pedestrians:
    self.pedestrians[pid].stop()

for vid in self.vehicles:
    print(
            "|VID: {:3d}|Jump Back Count: {:3d}|Max Jump Back Dist: {:9.6f}|Plan Change Count: {:3d}|Min Tick Diff: {:3d}|".format(
                int(vid),
                int(self.vehicles[vid].jump_back_count),
                float(self.vehicles[vid].max_jump_back_dist),
                int(self.vehicles[vid].plan_change_count),
                int(self.vehicles[vid].min_tick_diff)
            )
    )

In sv/Vehicle.py:


def __init__(self, vid, name, btree_root, start_state,  lanelet_map:LaneletMap, lanelet_route, start_state_in_frenet=False):
...
self.jump_back_count = 0
self.max_jump_back_dist = 0
self.plan_change = False
self.plan_change_count = 0
self.min_tick_diff = 100
...
def tick(self, tick_count, delta_time, sim_time):
...
self.plan_change = False

#Read planner
if self.sv_planner:
    plan = self.sv_planner.get_plan()
    if plan:
        self.plan_change = True
...
    #Compute new state
    self.compute_vehicle_state(delta_time, tick_count)

def compute_vehicle_state(self, delta_time, tick_count): ... old_s = self.state.s old_pos = np.array([self.state.x, self.state.y])

    # update frenet state

... heading = np.array([y_vector[1], x_vector[1]])

if self.motion_plan.reversing:

    #     heading *= -1
    self.state.yaw = math.degrees(math.atan2(heading[1], heading[0]))

    if old_s > self.state.s:
        self.jump_back_count += 1

        new_pos = np.array([self.state.x, self.state.y])
        delta_pos = new_pos - old_pos
        dist = math.sqrt(delta_pos[0]*delta_pos[0] + delta_pos[1]*delta_pos[1])
        self.max_jump_back_dist = max(self.max_jump_back_dist, dist)

        if self.plan_change:
            self.plan_change_count += 1

            self.min_tick_diff = min(self.min_tick_diff, tick_count - self.motion_plan.reversing)

...

- In `sv/SDVPlanner.py`:
```py
def run_planner_process(self, traffic_state_sharr, mplan_sharr, debug_shdata):
...
        state_time = header[2]
        tick_count = header[0]
...
                plan.new_frenet_frame = ref_path_changed
                plan.reversing = tick_count#mconfig.mkey == Maneuver.M_REVERSE
...

GeoScenario Config

In SimConfig.py:

#Sim Config
TRAFFIC_RATE = 30
...
#Dash Config
SHOW_DASHBOARD = True
DASH_RATE = 30
...
#Global Map
SHOW_MPLOT = True
MPLOT_SIZE = 100
#Frenet Map
SHOW_FFPLOT = True
FFPLOT_ASPECT = False
FFPLOT_LENGTH = 60
FFPLOT_LITE = False
#Cartesian
SHOW_CPLOT = True
CPLOT_SIZE = 40
REFERENCE_PATH = True
#Vehicle trajectory
VEH_TRAJ_CHART = False
#BTree
SHOW_BTREE = True
# trajectory plots
SHOW_TRAJ = False
...
#Planning
PLANNER_RATE = 5
#Collision
VEH_COLLISION = False
PED_COLLISION = False
OBJ_COLLISION = False
TRAJECTORY_SPLIT = 10

Test

The timeout in scenarios/test_scenarios/gs_ringroad_stress_loop.osm was changed to 60 seconds

The following command was run:

python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm

Results

...
|VID:   4|Jump Back Count:  21|Max Jump Back Dist:  2.604170|Plan Change Count:  21|Min Tick Diff:   2|
|VID:   3|Jump Back Count:  15|Max Jump Back Dist:  2.308087|Plan Change Count:  15|Min Tick Diff:   3|
|VID:   5|Jump Back Count:  25|Max Jump Back Dist:  2.773198|Plan Change Count:  25|Min Tick Diff:   3|
|VID:  10|Jump Back Count:  25|Max Jump Back Dist:  2.429843|Plan Change Count:  25|Min Tick Diff:   3|
|VID:   9|Jump Back Count:  22|Max Jump Back Dist:  1.888675|Plan Change Count:  22|Min Tick Diff:   2|
|VID:   8|Jump Back Count:  11|Max Jump Back Dist:  0.924934|Plan Change Count:  11|Min Tick Diff:   3|
|VID:   7|Jump Back Count:  10|Max Jump Back Dist:  0.932807|Plan Change Count:  10|Min Tick Diff:   2|
|VID:   6|Jump Back Count:   7|Max Jump Back Dist:  0.302368|Plan Change Count:   7|Min Tick Diff:   3|
|VID:   2|Jump Back Count:  14|Max Jump Back Dist:  1.359995|Plan Change Count:  14|Min Tick Diff:   2|
|VID:   1|Jump Back Count:  16|Max Jump Back Dist:  0.966820|Plan Change Count:  16|Min Tick Diff:   2|
TICK_COUNT: 996
...

Jump Back Count seems more uniformly distributed and doesn't look to be as high when compared with Results (it could just be this one test)
Plan Change Count was identical to Jump Back Count, and so all cases of "jump back" occured when the vehicle got a new trajectory
The maximum Max Jump Back Dist was 2.77 meters: the farthest any vehicle "jumped back" in this test
Min Tick Diff was either 2 or 3; this means that the minimum number of ticks, between when a given planner read the vehicle state and when the corresponding vehicle used the new trajectory generated by the planner, was 2 or 3

mantkiew commented 3 years ago

It is clear that planning of the new trajectory should start from the projected position of the agent along the old trajectory. Let's ignore the high-frequency planner idea for now as there is no slipping and other physical effects here that would warrant it. Because we know how long planning takes (we can measure that and use for the projection for the next planning cycle), we can pretty precisely project the agents position along the old trajectory.