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 test_motion_primitives_concat_state(self):
params = ParameterServer(
filename="modules/runtime/tests/data/highway_merging.json")
scenario_generation = UniformVehicleDistribution(num_scenarios=3,
random_seed=0,
params=params)
state_observer = StateConcatenation()
action_wrapper = MotionPrimitives()
evaluator = GoalReached()
viewer = PygameViewer(
params=params, use_world_bounds=True
) # x_range=[-40,40], y_range=[-40,40], follow_agent_id=True)
runtimerl = RuntimeRL(action_wrapper=action_wrapper,
nn_observer=state_observer,
evaluator=evaluator,
step_time=0.2,
viewer=viewer,
scenario_generator=scenario_generation)
for _ in range(0, 5): # run 5 scenarios in a row, repeating after 3
nn_state = runtimerl.reset()
for _ in range(0, 10): # run each scenario for 10 steps
next_nn_state, reward, done, info = runtimerl.step(
action_wrapper.action_space.sample())
runtimerl.render()
print("State: {} \n Reward: {} \n Done {}, Info: {} \n \
=================================================".
format(next_nn_state, reward, done, info))
agent_2d_shape2 = CarLimousine()
init_state2 = np.array([0, -11, -8, 3.14 * 3.0 / 4.0, 5.2])
agent_params2 = param_server.addChild("agent2")
agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2,
agent_2d_shape2, agent_params2,
GoalDefinitionPolygon(goal_polygon), map_interface)
world.add_agent(agent2)
# viewer
"""
viewer = Panda3dViewer(params=param_server,
x_range=[-200, 200],
y_range=[-200, 200],)
"""
viewer = PygameViewer(params=param_server,
x_range=[-200, 200],
y_range=[-200, 200])
# World Simulation
sim_step_time = param_server["simulation"]["step_time",
"Step-time used in simulation", 1]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 1]
for _ in range(0, 30):
world.step(sim_step_time)
viewer.drawWorld(world)
viewer.show(block=False)
time.sleep(sim_step_time / sim_real_time_factor)
param_server.save(
agent = Agent(
init_state,
behavior_model,
dynamic_model,
execution_model,
agent_2d_shape,
agent_params,
GoalDefinition(goal_polygon), # goal_lane_id
map_interface)
world.add_agent(agent)
# viewer
viewer = PygameViewer(params=param_server,
x_range=[-50, 50],
y_range=[-50, 50],
follow_agent_id=agent.id,
screen_dims=[500, 500])
#viewer = MPViewer(params=param_server)
# World Simulation
sim_step_time = param_server["simulation"]["step_time",
"Step-time in simulation", 0.05]
sim_real_time_factor = param_server["simulation"][
"real_time_factor", "execution in real-time or faster", 100]
for _ in range(0, 100):
viewer.clear()
world.step(sim_step_time)
viewer.drawWorld(world)
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.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)
self.carla_controller = Controller(self.carla_client)
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, carla_ego_id):
bark_ego_id = self.carla_2_bark_id[carla_ego_id]
self.bark_viewer.clear()
self.cosimulation_viewer.tick()
agent_state_map = self.carla_client.get_vehicles_state(
self.carla_2_bark_id)
self.bark_world.fillWorldFromCarla(0, agent_state_map)
plan = self.bark_world.plan_agents(DELTA_SECOND,
[bark_ego_id])[bark_ego_id]
self.carla_controller.control(
self.carla_client.get_actor(carla_ego_id), plan[-2][1:3],
plan[-1][1:3], plan[-1][4], plan[-1][3])
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,
eval_agent_ids=[bark_ego_id])
self.cosimulation_viewer.update_bark(self.bark_viewer.screen_surface)
self.cosimulation_viewer.show()
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
scenario, idx = scenario_generation.get_next_scenario()
world_state = scenario.get_world_state()
print("Running scenario {} of {}".format(
idx, scenario_generation.num_scenarios))
for _ in range(0, 10): # run each scenario for 3 steps
world_state.step(sim_step_time)
viewer.drawWorld(world_state)
viewer.show(block=False)
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.pygame_viewer import PygameViewer
from modules.runtime.viewer.threaded_viewer import ThreadedViewer
import time
import os
scenario_param_file ="highway_merging.json" # must be within examples params 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)
viewer = PygameViewer(params=param_server, x_range=[-50,50], y_range=[-20,80], 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, idx = scenario_generation.get_next_scenario()
world_state = scenario.get_world_state()
print("Running scenario {} of {}".format(idx, scenario_generation.num_scenarios))
for _ in range(0, 100): # run each scenario for 3 steps
world_state.do_planning(sim_step_time)
viewer.drawWorld(world_state)
# Copyright (c) 2019 fortiss GmbH
#
# This software is released under the MIT License.
# https://opensource.org/licenses/MIT
from modules.runtime.scenario.scenario_generation.scenario_generation import ScenarioGeneration
from modules.runtime.viewer.pygame_viewer import PygameViewer
from modules.runtime.viewer.matplotlib_viewer import MPViewer
from modules.runtime.commons.parameters import ParameterServer
import time
param_server = ParameterServer()
viewer = PygameViewer(params=param_server,
x_range=[4950, 5250],
y_range=[5100, 5300])
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]
scenario_generation = ScenarioGeneration()
scenario_generation.load_scenario_list(
filename="examples/scenarios/highway_merging.bark_scenarios")
for _ in range(0, 5): # run 5 scenarios in a row, repeating after 3
scenario, idx = scenario_generation.get_next_scenario()
world_state = scenario.get_world_state()
print("Running scenario {} of {}".format(
idx, scenario_generation.num_scenarios))
