Skip to content

Commit

Permalink
new track
Browse files Browse the repository at this point in the history
  • Loading branch information
Nitish Bhupathi Raju authored and Nitish Bhupathi Raju committed Feb 29, 2024
1 parent f4d3e2a commit dd2f633
Show file tree
Hide file tree
Showing 2 changed files with 123 additions and 66 deletions.
146 changes: 93 additions & 53 deletions sense.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,36 +16,33 @@
from verse.analysis.utils import wrap_to_pi



#probabilistic sensors
def very_left():
num = random.randint(1,100)
if num >=1 and num <= 98:
return 0
else:
return 1

def slightly_left():
num = random.randint(1, 100)
if num >=1 and num <= 90:
return 0
else:
return 1


def slightly_right():
num = random.randint(1, 100)
if num >=1 and num <= 80:
return 1
else:
return 0

return 0
def very_right():
num = random.randint(1, 100)
if num >=1 and num <= 95:
return 1
else:
return 0


class VechicleSensor:
def __init__(self):
self.sensor = 1
Expand All @@ -59,23 +56,38 @@ def sense(self, agent: BaseAgent, state_dict, lane_map):
tmp = np.array(list(state_dict.values())[0][0])
if tmp.ndim < 2:
if agent.id == 'car':

cont['ego.x'] = state_dict['car'][0][1]
cont['ego.y'] = state_dict['car'][0][2]
cont['ego.theta'] = state_dict['car'][0][3]
cont['ego.v'] = state_dict['car'][0][4]
cont['ego.t'] = state_dict['car'][0][5]
disc['ego.agent_mode'] = state_dict['car'][1][0]
x = cont['ego.x']
y = cont['ego.y']

if((x-14)**2 + (y-14)**2 > 13**2):
cont['ego.s'] = 0#very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
cont['ego.s'] = 0#slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
cont['ego.s'] = 1#slightly_right()

x = state_dict['car'][0][1]#cont['ego.x']
y = state_dict['car'][0][2]#cont['ego.y']
agent_mode = state_dict['car'][1][0]

#print(agent_mode)

if(agent_mode == "Left" or agent_mode == "Right"):
if((x-14)**2 + (y-14)**2 > 13**2):
cont['ego.s'] = very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
cont['ego.s'] = slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
cont['ego.s'] = slightly_right()
else:
cont['ego.s'] = very_right()
else:
cont['ego.s'] = 1#very_right()
if(x < 1):
cont['ego.s'] = very_left()
elif(x < 2):
cont['ego.s'] = slightly_left()
elif(x < 3):
cont['ego.s'] = slightly_right()
else:
cont['ego.s'] = very_right()

else:
if agent.id == 'car':
Expand All @@ -91,28 +103,49 @@ def sense(self, agent: BaseAgent, state_dict, lane_map):

x = cont['ego.x'][0]
y = cont['ego.y'][0]

if((x-14)**2 + (y-14)**2 > 13**2):
state1 = very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
state1 = slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
state1 = slightly_right()
agent_mode = state_dict['car'][1][0]


if(agent_mode == "Left" or agent_mode == "Right"):
if((x-14)**2 + (y-14)**2 > 13**2):
state1 = very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
state1 = slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
state1 = slightly_right()
else:
state1 = very_right()
else:
state1 = very_right()
if(x < 1):
state1 = very_left()
elif(x < 2):
state1 = slightly_left()
elif(x < 3):
state1 = slightly_right()
else:
state1 = very_right()

x = cont['ego.x'][1]
y = cont['ego.y'][1]


if((x-14)**2 + (y-14)**2 > 13**2):
state2 = very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
state2 = slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
state2 = slightly_right()
if(agent_mode == "Left" or agent_mode == "Right"):
if((x-14)**2 + (y-14)**2 > 13**2):
state2 = very_left()
elif((x-14)**2 + (y-14)**2 > 12**2):
state2 = slightly_left()
elif((x-14)**2 + (y-14)**2 > 11**2):
state2 = slightly_right()
else:
state2 = very_right()
else:
state2 = very_right()
if(x < 1):
state2 = very_left()
elif(x < 2):
state2 = slightly_left()
elif(x < 3):
state2 = slightly_right()
else:
state2 = very_right()

if(state1 > state2):
state1 = state2
Expand All @@ -129,15 +162,21 @@ def sense(self, agent: BaseAgent, state_dict, lane_map):
class M1(LaneMap):
def __init__(self):
super().__init__()
#segment0 = StraightLane("Seg0", [0, 0], [0, 100], 3)
segment0 = CircularLane("Seg0", [14, 14], 12, np.pi * 3 / 2, np.pi * (-7/2), True, 4)
#lane0 = Lane("T0", [segment0])
segment1 = StraightLane("Seg1", [0, 10], [0, 100], 4)
lane0 = Lane("T0", [segment0])
self.add_lanes([lane0])
lane1 = Lane("T1", [segment1])
self.add_lanes([lane0, lane1])
self.h_dict = {("T0", "Left", "Right"): "T0", ("T0", "Right", "Left"): "T0",
("T0", "Left", "Left"): "T0",("T0", "Right", "Right"): "T0"}
("T0", "Left", "Left"): "T0",("T0", "Right", "Right"): "T0",

("T0", "Left", "StraightLeft"): "T1", ("T0", "Right", "StraightRight"): "T1",
("T0", "Left", "StraightRight"): "T1",("T0", "Right", "StraightLeft"): "T1",

