Skip to content
Snippets Groups Projects
Commit 4854f763 authored by li213's avatar li213
Browse files

small fixes about sensor

parent 5e315cad
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
demo/ball_bounces_dev.py
+ 7
− 2
View file @ 4854f763
...@@ -45,6 +45,7 @@ from verse.agents.example_agent.ball_agent import BallAgent ...@@ -45,6 +45,7 @@ from verse.agents.example_agent.ball_agent import BallAgent
from verse import Scenario from verse import Scenario
from verse.plotter.plotter2D import * from verse.plotter.plotter2D import *
import plotly.graph_objects as go import plotly.graph_objects as go
from verse.sensor.example_sensor.single_sensor import SingleSensor
if __name__ == "__main__": if __name__ == "__main__":
ball_controller = './demo/ball_bounces_dev.py' ball_controller = './demo/ball_bounces_dev.py'
...@@ -67,9 +68,13 @@ if __name__ == "__main__": ...@@ -67,9 +68,13 @@ if __name__ == "__main__":
(BallTypeMode.TYPE2,) (BallTypeMode.TYPE2,)
] ]
) )
bouncingBall.set_sensor(SingleSensor())
traces = bouncingBall.verify(10, 0.01)
fig = go.Figure()
fig = reachtube_tree(traces, fig=fig)
fig.show()
traces = bouncingBall.simulate(10, 0.01) traces = bouncingBall.simulate(10, 0.01)
traces.dump('./output.json')
traces = AnalysisTree.load('./output.json')
fig = go.Figure() fig = go.Figure()
fig = simulation_tree(traces, fig=fig) fig = simulation_tree(traces, fig=fig)
fig.show() fig.show()
demo/vehicle/demo2.py
+ 10
− 64
View file @ 4854f763
from verse.agents.example_agent import CarAgent from verse.agents.example_agent import CarAgent
from verse.map.example_map import SimpleMap3 from verse.map.example_map import SimpleMap3
from verse.sensor.example_sensor import FakeSensor2 from verse.sensor.example_sensor.single_sensor import SingleSensor
from verse import Scenario from verse import Scenario
from verse.plotter.plotter2D import * from verse.plotter.plotter2D import *
from enum import Enum, auto from enum import Enum, auto
...@@ -43,75 +43,21 @@ if __name__ == "__main__": ...@@ -43,75 +43,21 @@ if __name__ == "__main__":
scenario.add_agent(car2) scenario.add_agent(car2)
tmp_map = SimpleMap3() tmp_map = SimpleMap3()
scenario.set_map(tmp_map) scenario.set_map(tmp_map)
# scenario.set_sensor(FakeSensor2()) scenario.set_sensor(SingleSensor())
# # TEST1: wrong dims. AssertionError: the length of element in init not fit the number of continuous variables
# scenario.set_init_single('car1', [
# [0, -0.2, 0, 1.0, 0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1))
# # TEST2: wrong dims. AssertionError: the length of element in init_mode not fit the number of discrete variables
# scenario.set_init_single('car1', [
# [0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1, LaneMode.Lane1))
# # TEST3: wrong agent id. AssertionError: agent_id not found
# scenario.set_init_single('car3', [
# [0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1))
# # TEST4: not all agents inited. AssertionError: init of car2 not initialized
# scenario.set_init_single('car1', [
# [0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1))
# # TEST5: init individually. works
# scenario.set_init_single('car1', [
# [0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1))
# scenario.set_init_single('car2', [[10, 0, 0, 0.5], [
# 10, 0, 0, 0.5]], (VehicleMode.Normal, LaneMode.Lane1))
# # TEST6: init together. works
# 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]],
# ],
# [
# (VehicleMode.Normal, LaneMode.Lane1),
# (VehicleMode.Normal, LaneMode.Lane1),
# ]
# )
# TEST7: init individually and omit the another limitation if it is the same as the first one. works
scenario.set_init_single('car1', [ scenario.set_init_single('car1', [
[0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1)) [0, -0.2, 0, 1.0], [0.1, 0.2, 0, 1.0]], (VehicleMode.Normal, LaneMode.Lane1))
scenario.set_init_single( scenario.set_init_single(
'car2', [[10, 0, 0, 0.5]], (VehicleMode.Normal, LaneMode.Lane1)) 'car2', [[10, 0, 0, 0.5]], (VehicleMode.Normal, LaneMode.Lane1))
traces = scenario.simulate(1, 0.001) traces = scenario.verify(40, 0.05)
fig = go.Figure() fig = go.Figure()
fig = simulation_anime(traces, tmp_map, fig, 1, 2, [0, 1, 2], fig = reachtube_tree(traces, tmp_map, fig, 1,
'lines', anime_mode='normal', full_trace=True, time_step=0.001) 2, [1, 2], 'lines', 'trace')
fig.show() fig.show()
# traces = scenario.simulate(30, 1) traces = scenario.simulate(40, 0.05)
# path = os.path.abspath('.') fig = go.Figure()
# if os.path.exists(path+'/demo'): fig = simulation_tree(traces, tmp_map, fig, 1,
# path += '/demo' 2, [1, 2], 'lines', 'trace')
# path += '/output' fig.show()
# if not os.path.exists(path):
# os.makedirs(path)
# file = path+"/output.json"
# write_json(traces, file)
# root = read_json(file)
# fig = go.Figure()
# fig = simulation_tree(root, tmp_map, fig, 1, 2, 'lines')
# fig.show()
# # traces = scenario.verify(70, 0.05)
# fig = go.Figure()
# fig = simulation_tree(traces, tmp_map, fig, 1, 2,
# 'lines', 'trace', print_dim_list=[1, 2])
# # # fig = reachtube_anime(traces, tmp_map, fig, 1,
# # # 2, 'lines', 'trace', print_dim_list=[1, 2])
# fig.show()
# fig = go.Figure()
# # traces = scenario.verify(30, 0.2)
# path = os.path.abspath(__file__)
# path = path.replace('demo2.py', 'output.json')
# # write_json(traces, path)
# traces = read_json(path)
# fig = reachtube_anime(traces, tmp_map, fig, 1,
# 2, 'lines', 'trace', print_dim_list=[1, 2], sample_rate=1, speed_rate=5)
# fig.show()
verse/sensor/base_sensor.py
+ 3
− 2
View file @ 4854f763
import numpy as np import numpy as np
from verse.agents.base_agent import BaseAgent from verse.agents.base_agent import BaseAgent
...@@ -40,7 +41,7 @@ def add_states_2d(cont, disc, thing, val, cont_var, disc_var, stat_var): ...@@ -40,7 +41,7 @@ def add_states_2d(cont, disc, thing, val, cont_var, disc_var, stat_var):
def add_states_3d(cont, disc, thing, val, cont_var, disc_var, stat_var): def add_states_3d(cont, disc, thing, val, cont_var, disc_var, stat_var):
state, mode, static = val state, mode, static = val
transp = np.transpose(np.array(state)[:, 1:]) transp = np.transpose(np.array(state)[:, 1:])
assert len(transp) == 4 # assert len(transp) == 4
adds(cont, thing, cont_var, transp) adds(cont, thing, cont_var, transp)
adds(disc, thing, disc_var, mode) adds(disc, thing, disc_var, mode)
adds(disc, thing, stat_var, static) adds(disc, thing, stat_var, static)
...@@ -124,4 +125,4 @@ class BaseSensor(): ...@@ -124,4 +125,4 @@ class BaseSensor():
stat_var = agent.controller.state_defs[arg_type].static stat_var = agent.controller.state_defs[arg_type].static
add_states_3d(cont, disc, arg_name, state_dict[agent_id], cont_var, disc_var, stat_var) add_states_3d(cont, disc, arg_name, state_dict[agent_id], cont_var, disc_var, stat_var)
return cont, disc, len_dict return cont, disc, len_dict
\ No newline at end of file
verse/sensor/example_sensor/__init__.py
+ 0
− 1
View file @ 4854f763
from verse.sensor.example_sensor.fake_sensor import *
verse/sensor/example_sensor/fake_sensor.py → verse/sensor/example_sensor/single_sensor.py
+ 196
− 0
View file @ 4854f763
import numpy as np import numpy as np
class FakeSensor1:
def sense(self, scenario, agent, state_dict, lane_map):
cnts = {}
disc = {}
state = state_dict['ego'][0]
mode = state_dict['ego'][1].split(',')
cnts['ego.x'] = state[1]
cnts['ego.y'] = state[2]
cnts['ego.theta'] = state[3]
cnts['ego.v'] = state[4]
disc['ego.vehicle_mode'] = mode[0]
disc['ego.lane_mode'] = mode[1]
return cnts, disc, {}
def sets(d, thing, attrs, vals): def sets(d, thing, attrs, vals):
d.update({thing + "." + k: v for k, v in zip(attrs, vals)}) d.update({thing + "." + k: v for k, v in zip(attrs, vals)})
...@@ -28,78 +12,117 @@ def adds(d, thing, attrs, vals): ...@@ -28,78 +12,117 @@ def adds(d, thing, attrs, vals):
d[thing + '.' + k].append(v) d[thing + '.' + k].append(v)
def add_states_2d(cont, disc, thing, val): def set_states_2d(cnts, disc, thing, val, cont_var, disc_var, stat_var):
state, mode, static = val state, mode, static = val
adds(cont, thing, ['x', 'y', 'theta', 'v'], state[1:5]) sets(cnts, thing, cont_var, state[1:])
adds(disc, thing, ["vehicle_mode", "lane_mode"], mode) sets(disc, thing, disc_var, mode)
adds(disc, thing, ['type'], static) sets(disc, thing, stat_var, static)
def add_states_3d(cont, disc, thing, val): def set_states_3d(cnts, disc, thing, val, cont_var, disc_var, stat_var):
state, mode, static = val state, mode, static = val
transp = np.transpose(np.array(state)[:, 1:5]) transp = np.transpose(np.array(state)[:, 1:])
assert len(transp) == 4 # assert len(transp) == 4
adds(cont, thing, ["x", "y", "theta", "v"], transp) sets(cnts, thing, cont_var, transp)
adds(disc, thing, ["vehicle_mode", "lane_mode"], mode) sets(disc, thing, disc_var, mode)
adds(disc, thing, ['type'], static) sets(disc, thing, stat_var, static)
def set_states_2d(cnts, disc, thing, val): def add_states_2d(cont, disc, thing, val, cont_var, disc_var, stat_var):
state, mode, static = val state, mode, static = val
sets(cnts, thing, ["x", "y", "theta", "v"], state[1:5]) adds(cont, thing, cont_var, state[1:])
sets(disc, thing, ["vehicle_mode", "lane_mode"], mode) adds(disc, thing, disc_var, mode)
sets(disc, thing, ['type'], static) adds(disc, thing, stat_var, static)
def set_states_3d(cnts, disc, thing, val): def add_states_3d(cont, disc, thing, val, cont_var, disc_var, stat_var):
state, mode, static = val state, mode, static = val
transp = np.transpose(np.array(state)[:, 1:5]) transp = np.transpose(np.array(state)[:, 1:])
assert len(transp) == 4 # assert len(transp) == 4
sets(cnts, thing, ["x", "y", "theta", "v"], transp) adds(cont, thing, cont_var, transp)
sets(disc, thing, ["vehicle_mode", "lane_mode"], mode) adds(disc, thing, disc_var, mode)
sets(disc, thing, ['type'], static) adds(disc, thing, stat_var, static)
# TODO-PARSER: Update base sensor
class FakeSensor2:
class SingleSensor():
# The baseline sensor is omniscient. Each agent can get the state of all other agents
def sense(self, scenario, agent, state_dict, lane_map): def sense(self, scenario, agent, state_dict, lane_map):
cnts = {} cont = {}
disc = {} disc = {}
tmp = np.array(state_dict['car1'][0]) len_dict = {'others': len(state_dict)-1}
tmp = np.array(list(state_dict.values())[0][0])
if tmp.ndim < 2: if tmp.ndim < 2:
if agent.id == 'car1': for agent_id in state_dict:
set_states_2d(cnts, disc, "ego", state_dict["car1"]) if agent_id == agent.id:
set_states_2d(cnts, disc, "other", state_dict["car2"]) # Get type of ego
if "sign" in state_dict: controller_args = agent.controller.args
set_states_2d(cnts, disc, "sign", state_dict["sign"]) arg_type = None
elif agent.id == 'car2': for arg in controller_args:
set_states_2d(cnts, disc, "other", state_dict["car1"]) if arg.name == 'ego':
set_states_2d(cnts, disc, "ego", state_dict["car2"]) arg_type = arg.typ
if "sign" in state_dict: break
set_states_2d(cnts, disc, "sign", state_dict["sign"]) if arg_type is None:
elif agent.id == 'sign': continue
set_states_2d(cnts, disc, "ego", state_dict["sign"]) raise ValueError(f"Invalid arg for ego")
set_states_2d(cnts, disc, "other", state_dict["car2"]) cont_var = agent.controller.state_defs[arg_type].cont
if "sign" in state_dict: disc_var = agent.controller.state_defs[arg_type].disc
set_states_2d(cnts, disc, "sign", state_dict["sign"]) stat_var = agent.controller.state_defs[arg_type].static
return cnts, disc, {} set_states_2d(
else: cont, disc, 'ego', state_dict[agent_id], cont_var, disc_var, stat_var)
if agent.id == 'car1': else:
set_states_3d(cnts, disc, "ego", state_dict["car1"]) controller_args = agent.controller.args
set_states_3d(cnts, disc, "other", state_dict["car2"]) arg_type = None
if "sign" in state_dict: arg_name = None
set_states_3d(cnts, disc, "sign", state_dict["sign"]) for arg in controller_args:
elif agent.id == 'car2': if arg.name != 'ego' and 'map' not in arg.name:
set_states_3d(cnts, disc, "other", state_dict["car1"]) arg_name = arg.name
set_states_3d(cnts, disc, "ego", state_dict["car2"]) arg_type = arg.typ
if "sign" in state_dict: break
set_states_3d(cnts, disc, "sign", state_dict["sign"]) if arg_type is None:
elif agent.id == 'sign': continue
set_states_3d(cnts, disc, "ego", state_dict["sign"]) raise ValueError(f"Invalid arg for others")
set_states_3d(cnts, disc, "other", state_dict["car2"]) cont_var = agent.controller.state_defs[arg_type].cont
if "sign" in state_dict: disc_var = agent.controller.state_defs[arg_type].disc
set_states_3d(cnts, disc, "sign", state_dict["sign"]) stat_var = agent.controller.state_defs[arg_type].static
return cnts, disc, {} add_states_2d(
cont, disc, arg_name, state_dict[agent_id], cont_var, disc_var, stat_var)
else:
for agent_id in state_dict:
if agent_id == agent.id:
# Get type of ego
controller_args = agent.controller.args
arg_type = None
for arg in controller_args:
if arg.name == 'ego':
arg_type = arg.typ
break
if arg_type is None:
raise ValueError(f"Invalid arg for ego")
cont_var = agent.controller.state_defs[arg_type].cont
disc_var = agent.controller.state_defs[arg_type].disc
stat_var = agent.controller.state_defs[arg_type].static
set_states_3d(
cont, disc, 'ego', state_dict[agent_id], cont_var, disc_var, stat_var)
else:
controller_args = agent.controller.args
arg_type = None
arg_name = None
for arg in controller_args:
if arg.name != 'ego' and 'map' not in arg.name:
arg_name = arg.name
arg_type = arg.typ
break
if arg_type is None:
raise ValueError(f"Invalid arg for others")
cont_var = agent.controller.state_defs[arg_type].cont
disc_var = agent.controller.state_defs[arg_type].disc
stat_var = agent.controller.state_defs[arg_type].static
set_states_3d(cont, disc, arg_name, state_dict[agent_id], cont_var, disc_var, stat_var)
return cont, disc, len_dict
class FakeSensor3: class FakeSensor3:
def sense(self, scenario, agent, state_dict, lane_map): def sense(self, scenario, agent, state_dict, lane_map):
......
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