Skip to content
Snippets Groups Projects
Commit be8a9523 authored by crides's avatar crides
Browse files

update(example): add switching time to switch-2 example for debounce

parent 33694800
No related branches found
No related tags found
2 merge requests!26Visualize dot,!25parallel, incremental updates, and (updated) experiments for atva2023
Hide whitespace changes
Inline Side-by-side
demo/tacas2023/exp11/decision_logic/inc-expr-switch-2.py
+ 35
− 28
View file @ be8a9523
......@@ -30,6 +30,7 @@ class State:
y = 0.0
theta = 0.0
v = 0.0
sw_time = 0.0
agent_mode: AgentMode = AgentMode.Normal
track_mode: TrackMode = TrackMode.T0
type: LaneObjectMode = LaneObjectMode.Vehicle
......@@ -40,32 +41,38 @@ def car_ahead(ego, others, track, track_map, thresh_far, thresh_close):
def decisionLogic(ego:State, others:List[State], track_map):
output = copy.deepcopy(ego)
if ego.agent_mode == AgentMode.Normal:
left_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft)
left2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft2)
right_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight)
right2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight2)
if car_ahead(ego, others, ego.track_mode, track_map, 5, 3):
# Switch left if left lane is empty
if left_lane != None and not car_ahead(ego, others, left_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchLeft
output.track_mode = left_lane
if left2_lane != None and not car_ahead(ego, others, left2_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchLeft2
output.track_mode = left2_lane
if right_lane != None and not car_ahead(ego, others, right_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchRight
output.track_mode = right_lane
if right2_lane != None and not car_ahead(ego, others, right2_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchRight2
output.track_mode = right2_lane
else: # If switched enough, return to normal mode
lat = track_map.get_lateral_distance(ego.track_mode, [ego.x, ego.y])
lane_width = track_map.get_lane_width(ego.track_mode)
if ego.agent_mode == AgentMode.SwitchLeft and lat >= (lane_width-0.2) \
or ego.agent_mode == AgentMode.SwitchLeft2 and lat >= (lane_width * 2-0.2) \
or ego.agent_mode == AgentMode.SwitchRight and lat <= -(lane_width -0.2) \
or ego.agent_mode == AgentMode.SwitchRight2 and lat <= -(lane_width * 2-0.2):
output.agent_mode = AgentMode.Normal
output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal)
if ego.sw_time >= 1:
if ego.agent_mode == AgentMode.Normal:
left_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft)
left2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchLeft2)
right_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight)
right2_lane = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.SwitchRight2)
if car_ahead(ego, others, ego.track_mode, track_map, 5, 3):
# Switch left if left lane is empty
if left_lane != None and not car_ahead(ego, others, left_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchLeft
output.track_mode = left_lane
output.sw_time = 0
if left2_lane != None and not car_ahead(ego, others, left2_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchLeft2
output.track_mode = left2_lane
output.sw_time = 0
if right_lane != None and not car_ahead(ego, others, right_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchRight
output.track_mode = right_lane
output.sw_time = 0
if right2_lane != None and not car_ahead(ego, others, right2_lane, track_map, 8, -3):
output.agent_mode = AgentMode.SwitchRight2
output.track_mode = right2_lane
output.sw_time = 0
else: # If switched enough, return to normal mode
lat = track_map.get_lateral_distance(ego.track_mode, [ego.x, ego.y])
lane_width = track_map.get_lane_width(ego.track_mode)
if ego.agent_mode == AgentMode.SwitchLeft and lat >= (lane_width-0.2) \
or ego.agent_mode == AgentMode.SwitchLeft2 and lat >= (lane_width * 2-0.2) \
or ego.agent_mode == AgentMode.SwitchRight and lat <= -(lane_width -0.2) \
or ego.agent_mode == AgentMode.SwitchRight2 and lat <= -(lane_width * 2-0.2):
output.agent_mode = AgentMode.Normal
output.track_mode = track_map.h(ego.track_mode, ego.agent_mode, AgentMode.Normal)
output.sw_time = 0
return output
demo/tacas2023/exp11/inc-expr-switch-2.py
+ 4
− 4
View file @ be8a9523
......@@ -91,10 +91,10 @@ if __name__ == "__main__":
[(LaneObjectMode.Vehicle,) for _ in range(8)])
poses = [
[0, 0, 0, 1.0], [10, 0, 0, 0.5],
[14, 3, 0, 0.6], [20, 3, 0, 0.5],
[30, 0, 0, 0.5], [28.5, -3, 0, 0.5],
[39.5, -3, 0, 0.5], [30, -3, 0, 0.6],
[0, 0, 0, 1.0, 0], [10, 0, 0, 0.5, 0],
[14, 3, 0, 0.6, 0], [20, 3, 0, 0.5, 0],
[30, 0, 0, 0.5, 0], [28.5, -3, 0, 0.5, 0],
[39.5, -3, 0, 0.5, 0], [30, -3, 0, 0.6, 0],
]
_jerks = [[0, 0.05] if i + 1 in smarts else [] for i in range(8)]
cont_inits = dupi(poses)
......
verse/agents/example_agent/car_agent.py
+ 13
− 6
View file @ be8a9523
......@@ -22,18 +22,21 @@ class NPCAgent(BaseAgent):
@staticmethod
def dynamic(t, state, u):
x, y, theta, v = state
theta, v = state[2:4]
delta, a = u
x_dot = v*np.cos(theta+delta)
y_dot = v*np.sin(theta+delta)
theta_dot = v/1.75*np.sin(delta)
v_dot = a
return [x_dot, y_dot, theta_dot, v_dot]
dots = [x_dot, y_dot, theta_dot, v_dot]
if len(state) == 4:
return dots
return dots + [1]
def action_handler(self, mode, state, lane_map:LaneMap)->Tuple[float, float]:
''' Computes steering and acceleration based on current lane, target lane and
current state using a Stanley controller-like rule'''
x,y,theta,v = state
x,y,theta,v = state[:4]
vehicle_mode = mode[0]
vehicle_lane = mode[1]
vehicle_pos = np.array([x,y])
......@@ -147,11 +150,15 @@ class WeirdCarAgent(CarAgent):
return 0, 0
class CarAgentSwitch2(CarAgent):
def __init__(self, id, code = None, file_name = None):
super().__init__(id, code, file_name)
def __init__(self, id, code = None, file_name = None, initial_state = None, initial_mode = None, speed: float = 2, accel: float = 1):
super().__init__(id, code, file_name, initial_state=initial_state, initial_mode=initial_mode, speed=speed, accel=accel)
@staticmethod
def dynamic(t, state, u):
return super(CarAgentSwitch2, CarAgentSwitch2).dynamic(t, state[:4], u) + [1]
def action_handler(self, mode: List[str], state, lane_map:LaneMap)->Tuple[float, float]:
x,y,theta,v = state
x,y,theta,v,_ = state
vehicle_mode, vehicle_lane = mode
vehicle_pos = np.array([x,y])
a = 0
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment