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()
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(
os.path.join(os.path.dirname(os.path.abspath(__file__)), "params",
"od8_const_vel_one_agent_written.json"))
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)
time.sleep(sim_step_time / sim_real_time_factor)
scenario_generation.dump_scenario_list(
os.path.join(
scenario_dump_folder,
"{}.bark_scenarios".format(os.path.splitext(scenario_param_file)[0])))