("T1", "StraightLeft", "StraightRight"): "T1", ("T1", "StraightLeft", "StraightLeft"): "T1",
("T1", "StraightRight", "StraightRight"): "T1", ("T1", "StraightRight", "StraightLeft"): "T1"}


#modified T
def car_dynamics(t, state, u):
x, y, theta, v, t = state
delta, a = u
Expand Down Expand Up @@ -166,22 +205,26 @@ def car_action_handler(mode: List[str], state, lane_map) -> Tuple[float, float]:
'''

new_theta = 0
if vehicle_mode == "Left":
if vehicle_mode == "Left" or vehicle_mode == "StraightLeft":
new_theta = 45
elif vehicle_mode == "Right":
elif vehicle_mode == "Right" or vehicle_mode == "StraightRight":
new_theta = -45
else:
raise ValueError(f"Invalid mode: {vehicle_mode}")
#print(pos)

#print(vehicle_lane)

new_theta = np.radians(new_theta)
#heading = np.radians(heading)
heading = lane_map.get_lane_heading(vehicle_lane, pos) + np.arctan2(0.45 * new_theta, v)
psi = wrap_to_pi(heading - theta)

if(vehicle_lane == "T0"):
heading = lane_map.get_lane_heading(vehicle_lane, pos) + np.arctan2(0.45 * new_theta, v)
psi = wrap_to_pi(heading - theta)
else:
heading = lane_map.get_lane_heading(vehicle_lane, pos) + np.arctan2(0.45 * new_theta, v)
psi = wrap_to_pi(heading - theta)

steering = psi
steering = np.clip(steering, -0.40, 0.40)

steering = np.clip(steering, -0.4, 0.4)

return steering, 0

Expand All @@ -194,8 +237,6 @@ def TC_simulate(
trace[1:, 0] = [round(i * time_step, 10) for i in range(num_points)]
trace[0, 1:] = init
for i in range(num_points):
if i==300:
print("stop")
steering, a = car_action_handler(mode, init, lane_map)
r = ode(car_dynamics)
r.set_initial_value(init).set_f_params([steering, a])
Expand All @@ -215,7 +256,7 @@ def __init__(self, id, code=None, file_name=None):
self.TC_simulate = TC_simulate


scenario = Scenario(ScenarioConfig(parallel=False))
scenario = Scenario()#ScenarioConfig(parallel=False))
scenario.set_map(M1())
scenario.set_sensor(VechicleSensor())

Expand All @@ -224,19 +265,18 @@ def __init__(self, id, code=None, file_name=None):
car1.set_initial([[14, 1, np.radians(180), 0.75, 0], [14, 1, np.radians(180), 0.75, 0]], (AgentMode.Right, TrackMode.T0))
scenario.add_agent(car1)

traces_simu = scenario.simulate(100,0.01)
#traces_veri = scenario.verify(15, 0.01)
traces_simu = scenario.simulate(40,0.01)
#traces_veri = scenario.verify(40, 0.01)


fig = go.Figure()
fig = simulation_tree(traces_simu, None, fig, 1, 2, [0, 1], "lines", "trace")
fig.show()

'''

fig = go.Figure()
fig = reachtube_tree(traces_veri, None, fig, 1, 2, [0, 1], "lines", "trace")
fig.show()
'''



Expand Down
43 changes: 30 additions & 13 deletions sensedl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,16 +3,19 @@
import copy
import math


#the states the car can be in
class AgentMode(Enum):
Left = auto()
Right = auto()
StraightLeft = auto()
StraightRight = auto()


#two tracks for the map
class TrackMode(Enum):
T0 = auto()
T1 = auto()

#modified T
#state of the car
class State:
x: float
y: float
Expand All @@ -26,18 +29,32 @@ def __init__(self, x, y, theta, v,t, agent_mode: AgentMode, track_mode: TrackMod
pass



#decision logic of the car; changes based on the track (y<15 is T0 and y>15 is T1)
def decisionLogic(ego: State, track_map):
output = copy.deepcopy(ego)

if ego.s == 0 and ego.t >= 1: #we're on the left side
output.agent_mode = AgentMode.Right #go right
output.t = 0
elif ego.s == 1 and ego.t >= 1: #we're on the right side
output.agent_mode = AgentMode.Left #go left
output.t = 0

assert (ego.x-14)**2 + (ego.y-14)**2 >= 10**2 and (ego.x-14)**2 + (ego.y-14)**2 <= 14**2
if(ego.y > 14):
if ego.s == 0 and ego.t >= 2: #we're on the left side
output.agent_mode = AgentMode.StraightRight #go right
output.track_mode = TrackMode.T1
output.t = 0
elif ego.s == 1 and ego.t >= 2: #we're on the right side
output.agent_mode = AgentMode.StraightLeft #go left
output.t = 0
output.track_mode = TrackMode.T1
elif(ego.y < 14):
if ego.s == 0 and ego.t >= 2: #we're on the left side
output.agent_mode = AgentMode.Right #go right
output.t = 0
output.track_mode = TrackMode.T0
elif ego.s == 1 and ego.t >= 2: #we're on the right side
output.agent_mode = AgentMode.Left #go left
output.t = 0
output.track_mode = TrackMode.T0



#if(output.track_mode == TrackMode.T0):
assert ((ego.x-14)**2 + (ego.y-14)**2 >= 10**2 and (ego.x-14)**2 + (ego.y-14)**2 <= 14**2 and ego.y <= 14) or (ego.x > 0 and ego.x < 4 and ego.y > 14)

return output

Expand Down

0 comments on commit dd2f633

Please sign in to comment.