Skip to content
Snippets Groups Projects
Commit 95f971c6 authored by li213's avatar li213
Browse files

getting two car straight lane switch example working

parent ff07146b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
example_two_car_lane_switch.py
+ 20
− 16
View file @ 95f971c6
......@@ -2,8 +2,6 @@ from enum import Enum,auto
from ourtool.map.lane_map import LaneMap
a = 1+2
class VehicleMode(Enum):
Normal = auto()
SwitchLeft = auto()
......@@ -13,6 +11,7 @@ class VehicleMode(Enum):
class LaneMode(Enum):
Lane0 = auto()
Lane1 = auto()
Lane2 = auto()
class State:
x = 0.0
......@@ -31,28 +30,24 @@ def controller(ego:State, other:State, lane_map):
if other.x - ego.x > 3 and other.x - ego.x < 5 and ego.lane_mode == other.lane_mode:
if lane_map.has_left(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchLeft
# output.lane_mode = lane_map.left_lane(ego.lane_mode)
if other.x - ego.x > 3 and other.x - ego.x < 5 and ego.lane_mode == other.lane_mode:
if lane_map.has_right(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchRight
# output.lane_mode = lane_map.right_lane(ego.lane_mode)
if ego.vehicle_mode == VehicleMode.SwitchLeft:
if ego.y >= 2.5:
if ego.y >= lane_map.lane_geometry(ego.lane_mode)-0.5:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.left_lane(ego.lane_mode)
output.y = ego.y-3
if ego.vehicle_mode == VehicleMode.SwitchRight:
if ego.y <= -2.5:
if ego.y <= lane_map.lane_geometry(ego.lane_mode)+0.5:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.right_lane(ego.lane_mode)
output.y = ego.y+3
return output
from ourtool.agents.car_agent import CarAgent
from ourtool.scenario.scenario import Scenario
from user.simple_sensor import SimpleSensor
from user.simple_map import SimpleMap
# from user.simple_sensor import SimpleSensor
from user.simple_map import SimpleMap, SimpleMap2
import matplotlib.pyplot as plt
import numpy as np
......@@ -64,26 +59,35 @@ if __name__ == "__main__":
scenario.add_agent(car)
car = CarAgent('car2', file_name=input_code_name)
scenario.add_agent(car)
scenario.add_map(SimpleMap())
scenario.add_map(SimpleMap2())
# scenario.set_sensor(SimpleSensor())
scenario.set_init(
[[0,0,0,1.0], [10,0,0,0.5]],
[[10,0,0,0.5], [0,-3,0,1.0]],
[
(VehicleMode.Normal, LaneMode.Lane0),
(VehicleMode.Normal, LaneMode.Lane0)
(VehicleMode.Normal, LaneMode.Lane1),
(VehicleMode.Normal, LaneMode.Lane2)
]
)
# simulator = Simulator()
traces = scenario.simulate(40)
plt.plot([0,40],[3,3],'g')
plt.plot([0,40],[0,0],'g')
plt.plot([0,40],[-3,-3],'g')
queue = [traces]
while queue!=[]:
node = queue.pop(0)
traces = node.trace
agent_id = 'ego'
# for agent_id in traces:
agent_id = 'car2'
trace = np.array(traces[agent_id])
plt.plot(trace[:,0], trace[:,2], 'b')
plt.plot(trace[:,1], trace[:,2], 'r')
agent_id = 'car1'
trace = np.array(traces[agent_id])
plt.plot(trace[:,1], trace[:,2], 'b')
# if node.child != []:
queue += node.child
plt.show()
ourtool/agents/car_agent.py
+ 6
− 4
View file @ 95f971c6
from ourtool.agents.base_agent import BaseAgent
import numpy as np
from scipy.integrate import ode
from ourtool.map.lane_map import LaneMap
class CarAgent(BaseAgent):
def __init__(self, id, code = None, file_name = None):
......@@ -16,7 +17,7 @@ class CarAgent(BaseAgent):
v_dot = a
return [x_dot, y_dot, theta_dot, v_dot]
def TC_simulate(self, mode, initialCondition, time_bound, map=None):
def TC_simulate(self, mode, initialCondition, time_bound, lane_map:LaneMap=None):
mode = mode.split(',')
vehicle_mode = mode[0]
vehicle_lane = mode[1]
......@@ -27,10 +28,11 @@ class CarAgent(BaseAgent):
init = initialCondition
trace = [[0]+init]
lane_parameter = lane_map.lane_geometry(vehicle_lane)
if vehicle_mode == "Normal":
for i in range(len(t)):
x,y,theta,v = init
d = -y
d = -y+lane_parameter
psi = -theta
steering = psi + np.arctan2(0.45*d, v)
steering = np.clip(steering, -0.61, 0.61)
......@@ -43,7 +45,7 @@ class CarAgent(BaseAgent):
elif vehicle_mode == "SwitchLeft":
for i in range(len(t)):
x,y,theta,v = init
d = -y+1
d = -y+3+lane_parameter
psi = -theta
steering = psi + np.arctan2(0.45*d, v)
steering = np.clip(steering, -0.61, 0.61)
......@@ -56,7 +58,7 @@ class CarAgent(BaseAgent):
elif vehicle_mode == "SwitchRight":
for i in range(len(t)):
x,y,theta,v = init
d = -y-1
d = -y-3+lane_parameter
psi = -theta
steering = psi + np.arctan2(0.45*d, v)
steering = np.clip(steering, -0.61, 0.61)
......
ourtool/automaton/guard.py
+ 11
− 5
View file @ 95f971c6
......@@ -226,11 +226,17 @@ class GuardExpression:
expr = root.data
# Check if the root is a function
if 'map' in expr:
tmp = re.split('\(|\)',expr)
while "" in tmp:
tmp.remove("")
for arg in tmp[1:]:
expr = expr.replace(arg,f'"{disc_var_dict[arg]}"')
# tmp = re.split('\(|\)',expr)
# while "" in tmp:
# tmp.remove("")
# for arg in tmp[1:]:
# if arg in disc_var_dict:
# expr = expr.replace(arg,f'"{disc_var_dict[arg]}"')
# res = eval(expr)
for arg in disc_var_dict:
expr = expr.replace(arg, f'"{disc_var_dict[arg]}"')
for arg in cnts_var_dict:
expr = expr.replace(arg, str(cnts_var_dict[arg]))
res = eval(expr)
return res
# Elif check if the root contain any discrete data
......
ourtool/map/lane_segment.py
+ 2
− 2
View file @ 95f971c6
......@@ -11,5 +11,5 @@ class LaneSegment:
if lane_parameter is not None:
self.lane_parameter = lane_parameter
def get_geometry():
pass
\ No newline at end of file
def get_geometry(self):
return self.lane_parameter
\ No newline at end of file
ourtool/scenario/scenario.py
+ 26
− 48
View file @ 95f971c6
from typing import Dict, List
import copy
import itertools
from ourtool.agents.base_agent import BaseAgent
from ourtool.automaton.guard import GuardExpression
......@@ -80,10 +81,10 @@ class Scenario:
init_list.append(self.init_dict[agent_id])
init_mode_list.append(self.init_mode_dict[agent_id])
agent_list.append(self.agent_dict[agent_id])
return self.simulator.simulate(init_list, init_mode_list, agent_list, self, time_horizon)
return self.simulator.simulate(init_list, init_mode_list, agent_list, self, time_horizon, self.map)
def get_all_transition(self, state_dict):
guard_hit = False
lane_map = self.map
satisfied_guard = []
for agent_id in state_dict:
agent:BaseAgent = self.agent_dict[agent_id]
......@@ -113,6 +114,7 @@ class Scenario:
# If the guard can be satisfied, handle resets
next_init = agent_state
dest = agent_mode.split(',')
possible_dest = [[elem] for elem in dest]
for reset in reset_list:
# Specify the destination mode
if "mode" in reset:
......@@ -120,54 +122,30 @@ class Scenario:
if discrete_variable_ego in reset:
break
tmp = reset.split('=')
tmp = tmp[1].split('.')
if tmp[0].strip(' ') in agent.controller.modes:
dest[i] = tmp[1]
if 'map' in tmp[1]:
tmp = tmp[1]
for var in discrete_variable_dict:
tmp = tmp.replace(var, f"'{discrete_variable_dict[var]}'")
possible_dest[i] = eval(tmp)
else:
tmp = tmp[1].split('.')
if tmp[0].strip(' ') in agent.controller.modes:
possible_dest[i] = [tmp[1]]
else:
for i, cts_variable in enumerate(agent.controller.variables):
if cts_variable in reset:
for i, cts_variable in enumerate(agent.controller.vars_dict['ego']['cont']):
if "output."+cts_variable in reset:
break
tmp = reset.split('=')
next_init[i] = float(tmp[1])
dest = ','.join(dest)
next_transition = (
agent_id, agent_mode, dest, next_init,
)
satisfied_guard.append(next_transition)
tmp = tmp[1]
for cts_variable in continuous_variable_dict:
tmp = tmp.replace(cts_variable, str(continuous_variable_dict[cts_variable]))
next_init[i] = eval(tmp)
all_dest = itertools.product(*possible_dest)
for dest in all_dest:
dest = ','.join(dest)
next_transition = (
agent_id, agent_mode, dest, next_init,
)
satisfied_guard.append(next_transition)
# guard_can_satisfy = guard_expression.execute_guard(discrete_variable_dict)
# if guard_can_satisfy:
# python_guard_string = guard_expression.generate_guard_string_python()
# # Substitute value into dryvr guard string
# for i, variable in enumerate(agent.controller.variables):
# python_guard_string = python_guard_string.replace(variable, str(agent_state[i]))
# # Evaluate the guard strings
# res = eval(python_guard_string)
# # if result is true, check reset and construct next mode
# if res:
# next_init = agent_state
# dest = agent_mode.split(',')
# for reset in reset_list:
# # Specify the destination mode
# if "mode" in reset:
# for i, discrete_variable in enumerate(agent.controller.discrete_variables):
# if discrete_variable in reset:
# break
# tmp = reset.split('=')
# tmp = tmp[1].split('.')
# if tmp[0].strip(' ') in agent.controller.modes:
# dest[i] = tmp[1]
# else:
# for i, cts_variable in enumerate(agent.controller.variables):
# if cts_variable in reset:
# break
# tmp = reset.split('=')
# next_init[i] = float(tmp[1])
# dest = ','.join(dest)
# next_transition = (
# agent_id, agent_mode, dest, next_init,
# )
# satisfied_guard.append(next_transition)
return satisfied_guard
\ No newline at end of file
ourtool/simulator/simulator.py
+ 2
− 2
View file @ 95f971c6
......@@ -31,7 +31,7 @@ class Simulator:
def __init__(self):
self.simulation_tree_root = None
def simulate(self, init_list, init_mode_list, agent_list:List[BaseAgent], transition_graph, time_horizon):
def simulate(self, init_list, init_mode_list, agent_list:List[BaseAgent], transition_graph, time_horizon, lane_map):
# Setup the root of the simulation tree
root = SimulationTreeNode()
for i, agent in enumerate(agent_list):
......@@ -57,7 +57,7 @@ class Simulator:
# Simulate the trace starting from initial condition
mode = node.mode[agent_id]
init = node.init[agent_id]
trace = node.agent[agent_id].TC_simulate(mode, init, remain_time)
trace = node.agent[agent_id].TC_simulate(mode, init, remain_time,lane_map)
trace[:,0] += node.start_time
node.trace[agent_id] = trace.tolist()
......
user/simple_map.py 0 → 100644
+ 29
− 0
View file @ 95f971c6
from ourtool.map.lane_map import LaneMap
from ourtool.map.lane_segment import LaneSegment
class SimpleMap(LaneMap):
def __init__(self):
super().__init__()
segment1 = LaneSegment('Lane0', 0)
segment2 = LaneSegment('Lane1', 3)
self.add_lanes([segment1,segment2])
self.left_lane_dict[segment1.id].append(segment2.id)
self.right_lane_dict[segment2.id].append(segment1.id)
class SimpleMap2(LaneMap):
def __init__(self):
super().__init__()
segment1 = LaneSegment('Lane0', 3)
segment2 = LaneSegment('Lane1', 0)
segment3 = LaneSegment('Lane2', -3)
self.add_lanes([segment1,segment2,segment3])
self.left_lane_dict[segment2.id].append(segment1.id)
self.left_lane_dict[segment3.id].append(segment2.id)
self.right_lane_dict[segment1.id].append(segment2.id)
self.right_lane_dict[segment2.id].append(segment3.id)
if __name__ == "__main__":
test_map = SimpleMap()
print(test_map.left_lane_dict)
print(test_map.right_lane_dict)
print(test_map.lane_segment_dict)
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