# Copyright (c) 2019 fortiss GmbH
#
# This software is released under the MIT License.
# https://opensource.org/licenses/MIT
from modules.runtime.scenario.scenario_generation.uniform_vehicle_distribution import UniformVehicleDistribution
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.pygame_viewer import PygameViewer
import time
import os
scenario_dump_folder = "examples/scenarios/"
scenario_param_file = "highway_merging.json" # must be within scenario dump folder
param_server = ParameterServer(
filename=os.path.join("examples/params/", scenario_param_file))
scenario_generation = UniformVehicleDistribution(num_scenarios=3,
random_seed=0,
params=param_server)
scenario_generation.params.save(
os.path.join(scenario_dump_folder, scenario_param_file))
viewer = PygameViewer(params=param_server, use_world_bounds=True)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation",
0.05]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
for _ in range(0, 2): # run 5 scenarios in a row, repeating after 3
def test_python_behavior_model(self):
# World Definition
scenario_param_file = "macro_actions_test.json" # must be within examples params folder
params = ParameterServer(filename=os.path.join(
"modules/world/tests/params/", scenario_param_file))
world = World(params)
# Define two behavior models one python one standard c++ model
behavior_model = PythonDistanceBehavior(params)
execution_model = ExecutionModelInterpolate(params)
dynamic_model = SingleTrackModel(params)
behavior_model2 = BehaviorConstantVelocity(params)
execution_model2 = ExecutionModelInterpolate(params)
dynamic_model2 = SingleTrackModel(params)
# Define the map interface and load a testing map
map_interface = MapInterface()
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
# Define the agent shapes
agent_2d_shape = CarRectangle()
init_state = np.array([0, 3, -5.25, 0, 20])
# Define the goal definition for agents
center_line = Line2d()
center_line.AddPoint(Point2d(0.0, -1.75))
center_line.AddPoint(Point2d(100.0, -1.75))
max_lateral_dist = (0.4, 0.5)
max_orientation_diff = (0.08, 0.1)
velocity_range = (5.0, 20.0)
goal_definition = GoalDefinitionStateLimitsFrenet(
center_line, max_lateral_dist, max_orientation_diff,
velocity_range)
# define two agents with the different behavior models
agent_params = params.AddChild("agent1")
agent = Agent(init_state, behavior_model, dynamic_model,
execution_model, agent_2d_shape, agent_params,
goal_definition, map_interface)
world.AddAgent(agent)
init_state2 = np.array([0, 25, -5.25, 0, 15])
agent2 = Agent(init_state2, behavior_model2, dynamic_model2,
execution_model2, agent_2d_shape, agent_params,
goal_definition, map_interface)
world.AddAgent(agent2)
# viewer
viewer = MPViewer(params=params, use_world_bounds=True)
# World Simulation
sim_step_time = params["simulation"]["step_time",
"Step-time in simulation", 0.2]
sim_real_time_factor = params["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
# Draw map
video_renderer = VideoRenderer(renderer=viewer,
world_step_time=sim_step_time)
for _ in range(0, 20):
world.Step(sim_step_time)
viewer.clear()
video_renderer.drawWorld(world)
video_renderer.drawGoalDefinition(goal_definition, "red", 0.5,
"red")
time.sleep(sim_step_time / sim_real_time_factor)
video_renderer.export_video(filename="./test_video_intermediate",
remove_image_dir=True)
from bark.world.agent import *
from bark.models.behavior import *
from bark.world import *
from bark.world.map import *
from bark.models.dynamic import *
from bark.models.execution import *
from bark.geometry import *
from bark.geometry.standard_shapes import *
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.pygame_viewer import PygameViewer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
# from modules.runtime.viewer.panda3d_viewer import Panda3dViewer
from modules.runtime.commons.xodr_parser import XodrParser
# Parameters Definitions
param_server = ParameterServer()
# set parameter that is accessible in Python as well as cpp
# param_server.setReal("wheel_base", 0.8)
# World Definition
world = World(param_server)
# Model Definitions
behavior_model = BehaviorConstantVelocity(param_server)
execution_model = ExecutionModelInterpolate(param_server)
dynamic_model = SingleTrackModel()
# Map Definition
xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr")
map_interface = MapInterface()
map_interface.set_open_drive_map(xodr_parser.map)
def test_configurable_scenario_generation_sample_behavior_types(self):
sink_source_dict = [{
"SourceSink": [[5111.626, 5006.8305], [5110.789, 5193.1725]],
"Description":
"left_lane",
"ConfigAgentStatesGeometries": {
"Type": "UniformVehicleDistribution",
"LanePositions": [0]
},
"ConfigBehaviorModels": {
"Type": "FixedBehaviorType",
"ModelType": "BehaviorIDMClassic",
"ModelParams": {
"BehaviorIDMClassic::MaxVelocity": 60.0
}
},
"ConfigExecutionModels": {
"Type": "FixedExecutionType"
},
"ConfigDynamicModels": {
"Type": "FixedDynamicType"
},
"ConfigGoalDefinitions": {
"Type": "FixedGoalTypes"
},
"ConfigControlledAgents": {
"Type": "NoneControlled"
},
"AgentParams": {}
}, {
"SourceSink": [[5111.626, 5006.8305], [5110.789, 5193.1725]],
"Description":
"right_lane",
"ConfigAgentStatesGeometries": {
"Type": "UniformVehicleDistribution",
"LanePositions": [1]
},
"ConfigBehaviorModels": {
"Type": "SampleBehaviorType"
},
"ConfigExecutionModels": {
"Type": "FixedExecutionType"
},
"ConfigDynamicModels": {
"Type": "FixedDynamicType"
},
"ConfigGoalDefinitions": {
"Type": "FixedGoalTypes"
},
"ConfigControlledAgents": {
"Type": "RandomSingleAgent"
},
"AgentParams": {}
}]
params = ParameterServer()
params["Scenario"]["Generation"]["ConfigurableScenarioGeneration"][
"SinksSources"] = sink_source_dict
scenario_generation = ConfigurableScenarioGeneration(num_scenarios=2,
params=params)
scenario_generation.dump_scenario_list("test.scenario")
params.Save("default_params_behavior_type_sampling.json")
def test_uct_single_agent(self):
try:
from bark.models.behavior import BehaviorUCTSingleAgentMacroActions
except:
print("Rerun with --define planner_uct=true")
return
# World Definition
scenario_param_file = "macro_actions_test.json" # must be within examples params folder
params = ParameterServer(filename=os.path.join(
"modules/world/tests/params/", scenario_param_file))
world = World(params)
# Model Definitions
behavior_model = BehaviorUCTSingleAgentMacroActions(params)
execution_model = ExecutionModelInterpolate(params)
dynamic_model = SingleTrackModel(params)
behavior_model2 = BehaviorConstantVelocity(params)
execution_model2 = ExecutionModelInterpolate(params)
dynamic_model2 = SingleTrackModel(params)
# Map Definition
map_interface = MapInterface()
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
# agent_2d_shape = CarLimousine()
agent_2d_shape = CarRectangle()
init_state = np.array([0, 3, -5.25, 0, 20])
agent_params = params.AddChild("agent1")
# goal_polygon = Polygon2d(
# [1, 1, 0], [Point2d(0, 0), Point2d(0, 2), Point2d(2, 2), Point2d(2, 0)])
# goal_definition = GoalDefinitionPolygon(goal_polygon)
# goal_polygon = goal_polygon.Translate(Point2d(90, -2))
center_line = Line2d()
center_line.AddPoint(Point2d(0.0, -1.75))
center_line.AddPoint(Point2d(100.0, -1.75))
max_lateral_dist = (0.4, 0.5)
max_orientation_diff = (0.08, 0.1)
velocity_range = (5.0, 20.0)
goal_definition = GoalDefinitionStateLimitsFrenet(
center_line, max_lateral_dist, max_orientation_diff,
velocity_range)
agent = Agent(init_state, behavior_model, dynamic_model,
execution_model, agent_2d_shape, agent_params,
goal_definition, map_interface)
world.AddAgent(agent)
init_state2 = np.array([0, 25, -5.25, 0, 0])
agent2 = Agent(init_state2, behavior_model2, dynamic_model2,
execution_model2, agent_2d_shape, agent_params,
goal_definition, map_interface)
world.AddAgent(agent2)
# viewer
viewer = MPViewer(params=params, use_world_bounds=True)
# World Simulation
sim_step_time = params["simulation"]["step_time",
"Step-time in simulation", 0.2]
sim_real_time_factor = params["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
# Draw map
video_renderer = VideoRenderer(renderer=viewer,
world_step_time=sim_step_time)
for _ in range(0, 5):
world.Step(sim_step_time)
viewer.clear()
video_renderer.drawWorld(world)
video_renderer.drawGoalDefinition(goal_definition)
time.sleep(sim_step_time / sim_real_time_factor)
video_renderer.export_video(filename="./test_video_intermediate",
remove_image_dir=True)
# This software is released under the MIT License.
# https://opensource.org/licenses/MIT
from modules.runtime.scenario.scenario_generation.uniform_vehicle_distribution import UniformVehicleDistribution
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
from modules.runtime.viewer.video_renderer import VideoRenderer
import os
try:
from bark.models.behavior import BehaviorUCTSingleAgent
except RuntimeError:
RuntimeError(
"BehaviorUCTSingleAgent not available, using ConstantVelocityModel")
scenario_param_file = "uct_planner.json" # must be within examples params folder
param_server = ParameterServer(
filename=os.path.join("examples/params/", scenario_param_file))
scenario_generation = UniformVehicleDistribution(num_scenarios=1,
random_seed=0,
params=param_server)
viewer = MPViewer(params=param_server,
x_range=[-16, 16],
y_range=[-2, 30],
follow_agent_id=True)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation", 0.2]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
scenario, idx = scenario_generation.get_next_scenario()
def test_three_way_intersection(self):
# threeway_intersection
xodr_parser = XodrParser(
"modules/runtime/tests/data/threeway_intersection.xodr")
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
open_drive_map = world.map.GetOpenDriveMap()
viewer = MPViewer(params=params, use_world_bounds=True)
comb_all = []
start_point = [Point2d(-30, -2)]
end_point_list = [Point2d(30, -2), Point2d(-2, -30)]
comb = list(itertools.product(start_point, end_point_list))
comb_all = comb_all + comb
# starting on the right
start_point = [Point2d(30, 2)]
end_point_list = [Point2d(-30, 2)]
comb = list(itertools.product(start_point, end_point_list))
comb_all = comb_all + comb
# starting on the bottom
start_point = [Point2d(2, -30)]
end_point_list = [Point2d(30, -2), Point2d(-30, 2)]
comb = list(itertools.product(start_point, end_point_list))
comb_all = comb_all + comb
# check few corridors
def GenerateRoadCorridor(map_interface, comb):
(start_p, end_p) = comb
polygon = Polygon2d([0, 0, 0], [
Point2d(-1, -1),
Point2d(-1, 1),
Point2d(1, 1),
Point2d(1, -1)
])
start_polygon = polygon.Translate(start_p)
goal_polygon = polygon.Translate(end_p)
rc = map_interface.GenerateRoadCorridor(start_p, goal_polygon)
return rc
# assert road ids
rc = GenerateRoadCorridor(map_interface, comb_all[0])
self.assertEqual(rc.road_ids, [0, 11, 1])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[1])
self.assertEqual(rc.road_ids, [0, 5, 2])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[2])
self.assertEqual(rc.road_ids, [1, 10, 0])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[3])
self.assertEqual(rc.road_ids, [2, 6, 1])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
rc = GenerateRoadCorridor(map_interface, comb_all[4])
self.assertEqual(rc.road_ids, [2, 4, 0])
self.assertEqual(len(rc.lane_corridors), 3)
self.assertTrue(rc.polygon.Valid())
def test_evaluator_drivable_area(self):
# World Definition
params = ParameterServer()
world = World(params)
# Model Definitions
behavior_model = BehaviorConstantVelocity(params)
execution_model = ExecutionModelInterpolate(params)
dynamic_model = SingleTrackModel(params)
# Map Definition
map_interface = MapInterface()
xodr_map = MakeXodrMapOneRoadTwoLanes()
map_interface.SetOpenDriveMap(xodr_map)
world.SetMap(map_interface)
#open_drive_map = world.map.GetOpenDriveMap()
#agent_2d_shape = CarLimousine()
agent_2d_shape = Polygon2d(
[1.25, 1, 0],
[Point2d(-1, -1),
Point2d(-1, 1),
Point2d(3, 1),
Point2d(3, -1)])
init_state = np.array([0, 3, -1.75, 0, 5])
agent_params = params.addChild("agent1")
goal_polygon = Polygon2d(
[1, 1, 0],
[Point2d(0, 0),
Point2d(0, 2),
Point2d(2, 2),
Point2d(2, 0)])
goal_polygon = goal_polygon.Translate(Point2d(50, -2))
agent = Agent(
init_state,
behavior_model,
dynamic_model,
execution_model,
agent_2d_shape,
agent_params,
GoalDefinitionPolygon(goal_polygon), # goal_lane_id
map_interface)
world.AddAgent(agent)
evaluator = EvaluatorDrivableArea()
world.AddEvaluator("drivable_area", evaluator)
info = world.Evaluate()
self.assertFalse(info["drivable_area"])
viewer = MPViewer(params=params, use_world_bounds=True)
# Draw map
viewer.drawGoalDefinition(goal_polygon,
color=(1, 0, 0),
alpha=0.5,
facecolor=(1, 0, 0))
viewer.drawWorld(world)
viewer.drawRoadCorridor(agent.road_corridor)
viewer.show(block=False)
from modules.runtime.viewer.matplotlib_viewer import MPViewer
import numpy as np
# Name and Output Directory
# CHANGE THIS #
map_name = "city_highway_straight"
output_dir = "/home/hart/Dokumente/2020/" + map_name
# Map Definition
xodr_parser = XodrParser("modules/runtime/tests/data/" + map_name + ".xodr")
if not os.path.exists(output_dir):
os.makedirs(output_dir)
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
open_drive_map = world.map.GetOpenDriveMap()
viewer = MPViewer(params=params,
use_world_bounds=True)
viewer.drawWorld(world)
viewer.saveFig(output_dir + "/" + "world_plain.png")
color_triplet_gray = (0.7,0.7,0.7)
def __init__(self,
params=ParameterServer(),
eval_agent=None):
GoalReached.__init__(self,
params,
eval_agent)
def _run_benchmark_config(self,
benchmark_config,
viewer=None,
maintain_history=False):
scenario = benchmark_config.scenario
behavior = benchmark_config.behavior
parameter_server = ParameterServer(json=scenario._json_params)
scenario_history = []
step = 0
try:
world = scenario.get_world_state()
except Exception as e:
self.logger.error(
"For config-idx {}, Exception thrown in scenario.get_world_state: {}"
.format(benchmark_config.config_idx, e))
self._append_exception(benchmark_config, e)
return {
"config_idx": benchmark_config.config_idx,
"scen_set": benchmark_config.scenario_set_name,
"scen_idx": benchmark_config.scenario_idx,
"step": step,
"behavior": benchmark_config.behavior_name,
"Terminal": "exception_raised"
}
# if behavior is not None (None specifies that also the default model can be evalauted)
if behavior:
world.agents[scenario._eval_agent_ids[0]].behavior_model = behavior
if maintain_history:
self._append_to_scenario_history(scenario_history, world, scenario)
self._reset_evaluators(world, scenario._eval_agent_ids)
step_time = parameter_server["Simulation"]["StepTime", "", 0.2]
if not isinstance(step_time, float):
step_time = 0.2
terminal = False
terminal_why = None
while not terminal:
try:
evaluation_dict = self._get_evalution_dict(world)
except Exception as e:
self.logger.error(
"For config-idx {}, Exception thrown in evaluation: {}".
format(benchmark_config.config_idx, e))
terminal_why = "exception_raised"
self._append_exception(benchmark_config, e)
evaluation_dict = {}
break
terminal, terminal_why = self._is_terminal(evaluation_dict)
if not terminal:
if viewer:
viewer.drawWorld(
world,
scenario._eval_agent_ids,
scenario_idx=benchmark_config.scenario_idx)
viewer.show(block=False)
time.sleep(step_time)
viewer.clear()
try:
world.Step(step_time)
except Exception as e:
self.logger.error(
"For config-idx {}, Exception thrown in world.Step: {}"
.format(benchmark_config.config_idx, e))
terminal_why = "exception_raised"
self._append_exception(benchmark_config, e)
break
if maintain_history:
self._append_to_scenario_history(scenario_history, world,
scenario)
step += 1
dct = {
"config_idx": benchmark_config.config_idx,
"scen_set": benchmark_config.scenario_set_name,
"scen_idx": benchmark_config.scenario_idx,
"step": step,
"behavior": benchmark_config.behavior_name,
**evaluation_dict, "Terminal": terminal_why
}
return dct, scenario_history
def test_database_run_and_analyze(self):
dbs = DatabaseSerializer(test_scenarios=4,
test_world_steps=2,
num_serialize_scenarios=2)
dbs.process("data/database1")
local_release_filename = dbs.release(version="test")
db = BenchmarkDatabase(database_root=local_release_filename)
evaluators = {
"success": "EvaluatorGoalReached",
"collision": "EvaluatorCollisionEgoAgent",
"max_steps": "EvaluatorStepCount"
}
terminal_when = {
"collision": lambda x: x,
"max_steps": lambda x: x > 2
}
params = ParameterServer(
) # only for evaluated agents not passed to scenario!
behaviors_tested = {
"IDM": BehaviorIDMClassic(params),
"Const": BehaviorConstantVelocity(params)
}
benchmark_runner = BenchmarkRunnerMP(benchmark_database=db,
evaluators=evaluators,
terminal_when=terminal_when,
behaviors=behaviors_tested,
log_eval_avg_every=10)
result = benchmark_runner.run(maintain_history=True)
result.dump(os.path.join("./benchmark_results.pickle"))
result_loaded = BenchmarkResult.load(
os.path.join("./benchmark_results.pickle"))
params2 = ParameterServer()
fig = plt.figure(figsize=[10, 10])
viewer = MPViewer(params=params2,
center=[5112, 5165],
y_length=120,
enforce_y_length=True,
axis=fig.gca())
analyzer = BenchmarkAnalyzer(benchmark_result=result_loaded)
configs = analyzer.find_configs(criteria={
"behavior": lambda x: x == "IDM",
"success": lambda x: not x
})
configs_const = analyzer.find_configs(criteria={
"behavior": lambda x: x == "Const",
"success": lambda x: not x
})
#analyzer.visualize(configs_idx_list = configs,
# viewer = viewer, real_time_factor=10, fontsize=12)
plt.close(fig)
fig, (ax1, ax2) = plt.subplots(1, 2)
viewer1 = MPViewer(params=params2,
center=[5112, 5165],
y_length=120,
enforce_y_length=True,
axis=ax1)
viewer2 = MPViewer(params=params2,
center=[5112, 5165],
y_length=120,
enforce_y_length=True,
axis=ax2)
analyzer.visualize(configs_idx_list=[configs[1:3], configs_const[1:3]],
viewer=[viewer1, viewer2],
viewer_names=["IDM", "ConstVelocity"],
real_time_factor=1,
fontsize=12)
def test_one_agent_at_goal_state_limits_frenet(self):
param_server = ParameterServer()
# Model Definition
behavior_model = BehaviorConstantVelocity(param_server)
execution_model = ExecutionModelInterpolate(param_server)
dynamic_model = SingleTrackModel(param_server)
# Agent Definition
agent_2d_shape = CarLimousine()
agent_params = param_server.addChild("agent1")
center_line = Line2d()
center_line.AddPoint(Point2d(5.0, 5.0))
center_line.AddPoint(Point2d(10.0, 10.0))
center_line.AddPoint(Point2d(20.0, 10.0))
max_lateral_dist = (0.4, 1)
max_orientation_diff = (0.08, 0.1)
velocity_range = (20.0, 25.0)
goal_definition = GoalDefinitionStateLimitsFrenet(
center_line, max_lateral_dist, max_orientation_diff,
velocity_range)
# not at goal x,y, others yes
agent1 = Agent(np.array([0, 6, 8, 3.14 / 4.0, velocity_range[0]]),
behavior_model, dynamic_model, execution_model,
agent_2d_shape, agent_params, goal_definition, None)
# at goal x,y and others
agent2 = Agent(np.array([0, 5.0, 5.5, 3.14 / 4.0, velocity_range[1]]),
behavior_model, dynamic_model, execution_model,
agent_2d_shape, agent_params, goal_definition, None)
# not at goal x,y,v yes but not orientation
agent3 = Agent(
np.array(
[0, 5, 5.5, 3.14 / 4.0 + max_orientation_diff[1] + 0.001,
20]), behavior_model, dynamic_model, execution_model,
agent_2d_shape, agent_params, goal_definition, None)
# not at goal x,y, orientation but not v
agent4 = Agent(
np.array([
0, 5, 4.5, 3.14 / 4 - max_orientation_diff[0],
velocity_range[0] - 0.01
]), behavior_model, dynamic_model, execution_model, agent_2d_shape,
agent_params, goal_definition, None)
# at goal x,y, at lateral limit
agent5 = Agent(
np.array([
0, 15, 10 - max_lateral_dist[0] + 0.05, 0, velocity_range[1]
]), behavior_model, dynamic_model, execution_model, agent_2d_shape,
agent_params, goal_definition, None)
# not at goal x,y slightly out of lateral limit
agent6 = Agent(
np.array([
0, 15, 10 + max_lateral_dist[0] + 0.05,
3.14 / 4 + max_orientation_diff[0], velocity_range[0]
]), behavior_model, dynamic_model, execution_model, agent_2d_shape,
agent_params, goal_definition, None)
# not at goal x,y,v yes but not orientation
agent7 = Agent(
np.array(
[0, 5, 5.5, 3.14 / 4.0 - max_orientation_diff[0] - 0.001,
20]), behavior_model, dynamic_model, execution_model,
agent_2d_shape, agent_params, goal_definition, None)
world = World(param_server)
world.AddAgent(agent1)
world.AddAgent(agent2)
world.AddAgent(agent3)
world.AddAgent(agent4)
world.AddAgent(agent5)
world.AddAgent(agent6)
world.AddAgent(agent7)
evaluator1 = EvaluatorGoalReached(agent1.id)
evaluator2 = EvaluatorGoalReached(agent2.id)
evaluator3 = EvaluatorGoalReached(agent3.id)
evaluator4 = EvaluatorGoalReached(agent4.id)
evaluator5 = EvaluatorGoalReached(agent5.id)
evaluator6 = EvaluatorGoalReached(agent6.id)
evaluator7 = EvaluatorGoalReached(agent7.id)
world.AddEvaluator("success1", evaluator1)
world.AddEvaluator("success2", evaluator2)
world.AddEvaluator("success3", evaluator3)
world.AddEvaluator("success4", evaluator4)
world.AddEvaluator("success5", evaluator5)
world.AddEvaluator("success6", evaluator6)
world.AddEvaluator("success7", evaluator7)
info = world.Evaluate()
self.assertEqual(info["success1"], False)
self.assertEqual(info["success2"], True)
self.assertEqual(info["success3"], False)
self.assertEqual(info["success4"], False)
self.assertEqual(info["success5"], True)
self.assertEqual(info["success6"], False)
self.assertEqual(info["success7"], False)
from bark.models.behavior import *
from bark.world import *
from bark.world.map import *
from bark.world.goal_definition import GoalDefinitionPolygon
from bark.models.dynamic import *
from bark.models.execution import *
from bark.geometry import *
from bark.geometry.standard_shapes import *
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.pygame_viewer import PygameViewer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
from modules.runtime.viewer.panda3d_viewer import Panda3dViewer
from modules.runtime.commons.xodr_parser import XodrParser
# Parameters Definitions
param_server = ParameterServer(
filename="examples/params/od8_const_vel_one_agent.json")
# set parameter that is accessible in Python as well as cpp
# param_server.setReal("wheel_base", 0.8)
# World Definition
world = World(param_server)
# Model Definitions
behavior_model = BehaviorConstantVelocity(param_server)
execution_model = ExecutionModelInterpolate(param_server)
dynamic_model = SingleTrackModel()
# Map Definition
xodr_parser = XodrParser("modules/runtime/tests/data/Crossing8Course.xodr")
map_interface = MapInterface()
map_interface.set_open_drive_map(xodr_parser.map)
# Copyright (c) 2020 Julian Bernhard, Klemens Esterle, Patrick Hart and
# Tobias Kessler
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
from modules.runtime.scenario.scenario_generation.interaction_dataset_scenario_generation import \
InteractionDatasetScenarioGeneration
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
from modules.runtime.viewer.video_renderer import VideoRenderer
import os
import argparse
# set you json config that contains a map and matching tracks.
param_server = ParameterServer(
filename=os.path.join("examples/params/interaction_example.json"))
scenario_generation = InteractionDatasetScenarioGeneration(num_scenarios=1,
random_seed=0,
params=param_server)
viewer = MPViewer(params=param_server, use_world_bounds=True)
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation", 0.2]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
scenario = scenario_generation.create_scenarios(param_server, 1)[0]
world_state = scenario.GetWorldState()
sim_time_steps = param_server["simulation"]["simulation_time_steps",
def test_road_corridor_forward(self):
xodr_parser = XodrParser(
"modules/runtime/tests/data/road_corridor_test.xodr")
# World Definition
params = ParameterServer()
world = World(params)
map_interface = MapInterface()
map_interface.SetOpenDriveMap(xodr_parser.map)
world.SetMap(map_interface)
open_drive_map = world.map.GetOpenDriveMap()
viewer = MPViewer(params=params, use_world_bounds=True)
# Draw map
viewer.drawWorld(world)
viewer.show(block=False)
# Generate RoadCorridor
roads = [0, 1, 2]
driving_direction = XodrDrivingDirection.forward
map_interface.GenerateRoadCorridor(roads, driving_direction)
road_corridor = map_interface.GetRoadCorridor(roads, driving_direction)
# Assert road corridor
# Assert: 3 roads
self.assertEqual(len(road_corridor.roads), 3)
# Assert: road1: 2 lanes, road2: 1 lane, road3: 1 lane
self.assertEqual(len(road_corridor.GetRoad(0).lanes), 3)
self.assertEqual(len(road_corridor.GetRoad(1).lanes), 2)
self.assertEqual(len(road_corridor.GetRoad(2).lanes), 3)
# Assert: left and right lanes
self.assertEqual(
road_corridor.GetRoad(0).GetLane(2).right_lane.lane_id, 3)
self.assertEqual(
road_corridor.GetRoad(0).GetLane(3).left_lane.lane_id, 2)
self.assertEqual(
road_corridor.GetRoad(2).GetLane(7).right_lane.lane_id, 8)
self.assertEqual(
road_corridor.GetRoad(2).GetLane(8).left_lane.lane_id, 7)
# Assert: next road
self.assertEqual(road_corridor.GetRoad(0).next_road.road_id, 1)
self.assertEqual(road_corridor.GetRoad(1).next_road.road_id, 2)
# Assert: lane links
self.assertEqual(
road_corridor.GetRoad(0).GetLane(3).next_lane.lane_id, 5)
self.assertEqual(
road_corridor.GetRoad(1).GetLane(5).next_lane.lane_id, 8)
# Assert: LaneCorridor
self.assertEqual(len(road_corridor.lane_corridors), 3)
colors = ["blue", "red", "green"]
count = 0
for lane_corridor in road_corridor.lane_corridors:
viewer.drawPolygon2d(lane_corridor.polygon,
color=colors[count],
alpha=0.5)
viewer.drawLine2d(lane_corridor.left_boundary, color="red")
viewer.drawLine2d(lane_corridor.right_boundary, color="blue")
viewer.drawLine2d(lane_corridor.center_line, color="black")
viewer.show(block=False)
plt.pause(2.)
count += 1
def create_single_scenario(self):
scenario = Scenario(map_file_name=self._map_file_name,
json_params=self._params.convert_to_dict())
world = scenario.get_world_state()
collected_sources_sinks_agent_states_geometries = []
collected_sources_sinks_default_param_configs = []
# Loop through each source sink config and first only create state
# and geometry information
road_corridors = []
kwargs_agent_states_geometry = []
sink_source_default_params = []
for idx, sink_source_config in enumerate(self._sinks_sources):
road_corridor = self.get_road_corridor_from_source_sink(sink_source_config, world.map)
road_corridors.append(road_corridor)
#1) create agent states and geometries for this source
args = [road_corridor]
agent_states, agent_geometries, kwargs_dict, default_params_state_geometry = \
self.eval_configuration( sink_source_config, "ConfigAgentStatesGeometries",
args, {})
kwargs_agent_states_geometry.append(kwargs_dict)
# collect default parameters of this config
sink_source_default_params.append(sink_source_config)
sink_source_default_params[idx]["ConfigAgentStatesGeometries"] = default_params_state_geometry.convert_to_dict()
collected_sources_sinks_agent_states_geometries.append((agent_states, agent_geometries))
#2 remove overlapping agent states from different sources and sinks
collected_sources_sinks_agent_states_geometries = \
self.resolve_overlaps_in_sources_sinks_agents(collected_sources_sinks_agent_states_geometries)
agent_list = []
controlled_agent_ids_all = []
for idx, agent_states_geometries in enumerate(
collected_sources_sinks_agent_states_geometries):
sink_source_config = self._sinks_sources[idx]
agent_states = agent_states_geometries[0]
agent_geometries = agent_states_geometries[1]
road_corridor = road_corridors[idx]
if(len(agent_states)== 0):
continue
#3) create behavior, execution and dynamic models
args_list = [road_corridor, agent_states ]
kwargs_dict = {**kwargs_agent_states_geometry[idx]}
config_return, kwargs_dict_tmp, default_params_behavior = \
self.eval_configuration(
sink_source_config, "ConfigBehaviorModels",
args_list, kwargs_dict)
behavior_models = config_return
sink_source_default_params[idx]["ConfigBehaviorModels"] = default_params_behavior.convert_to_dict()
kwargs_dict = {**kwargs_dict, **kwargs_dict_tmp}
config_return, kwargs_dict_tmp, default_params_execution = \
self.eval_configuration(
sink_source_config, "ConfigExecutionModels",
args_list, kwargs_dict)
execution_models = config_return
sink_source_default_params[idx]["ConfigExecutionModels"] = default_params_execution.convert_to_dict()
kwargs_dict = {**kwargs_dict, **kwargs_dict_tmp}
config_return, kwargs_dict_tmp, default_params_dynamic = \
self.eval_configuration(
sink_source_config, "ConfigDynamicModels",
args_list, kwargs_dict)
dynamic_models = config_return
sink_source_default_params[idx]["ConfigDynamicModels"] = default_params_dynamic.convert_to_dict()
kwargs_dict = {**kwargs_dict, **kwargs_dict_tmp}
#4 create goal definitions and controlled agents
config_return, kwargs_dict_tmp, default_params_controlled_agents = \
self.eval_configuration(
sink_source_config, "ConfigControlledAgents",
args_list, kwargs_dict)
controlled_agent_ids = config_return
controlled_agent_ids_all.extend(controlled_agent_ids)
sink_source_default_params[idx]["ConfigControlledAgents"] = default_params_controlled_agents.convert_to_dict()
kwargs_dict = {**kwargs_dict, **kwargs_dict_tmp}
args_list = [*args_list, controlled_agent_ids]
config_return, kwargs_dict_tmp, default_params_goals = \
self.eval_configuration(
sink_source_config, "ConfigGoalDefinitions",
args_list, kwargs_dict)
goal_definitions = config_return
sink_source_default_params[idx]["ConfigGoalDefinitions"] = default_params_goals.convert_to_dict()
#5 Build all agents for this source config
agent_params = ParameterServer(json = sink_source_config["AgentParams"])
sink_source_agents = self.create_source_config_agents(agent_states,
agent_geometries, behavior_models, execution_models,
dynamic_models, goal_definitions, controlled_agent_ids,
world, agent_params)
sink_source_default_params[idx]["AgentParams"] = agent_params.convert_to_dict()
agent_list.extend(sink_source_agents)
collected_sources_sinks_default_param_configs.append(sink_source_config)
self._sink_source_default_params = sink_source_default_params
scenario._agent_list = self.update_agent_ids(agent_list)
scenario._eval_agent_ids = [idx for idx, value in enumerate(controlled_agent_ids_all) if value==True]
return scenario
def __init__(self, params=ParameterServer()):
ClosestAgentsObserver.__init__(self, params)
self._perform_lane_change = False
self._observation_len = \
self._len_ego_state + \
self._max_num_vehicles*self._len_relative_agent_state + 1
# This software is released under the MIT License.
# https://opensource.org/licenses/MIT
import numpy as np
import time
from modules.runtime.commons.parameters import ParameterServer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
from modules.runtime.viewer.video_renderer import VideoRenderer
from modules.runtime.scenario.scenario_generation.config_with_ease import \
LaneCorridorConfig, ConfigWithEase
from modules.runtime.runtime import Runtime
from modules.runtime.viewer.panda3d_easy import Panda3dViewer
from bark.models.behavior import *
# parameters
param_server = ParameterServer()
param_server["BehaviorLaneChangeRuleBased"]["MinVehicleRearDistance"] = 4.
param_server["BehaviorLaneChangeRuleBased"]["MinVehicleFrontDistance"] = 2.
param_server["BehaviorLaneChangeRuleBased"]["TimeKeepingGap"] = 0.
# custom lane configuration that sets a different behavior model
# and sets the desired speed for the behavior
class HighwayLaneCorridorConfig(LaneCorridorConfig):
def __init__(self, params=None, **kwargs):
super().__init__(params=params, **kwargs)
self._count = 0
def velocity(self):
return np.random.uniform(2., 10.)
class Cosimulation:
def __init__(self):
self.carla_server = None
self.carla_client = None
self.carla_controller = None
self.bark_viewer = None
self.cosimulation_viewer = None
self.cam_manager = None
self.launch_args = ["external/carla/CarlaUE4.sh", "-quality-level=Low"]
# Bark parameter server
self.param_server = ParameterServer(
filename=BARK_PATH +
"examples/params/od8_const_vel_one_agent.json")
# World Definition
self.bark_world = World(self.param_server)
# Model Definitions
self.behavior_model = BehaviorIDMClassic(self.param_server)
self.execution_model = ExecutionModelInterpolate(self.param_server)
self.dynamic_model = SingleTrackModel(self.param_server)
# Map Definition
xodr_parser = XodrParser(BARK_PATH + "modules/runtime/tests/data/" +
BARK_MAP + ".xodr")
self.map_interface = MapInterface()
self.map_interface.SetOpenDriveMap(xodr_parser.map)
self.bark_world.SetMap(self.map_interface)
# Bark agent definition
self.agent_2d_shape = CarLimousine()
# use for converting carla actor id to bark agent id
self.carla_2_bark_id = dict()
# store the camera id attached to an agent
self.carla_agents_cam = dict()
def initialize_viewer(self):
# Viewer of Bark simulation, the pygame surface will be extracted
self.bark_viewer = PygameViewer(params=self.param_server,
use_world_bounds=True,
screen_dims=BARK_SCREEN_DIMS)
# Viewer of cosimulation
# Set the number of cameras to show both simulation side by side
# windows from Bark simulation & camera image from Carla simulation
self.cosimulation_viewer = CosimulationViewer(BARK_SCREEN_DIMS,
num_cameras=NUM_CAMERAS)
def close(self):
pg.display.quit()
pg.quit()
# kill the child of the subprocess
# sometimes the socket is not killed, blocking the launch of carla
# server
os.system("fuser {}/tcp -k".format(CARLA_PORT))
exit()
def launch_carla_server(self):
self.carla_server = subprocess.Popen(
self.launch_args[0] if not CARLA_LOW_QUALITY else self.launch_args)
# Wait for launching carla
time.sleep(8)
def connect_carla_server(self):
"""
create a carla client and try connect to carla server
"""
self.carla_client = CarlaClient()
self.carla_client.connect(carla_map=CARLA_MAP,
port=CARLA_PORT,
timeout=10)
self.carla_client.set_synchronous_mode(SYNCHRONOUS_MODE, DELTA_SECOND)
def spawn_npc_agents(self, num_agents):
"""spawn npc agents in both Carla and Bark
Arguments:
num_agents {int} -- number of agents to be spawned
"""
for i in range(num_agents):
carla_agent_id = self.carla_client.spawn_random_vehicle(
num_retries=5)
if carla_agent_id is not None:
self.carla_client.set_autopilot(carla_agent_id, True)
# spawn agent object in BARK
agent_params = self.param_server.addChild("agent{}".format(i))
bark_agent = Agent(
np.empty(5),
self.behavior_model,
self.dynamic_model,
self.execution_model,
self.agent_2d_shape,
agent_params,
None, # goal_lane_id
self.map_interface)
self.bark_world.AddAgent(bark_agent)
self.carla_2_bark_id[carla_agent_id] = bark_agent.id
if len(self.carla_2_bark_id) != num_agents:
logging.warning(
"Some agents cannot be spawned due to collision in the spawning location, {} agents are spawned"
.format(len(self.carla_2_bark_id)))
else:
logging.info("{} agents spawned sucessfully.".format(num_agents))
def initialize_camera_manager(self, surfaces):
"""create object for fetching image from carla
Arguments:
surfaces {list} -- list of pygame surfaces
"""
self.cam_manager = CameraManager(surfaces,
synchronous_mode=SYNCHRONOUS_MODE)
def simulation_loop(self):
self.bark_viewer.clear()
self.cosimulation_viewer.tick()
if SYNCHRONOUS_MODE:
frame_id = self.carla_client.tick()
self.cam_manager.fetch_image(frame_id)
self.cosimulation_viewer.update_cameras(self.cam_manager.surfaces)
# get agents' state in carla, and fill the state into bark
carla_agent_states = self.carla_client.get_vehicles_state(
self.carla_2_bark_id)
self.bark_world.fillWorldFromCarla(DELTA_SECOND, carla_agent_states)
self.bark_viewer.drawWorld(self.bark_world, show=False)
self.cosimulation_viewer.update_bark(self.bark_viewer.screen_surface)
self.cosimulation_viewer.show()
def model_from_model_type(self, model_type):
params = ParameterServer()
bark_model = eval("{}(params)".format(model_type))
return bark_model
from modules.runtime.commons.parameters import ParameterServer
from configurations.bark_agent import BARKMLBehaviorModel
from configurations.sac_highway.configuration import SACHighwayConfiguration
params = ParameterServer(
filename="examples/params/deterministic_scenario.json")
# NOTE(@all): this won't work as a test since we need to have writing access
# to the summaries folder in bark-ml
# bark_agent is of type behavior model (can be a trained ml agent)
# sac_configuration = SACHighwayConfiguration(params)
# bark_agent = BARKMLBehaviorModel(configuration=sac_configuration)
