Skip to content

Commit

Permalink
Changed StraightLane
Browse files Browse the repository at this point in the history
  • Loading branch information
Nitish Bhupathi Raju authored and Nitish Bhupathi Raju committed Mar 4, 2024
1 parent 9f333d9 commit bbd9183
Show file tree
Hide file tree
Showing 4 changed files with 77 additions and 150 deletions.
172 changes: 49 additions & 123 deletions sense.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,38 +56,26 @@ 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 = 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()
x = cont['ego.x']
y = cont['ego.y']
position = np.array([x,y])
lateral = lane_map.get_lateral_distance("T0", position)

if(lateral < -1):
cont['ego.s'] = 0
elif(lateral < 0):
cont['ego.s'] = slightly_left()
elif(lateral < 1):
cont['ego.s'] = slightly_right()
else:
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()
cont['ego.s'] = 1

else:
if agent.id == 'car':
Expand All @@ -98,84 +86,38 @@ def sense(self, agent: BaseAgent, state_dict, lane_map):
cont['ego.t'] = [state_dict['car'][0][0][5], state_dict['car'][0][1][5]]
disc['ego.agent_mode'] = state_dict['car'][1][0]

state1 = -1
state2 = -1

x = cont['ego.x'][0]
y = cont['ego.y'][0]
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:
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(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()
lateral = []
for x in cont['ego.x']:
for y in cont['ego.y']:
position = np.array([x,y])
print(position)
lateral.append(lane_map.get_lateral_distance("T0", position))

min_lateral = min(lateral)

if(min_lateral < -1):
cont['ego.s'] = [0,0]
elif(min_lateral < 0):
cont['ego.s'] = [0,1]
elif(min_lateral < 1):
cont['ego.s'] = [0,1]
else:
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
cont['ego.s'] = [state1, state2]

'''
print(cont['ego.s'])
cont['ego.s'] = [1,1]

if(cont['ego.s'] == [0,1]):
print("hello")
'''

return cont, disc, len_dict

class M1(LaneMap):
def __init__(self):
super().__init__()
segment0 = CircularLane("Seg0", [14, 14], 12, np.pi * 3 / 2, np.pi * (-7/2), True, 4)
segment1 = StraightLane("Seg1", [0, 10], [0, 100], 4)
lane0 = Lane("T0", [segment0])
lane1 = Lane("T1", [segment1])
self.add_lanes([lane0, lane1])
segment0 = CircularLane("Seg0", [14, 14], 12, np.pi * 3 / 2, np.pi, True, 4)
segment1 = StraightLane("Seg1", [2, 14], [2, 24], 4)
segment2 = CircularLane("Seg2", [14, 24], 12, np.pi, 0, True, 4)
segment3 = StraightLane("Seg3", [26, 24], [26, 14], 4)
lane0 = Lane("T0", [segment0, segment1, segment2])# segment3])
self.add_lanes([lane0])
self.h_dict = {("T0", "Left", "Right"): "T0", ("T0", "Right", "Left"): "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"}

("T0", "Left", "Left"): "T0",("T0", "Right", "Right"): "T0"}

def car_dynamics(t, state, u):
x, y, theta, v, t = state
Expand All @@ -195,37 +137,20 @@ def car_action_handler(mode: List[str], state, lane_map) -> Tuple[float, float]:
d = 0
pos = np.array([x,y])

'''
center = np.array([14,14])
v = pos - center
if(np.sum(v) == 0):
track_pos = pos
else:
track_pos = center + (v/np.linalg.norm(v))*12
'''

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

#print(vehicle_lane)

new_theta = np.radians(new_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)

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.4, 0.4)

return steering, 0

