Skip to content
Snippets Groups Projects
Commit 205411f3 authored by li213's avatar li213
Browse files

refining the examples a little bit

parent 19778f5f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
demo/demo.py → demo/demo1.py
+ 16
− 14
View file @ 205411f3
from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
......@@ -34,33 +34,35 @@ if __name__ == "__main__":
input_code_name = 'example_controller1.py'
scenario = Scenario()
car = CarAgent('car1', file_name=input_code_name)
car = NPCAgent('car1')
scenario.add_agent(car)
car = CarAgent('car2', file_name=input_code_name)
scenario.add_agent(car)
tmp_map = SimpleMap6()
tmp_map = SimpleMap3()
scenario.set_map(tmp_map)
scenario.set_sensor(FakeSensor2())
scenario.set_init(
[
[[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
[[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
[[10.0, 0, 0, 0.5],[10.0, 0, 0, 0.5]],
[[5.0, -0.2, 0, 2.0],[6.0, 0.2, 0, 3.0]],
],
[
(VehicleMode.Normal, LaneMode.Lane1),
(VehicleMode.Normal, LaneMode.Lane1),
]
)
# res_list = scenario.simulate_multi(40,1)
traces = scenario.verify(40)
res_list = scenario.simulate_multi(10,1)
# traces = scenario.verify(10)
fig = plt.figure(2)
fig = plot_map(tmp_map, 'g', fig)
fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
# for traces in res_list:
# generate_simulation_anime(traces, tmp_map)
# # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig,x_lim,y_lim)
# # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig,x_lim,y_lim)
# fig = plot_map(tmp_map, 'g', fig)
# fig = plot_reachtube_tree(traces, 'car1', 0, [1], 'b', fig, (1000,-1000), (1000,-1000))
# fig = plot_reachtube_tree(traces, 'car2', 0, [1], 'r', fig)
for traces in res_list:
# fig = plot_simulation_tree(traces, 'car1', 0, [1], 'b', fig, (1000,-1000), (1000,-1000))
# fig = plot_simulation_tree(traces, 'car2', 0, [1], 'r', fig)
generate_simulation_anime(traces, tmp_map, fig)
plt.show()
demo/demo2.py
+ 14
− 16
View file @ 205411f3
from dryvr_plus_plus.example.example_agent.car_agent import CarAgent, NPCAgent
from dryvr_plus_plus.example.example_agent.car_agent import CarAgent
from dryvr_plus_plus.scene_verifier.scenario.scenario import Scenario
from dryvr_plus_plus.example.example_map.simple_map2 import SimpleMap2, SimpleMap3, SimpleMap5, SimpleMap6
from dryvr_plus_plus.plotter.plotter2D import *
......@@ -34,35 +34,33 @@ if __name__ == "__main__":
input_code_name = 'example_controller2.py'
scenario = Scenario()
car = NPCAgent('car1')
car = CarAgent('car1', file_name=input_code_name)
scenario.add_agent(car)
car = CarAgent('car2', file_name=input_code_name)
scenario.add_agent(car)
tmp_map = SimpleMap6()
tmp_map = SimpleMap3()
scenario.set_map(tmp_map)
scenario.set_sensor(FakeSensor2())
scenario.set_init(
[
[[20, 0, 0, 0.5],[20, 0, 0, 0.5]],
[[15, -0.2, 0, 2.0],[16.0, 0.2, 0, 3.0]],
[[0, -0.2, 0, 1.0],[0.1, 0.2, 0, 1.0]],
[[10, 0, 0, 0.5],[10, 0, 0, 0.5]],
],
[
(VehicleMode.Normal, LaneMode.Lane1),
(VehicleMode.Normal, LaneMode.Lane1),
]
)
res_list = scenario.simulate_multi(10,10)
# traces = scenario.verify(10)
# res_list = scenario.simulate_multi(40,1)
traces = scenario.verify(40)
fig = plt.figure(2)
# fig = plot_map(tmp_map, 'g', fig)
# fig = plot_reachtube_tree(traces, 'car1', 0, [1], 'b', fig, (1000,-1000), (1000,-1000))
# fig = plot_reachtube_tree(traces, 'car2', 0, [1], 'r', fig)
for traces in res_list:
fig = plot_simulation_tree(traces, 'car1', 0, [1], 'b', fig, (1000,-1000), (1000,-1000))
fig = plot_simulation_tree(traces, 'car2', 0, [1], 'r', fig)
# generate_simulation_anime(traces, tmp_map)
fig = plot_map(tmp_map, 'g', fig)
fig = plot_reachtube_tree(traces, 'car1', 1, [2], 'b', fig)
fig = plot_reachtube_tree(traces, 'car2', 1, [2], 'r', fig)
# for traces in res_list:
# generate_simulation_anime(traces, tmp_map, fig)
# # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car1', 1, [2], 'b', fig,x_lim,y_lim)
# # fig,x_lim,y_lim = plot_simulation_tree(traces, 'car2', 1, [2], 'r', fig,x_lim,y_lim)
plt.show()
demo/example_controller1.py
+ 8
− 25
View file @ 205411f3
from enum import Enum, auto
import copy
class LaneObjectMode(Enum):
Vehicle = auto()
Ped = auto() # Pedestrians
Sign = auto() # Signs, stop signs, merge, yield etc.
Signal = auto() # Traffic lights
Obstacle = auto() # Static (to road/lane) obstacles
class VehicleMode(Enum):
Normal = auto()
SwitchLeft = auto()
......@@ -28,29 +21,19 @@ class State:
lane_mode: LaneMode = LaneMode.Lane0
def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
pass
self.data = []
def controller(ego:State, other:State, lane_map):
output = copy.deepcopy(ego)
if ego.vehicle_mode == VehicleMode.Normal:
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 5 \
and ego.lane_mode == other.lane_mode:
if lane_map.has_left(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchLeft
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 5 \
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 0 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 3 \
and ego.lane_mode == other.lane_mode:
if lane_map.has_right(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchRight
if ego.vehicle_mode == VehicleMode.SwitchLeft:
if lane_map.get_lateral_distance(ego.lane_mode, [ego.x, ego.y]) >= 2.5:
output.vehicle_mode = VehicleMode.Brake
elif ego.vehicle_mode == VehicleMode.Brake:
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 10 \
or ego.lane_mode != other.lane_mode:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.left_lane(ego.lane_mode)
if ego.vehicle_mode == VehicleMode.SwitchRight:
if lane_map.get_lateral_distance(ego.lane_mode, [ego.x, ego.y]) <= -2.5:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.right_lane(ego.lane_mode)
return output
demo/example_controller2.py
+ 25
− 8
View file @ 205411f3
from enum import Enum, auto
import copy
class LaneObjectMode(Enum):
Vehicle = auto()
Ped = auto() # Pedestrians
Sign = auto() # Signs, stop signs, merge, yield etc.
Signal = auto() # Traffic lights
Obstacle = auto() # Static (to road/lane) obstacles
class VehicleMode(Enum):
Normal = auto()
SwitchLeft = auto()
......@@ -21,19 +28,29 @@ class State:
lane_mode: LaneMode = LaneMode.Lane0
def __init__(self, x, y, theta, v, vehicle_mode: VehicleMode, lane_mode: LaneMode):
self.data = []
pass
def controller(ego:State, other:State, lane_map):
output = copy.deepcopy(ego)
if ego.vehicle_mode == VehicleMode.Normal:
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 0 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 3 \
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 5 \
and ego.lane_mode == other.lane_mode:
if lane_map.has_left(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchLeft
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 3 \
and lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) < 5 \
and ego.lane_mode == other.lane_mode:
output.vehicle_mode = VehicleMode.Brake
elif ego.vehicle_mode == VehicleMode.Brake:
if lane_map.get_longitudinal_position(other.lane_mode, [other.x,other.y]) - lane_map.get_longitudinal_position(ego.lane_mode, [ego.x,ego.y]) > 10 \
or ego.lane_mode != other.lane_mode:
if lane_map.has_right(ego.lane_mode):
output.vehicle_mode = VehicleMode.SwitchRight
if ego.vehicle_mode == VehicleMode.SwitchLeft:
if lane_map.get_lateral_distance(ego.lane_mode, [ego.x, ego.y]) >= 2.5:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.left_lane(ego.lane_mode)
if ego.vehicle_mode == VehicleMode.SwitchRight:
if lane_map.get_lateral_distance(ego.lane_mode, [ego.x, ego.y]) <= -2.5:
output.vehicle_mode = VehicleMode.Normal
output.lane_mode = lane_map.right_lane(ego.lane_mode)
return output
dryvr_plus_plus/example/example_map/simple_map2.py
+ 4
− 4
View file @ 205411f3
......@@ -13,7 +13,7 @@ class SimpleMap2(LaneMap):
[100,0],
3
)
lane0 = Lane('Lane0', [segment0])
lane0 = Lane('Lane1', [segment0])
self.add_lanes([lane0])
class SimpleMap3(LaneMap):
......@@ -22,21 +22,21 @@ class SimpleMap3(LaneMap):
segment0 = StraightLane(
'Seg0',
[0,3],
[100,3],
[50,3],
3
)
lane0 = Lane('Lane0', [segment0])
segment1 = StraightLane(
'seg0',
[0,0],
[100,0],
[50,0],
3
)
lane1 = Lane('Lane1', [segment1])
segment2 = StraightLane(
'seg0',
[0,-3],
[100,-3],
[50,-3],
3
)
lane2 = Lane('Lane2', [segment2])
......
dryvr_plus_plus/plotter/plotter2D.py
+ 22
− 8
View file @ 205411f3
......@@ -84,11 +84,22 @@ def plot_map(map, color = 'b', fig = None, x_lim = None,y_lim = None):
lane = map.lane_dict[lane_idx]
for lane_seg in lane.segment_list:
if lane_seg.type == 'Straight':
ax.plot([lane_seg.start[0], lane_seg.end[0]],[lane_seg.start[1], lane_seg.end[1]], color)
start1 = lane_seg.start + lane_seg.width/2 * lane_seg.direction_lateral
end1 = lane_seg.end + lane_seg.width/2 * lane_seg.direction_lateral
ax.plot([start1[0], end1[0]],[start1[1], end1[1]], color)
start2 = lane_seg.start - lane_seg.width/2 * lane_seg.direction_lateral
end2 = lane_seg.end - lane_seg.width/2 * lane_seg.direction_lateral
ax.plot([start2[0], end2[0]],[start2[1], end2[1]], color)
elif lane_seg.type == "Circular":
phase_array = np.linspace(start=lane_seg.start_phase, stop=lane_seg.end_phase, num=100)
x = np.cos(phase_array)*lane_seg.radius + lane_seg.center[0]
y = np.sin(phase_array)*lane_seg.radius + lane_seg.center[1]
r1 = lane_seg.radius - lane_seg.width/2
x = np.cos(phase_array)*r1 + lane_seg.center[0]
y = np.sin(phase_array)*r1 + lane_seg.center[1]
ax.plot(x,y,color)
r2 = lane_seg.radius + lane_seg.width/2
x = np.cos(phase_array)*r2 + lane_seg.center[0]
y = np.sin(phase_array)*r2 + lane_seg.center[1]
ax.plot(x,y,color)
else:
raise ValueError(f'Unknown lane segment type {lane_seg.type}')
......@@ -126,11 +137,15 @@ def plot_simulation_tree(root, agent_id, x_dim: int=0, y_dim_list: List[int]=[1]
return fig
def generate_simulation_anime(root, map):
def generate_simulation_anime(root, map, fig = None):
if fig is None:
fig = plt.figure()
fig = plot_map(map, 'g', fig)
timed_point_dict = {}
stack = [root]
ax = fig.gca()
x_min, x_max = float('inf'), -float('inf')
y_min, y_max = float('inf'), -float('inf')
y_min, y_max = ax.get_ylim()
while stack != []:
node = stack.pop()
traces = node.trace
......@@ -151,11 +166,10 @@ def generate_simulation_anime(root, map):
stack += node.child
frames = []
fig = plt.figure()
for time_point in timed_point_dict:
point_list = timed_point_dict[time_point]
plt.xlim((x_min-1, x_max+1))
plt.ylim((y_min-1, y_max+1))
plt.xlim((x_min-2, x_max+2))
plt.ylim((y_min-2, y_max+2))
plot_map(map,color = 'g', fig = fig)
for data in point_list:
point = data[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