Closed mchlswyr closed 8 months ago
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
PLANNER_RATE = 5
and TRAFFIC_RATE = 30
in SimConfig.py
)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
PLANNER_RATE = 0.5
in SimConfig.py
, seems to remove the "jumping back" effectReadServerState()
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
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
yaw
and new_yaw
are in the range [0, 360), then when "jump back" occurs, abs(yaw - new_yaw)
is roughly 180 degreesabs(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)compute_vehicle_state()
in the file Vehicle.py
, I made the following changes:
...
old_pos = np.array([self.state.x, self.state.y])
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
...
- `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()
...
self.freq
is printed, so the results can be seen at the endpython3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
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
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
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
PLANNER_RATE = 0.5
, as was done in Note 2VID: 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
test_scenarios/gs_ringroad_single.osm
with the same changes as Note 2 and with a timeout of 60 seconds:VID: 1, FREQ: 211
test_scenarios/gs_ringroad_single.osm
with PLANNER_RATE = 0.5
, or with PLANNER_RATE = 5
, and with a timeout of 60 seconds:VID: 1, FREQ: 0
PLANNER_RATE
is increased, the performance decreased (i.e., lots of vehicles), and more frequently in leading vehiclesplan
in tick()
(in Vehicle.py
) is not None
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
)
compute_vehicle_state()
in Vehicle.py
:
...
old_s = self.state.s
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
direction_cost()
isn't catching what is being caught in compute_vehicle_state()
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()
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
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 stateSimTraffic.py
, when tick()
ticks each of the vehicles, the vehicles get the new trajectory (if a new one is available) from their corresponding planner processSimTraffic.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
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
direction_cost()
isn't seeing "jump back", but compute_vehicle_state()
isdirection_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 trajectorycompute_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 trajectoryVehicle.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 ...
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]])
# 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:
...
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)
...
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
...
JBC == PLC
means that if "jump back" occurs, then plan
has changed (i.e., there is a new trajectory); the converse isn't necessarily trueTDOC == 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 1MTD
) 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)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()
...
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
...
python3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
was run, with the timeout set to 60 secondsdirection_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
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
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
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)Lock
)tick
functions for vehicles
and pedestrians
for some number of ticks, and then runs the planner processes, etc.acquire
d by the SimTraffic
process, called t_sem
, and one that is acquire
d by the planner process, called p_sem
SimTraffic
process will release
each p_sem
, which will allow the planners to run; this can be thought of as spawning a threadSimTraffic
process will then acquire
each t_sem
; this can be thought of as joining the threadsIn 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))
...
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
...
Great detailed findings. Question about Solution2: Is the main process (SimTraffic) being blocked while the planners are released to run?
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.
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.
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]])
# 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
...
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
scenarios/test_scenarios/gs_ringroad_stress_loop.osm
was changed to 60 secondspython3.8 GSServer.py -s scenarios/test_scenarios/gs_ringroad_stress_loop.osm
...
|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 trajectoryMax Jump Back Dist
was 2.77 meters: the farthest any vehicle "jumped back" in this testMin 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 3It 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.
Tests
anm_unreal_test_suite
) was used for running the tests:GSS="${HOME}/anm_unreal_sim/submodules/geoscenarioserver" SIM="${HOME}/anm_unreal_sim/submodules/anm_sim_platform"
cleanup_and_exit() { killall python3.8 killall roslaunch
}
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