def TC_simulate(
Expand All @@ -248,30 +173,31 @@ def TC_simulate(
trace[i + 1, 1:] = init
return trace


class CarAgent(BaseAgent):
def __init__(self, id, code=None, file_name=None):
self.id = id
self.decision_logic = ControllerIR.parse(code, file_name)
self.TC_simulate = TC_simulate


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


car1 = CarAgent("car", file_name="sensedl.py")
car1.set_initial([[14, 1, np.radians(180), 0.75, 0], [14, 1, np.radians(180), 0.75, 0]], (AgentMode.Right, TrackMode.T0))
car1.set_initial([[14, 1, np.radians(180), 0.75, 0], [14, 2, np.radians(180), 0.75, 0]], (AgentMode.Right, TrackMode.T0))
scenario.add_agent(car1)

traces_simu = scenario.simulate(40,0.01)
#traces_veri = scenario.verify(40, 0.01)
#traces_simu = scenario.simulate(60,0.01)
traces_veri = scenario.verify(10, 0.01)

traces_veri.dump("out.json")

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

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


fig = go.Figure()
Expand Down
36 changes: 21 additions & 15 deletions sensedl.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,12 @@
from typing import List
import copy
import math
import numpy as np

#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):
Expand All @@ -32,6 +31,7 @@ def __init__(self, x, y, theta, v,t, agent_mode: AgentMode, track_mode: TrackMod
#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.y > 14):
if ego.s == 0 and ego.t >= 2: #we're on the left side
output.agent_mode = AgentMode.StraightRight #go right
Expand All @@ -42,20 +42,26 @@ def decisionLogic(ego: State, track_map):
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 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)


x = ego.x
y = ego.y
position = [x,y]
lateral = track_map.get_lateral_distance("T0", position)

# assert -2 <= lateral and lateral <= 2
#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 and ego.y <= 24) or ((ego.x-14)**2 + (ego.y-24)**2 >= 10**2 and (ego.x-14)**2 + (ego.y-24)**2 <= 14**2 and ego.y > 24)
assert True

return output


8 changes: 3 additions & 5 deletions verse/map/lane.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,17 +27,12 @@ def get_lane_segment(self, position: np.ndarray) -> AbstractLane:
seg = None
for seg_idx, segment in enumerate(self.segment_list):
logitudinal, lateral = segment.local_coordinates(position)
#if(logitudinal > 37.5):
#print(logitudinal)
is_on = 0 - Lane.COMPENSATE <= logitudinal < segment.length
if is_on:
if lateral < min_lateral:
idx = seg_idx
seg = segment
min_lateral = lateral
if seg is None:
print("here")
segment.local_coordinates(position)
return idx, seg

def get_heading(self, position: np.ndarray) -> float:
Expand All @@ -55,6 +50,9 @@ def get_longitudinal_position(self, position: np.ndarray) -> float:

def get_lateral_distance(self, position: np.ndarray) -> float:
seg_idx, segment = self.get_lane_segment(position)
# if(segment is None):
# return np.inf if(position[0] > 0) else -1*np.inf
# else:
longitudinal, lateral = segment.local_coordinates(position)
return lateral

Expand Down
11 changes: 4 additions & 7 deletions verse/map/lane_segment.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,8 @@ def width_at(self, longitudinal: float) -> float:
def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
delta = position - self.start
longitudinal = np.dot(delta, self.direction)
lateral = np.dot(delta, self.direction_lateral)
#changed
lateral = -1 * np.dot(delta, self.direction_lateral)
return float(longitudinal), float(lateral)

@classmethod
Expand Down Expand Up @@ -279,19 +280,15 @@ def width_at(self, longitudinal: float) -> float:

def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
delta = position - self.center
phi = np.arctan2(delta[1], delta[0])
wrap = (wrap_to_pi(phi - self.start_phase))
#if(wrap <= 0):
phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
r = np.linalg.norm(delta)
# longitudinal = (phi - self.start_phase) * self.radius
phi = np.arctan2(delta[1], delta[0])
phase_diff = wrap_to_pi(phi-self.start_phase)
if self.direction<0 and phase_diff > 0:
phase_diff -= np.pi*2
elif self.direction>0 and phase_diff <0:
phase_diff += np.pi*2
longitudinal = self.direction * phase_diff * self.radius
lateral = self.direction * (self.radius - r)
lateral = (self.radius - r)
return longitudinal, lateral

@classmethod
Expand Down

0 comments on commit bbd9183

Please sign in to comment.