def convert_scenario(input_folder, output_folder):
if not os.path.isdir(output_folder):
os.makedirs(output_folder)
file_names = os.listdir(input_folder)
for file_name in tqdm(file_names):
label_path = input_folder + file_name
label = load_label(label_path)
# Use the result of moving object classifier to set corresponding the dynamic constraints
if os.path.exists(args.dyna_obj_folder + file_name):
file_path = args.init_scenario_folder + dyna_init_scenario
else:
file_path = args.init_scenario_folder + static_init_scenario
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
for i in range(len(label)):
if label[i][0] != 'Car' and label[i][0] != 'Van' and label[i][
0] != 'Truck' and label[i][0] != 'Misc':
continue
# regulate orientation to [-pi, pi]
orient = label[i][7]
while orient < -np.pi:
orient += 2 * np.pi
while orient > np.pi:
orient -= 2 * np.pi
static_obstacle_id = scenario.generate_object_id()
static_obstacle_type = ObstacleType.PARKED_VEHICLE
static_obstacle_shape = Rectangle(width=label[i][5][1],
length=label[i][5][2])
static_obstacle_initial_state = State(
position=np.array([label[i][6][2], -label[i][6][0]]),
orientation=-(orient - 0.5 * np.pi),
time_step=0)
static_obstacle = StaticObstacle(static_obstacle_id,
static_obstacle_type,
static_obstacle_shape,
static_obstacle_initial_state)
scenario.add_objects(static_obstacle)
author = 'Jindi Zhang'
affiliation = 'City University of Hong Kong'
source = ''
tags = {Tag.CRITICAL, Tag.INTERSTATE}
fw = CommonRoadFileWriter(scenario, planning_problem_set, author,
affiliation, source, tags)
output_file_name = output_folder + '/' + file_name.split(
'.')[0] + '.xml'
fw.write_to_file(output_file_name, OverwriteExistingFile.ALWAYS)
def setUp(self):
"""Load the osm file and convert it to a scenario."""
try:
commonroad_reader = CommonRoadFileReader(
os.path.dirname(os.path.realpath(__file__)) +
f"/osm_xml_test_files/{self.xml_file_name}.xml")
scenario, _ = commonroad_reader.open()
except etree.XMLSyntaxError as xml_error:
print(f"SyntaxERror: {xml_error}")
print(
"There was an error during the loading of the selected CommonRoad file.\n"
)
l2osm = L2OSMConverter(self.proj_string)
self.osm = l2osm(scenario)
def main():
"""Short helper file to visualize an xml file
as a command line tool.
"""
args = parse_arguments()
filename = args.xml_file
scenario, _ = CommonRoadFileReader(filename).open()
draw_params = {
"scenario": {"lanelet_network": {"lanelet": {"show_label": args.show_labels}}}
}
# temporary fix to get a plotable view of the scenario
plot_center = scenario.lanelet_network.lanelets[0].left_vertices[0]
plt.style.use("classic")
plt.figure(figsize=(10, 10))
plt.gca().axis("equal")
plot_displacement_x = plot_displacement_y = 200
plot_limits = [
plot_center[0] - plot_displacement_x,
plot_center[0] + plot_displacement_x,
plot_center[1] - plot_displacement_y,
plot_center[1] + plot_displacement_y,
]
draw_object(scenario, plot_limits=plot_limits, draw_params=draw_params)
plt.show()
def openPath(self, path):
"""
Args:
path:
Returns:
"""
filename = os.path.basename(path)
self.inputCommonRoadFile.setText(filename)
try:
# fh = open(path, "rb")
# data = fh.read()
# fh.close()
commonroad_reader = CommonRoadFileReader(path)
scenario, _ = commonroad_reader.open()
except etree.XMLSyntaxError as e:
errorMsg = "Syntax Error: {}".format(e)
QMessageBox.warning(
self,
"CommonRoad XML error",
"There was an error during the loading of the selected CommonRoad file.\n\n{}".format(
errorMsg
),
QMessageBox.Ok,
)
return
except Exception as e:
errorMsg = "{}".format(e)
QMessageBox.warning(
self,
"CommonRoad XML error",
"There was an error during the loading of the selected CommonRoad file.\n\n{}".format(
errorMsg
),
QMessageBox.Ok,
)
return
self.openScenario(scenario)
def main():
args = setup_arg_parser()
parsed_args = args.parse_args()
scenario_file_name = 'C-USA_Lanker-1_1_T-1.xml'
file_path = load_scenarion_file(scenario_file_name)
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
result = add_simple_object_or_fail(scenario, *(vars(parsed_args).values()))
out = format_insert_for_communication(result)
if (parsed_args.get_state):
state = get_object_state(scenario, parsed_args.get_state) # 11434
out += " " + format_state_for_communication(state)
print(out)
def main() -> None:
figure = plt.figure(figsize=(19.20, 10.80), dpi=100)
plt.xlim([100, 220])
plt.ylim([25, 100])
file_path: os.path = os.path.join(
os.getcwd(), '../scenarios/DEU_B471-1_1_T-1_mod_2.xml')
common_road_input: Tuple[
Scenario, PlanningProblemSet] = CommonRoadFileReader(file_path).open()
scenario: Scenario = common_road_input[0]
planning_problem: Optional[PlanningProblem] = common_road_input[
1].planning_problem_dict.get(800)
# Draw goal region
DrawHelp.draw(DrawHelp.convert_to_drawable(planning_problem.goal))
plt.annotate("goal region",
xy=(156, 62),
xytext=(160, 55),
arrowprops=arrowprops)
# Draw lanes
DrawHelp.draw(DrawHelp.convert_to_drawable(scenario.lanelet_network))
# Draw static obstacles
for obstacle in scenario.static_obstacles:
DrawHelp.draw(DrawHelp.convert_to_drawable(obstacle))
x, y = obstacle.occupancy_at_time(0).shape.center
plt.annotate("static obstacle",
xy=(x + 2, y + 3),
xytext=(x, y + 9),
arrowprops=arrowprops)
# Generates frames of dynamic obstacles
frames: List[List[Artist]] = createDynamicObstaclesArtists(
scenario.dynamic_obstacles)
# Add predictions of ego vehicle to the frames
extend_frames_with_prediction(planning_problem.initial_state, frames,
scenario.dt)
# Add an animation of the frames
# NOTE The assignment is needed to force execution
ani: ArtistAnimation = ArtistAnimation(figure,
frames,
blit=True,
interval=int(
round(scenario.dt * 1000)),
repeat=True)
ani.save("2018-12-26_BasicUnoptimzedScenario.mp4", writer="ffmpeg")
plt.gca().set_aspect('equal')
plt.show()
def main():
file_path: os.path = os.path.join(
os.getcwd(), '../scenarios/DEU_B471-1_1_T-1_mod_2.xml')
common_road: Tuple[Scenario, PlanningProblemSet] = CommonRoadFileReader(
file_path).open()
scenario: Scenario = common_road[0]
planning_problem_set: PlanningProblemSet = common_road[1]
plt.figure(figsize=(25, 10))
draw_object(scenario, draw_params=draw_params)
draw_object(planning_problem_set, draw_params=draw_params)
plt.gca().set_aspect('equal')
plt.show()
def commonroad_to_osm(args, output_name: str):
"""Convert from CommonRoad to OSM.
Args:
args: Object containing parsed arguments.
output_name: Name of file where result should be written to.
"""
try:
commonroad_reader = CommonRoadFileReader(args.input_file)
scenario, _ = commonroad_reader.open()
except etree.XMLSyntaxError as xml_error:
print(f"SyntaxERror: {xml_error}")
print(
"There was an error during the loading of the selected CommonRoad file.\n"
)
l2osm = L2OSMConverter(args.proj)
osm = l2osm(scenario)
with open(f"{output_name}", "wb") as file_out:
file_out.write(
etree.tostring(osm,
xml_declaration=True,
encoding="UTF-8",
pretty_print=True))
def load_scenario(path: str) -> Tuple[Scenario, PlanningProblem]:
"""
Loads the given common road scenario.
:param path: The relative path to the common road file to load. The base of the relative path is based on
os.getcwd().
:type path: str
:return: A tuple returning the loaded scenario and the first planning problem found.
"""
scenario_path: os.path = os.path.join(os.getcwd(), path)
scenario, planning_problem_set = CommonRoadFileReader(scenario_path) \
.open()
if planning_problem_set:
for _, planning_problem in planning_problem_set.planning_problem_dict.items(
):
return scenario, planning_problem
def load_scenario(scenario_path):
# open and read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(scenario_path).open()
return scenario, planning_problem_set
def open_scenario(scenario_file):
"""Opens a scenario from file"""
return CommonRoadFileReader(os.getcwd() + scenario_file).open_scenario()
def main() -> None:
# Read CommonRoad scenario
cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \
.open()
if cr_planning_problem_set:
for _, pp in cr_planning_problem_set.planning_problem_dict.items():
cr_planning_problem = pp
break # Read only the first one
else:
raise Exception(
"The given CommonRoad scenario does not define a planning problem."
)
# Setup BeamNG
bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path)
bng_scenario = Scenario(
bng_scenario_environment,
bng_scenario_name,
authors='Stefan Huber',
description='Simple visualization of the CommonRoad scenario ' +
cr_scenario_name)
# Add lane network
lanes = cr_scenario.lanelet_network.lanelets
for lane in lanes:
lane_nodes = []
for center in lane.center_vertices:
lane_nodes.append((center[0], center[1], 0,
4)) # FIXME Determine appropriate width
road = Road(cr_road_material)
road.nodes.extend(lane_nodes)
bng_scenario.add_road(road)
# Add ego vehicle
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Cornflowerblue')
ego_init_state = cr_planning_problem.initial_state
ego_init_state.position[0] = 82.8235
ego_init_state.position[1] = 31.5786
add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state,
etk800_z_offset)
obstacles_to_move = dict()
# Add truck
semi = Vehicle('truck', model='semi', color='Red')
semi_init_state = cr_scenario.obstacle_by_id(206).initial_state
add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state,
semi_z_offset)
obstacles_to_move[206] = semi
# Add truck trailer
tanker_init_state = copy(semi_init_state)
tanker_init_state.position += [6, 3]
add_vehicle_to_bng_scenario(
bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'),
tanker_init_state, tanker_z_offset)
# Add other traffic participant
opponent = Vehicle('opponent',
model='etk800',
licence='VS',
color='Cornflowerblue')
add_vehicle_to_bng_scenario(bng_scenario, opponent,
cr_scenario.obstacle_by_id(207).initial_state,
etk800_z_offset)
obstacles_to_move[207] = opponent
# Add static obstacle
obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape
obstacle_pos = obstacle_shape.center
obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset
obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset)
for w in range(3):
for h in range(3):
for d in range(2):
obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d),
model='haybale',
color='Red')
haybale_pos_diff = obstacle_pos \
+ pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \
+ pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad)
bng_scenario.add_vehicle(obstacle,
pos=(haybale_pos_diff[0],
haybale_pos_diff[1], h * 1),
rot=(0, 0, obstacle_rot_deg))
bng_scenario.make(bng)
# Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported)
prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment,
'scenarios', bng_scenario_name + '.prefab')
lines = open(prefab_path, 'r').readlines()
for i in range(len(lines)):
if 'overObjects' in lines[i]:
lines[i] = lines[i].replace('0', '1')
open(prefab_path, 'w').writelines(lines)
# Start simulation
bng.open(launch=True)
try:
bng.load_scenario(bng_scenario)
bng.start_scenario()
bng.pause()
bng.display_gui_message(
"The scenario is fully prepared and paused. You may like to position the camera first."
)
delay_to_resume = 15
input("Press enter to continue the simulation... You have " +
str(delay_to_resume) +
" seconds to switch back to the BeamNG window.")
sleep(delay_to_resume)
bng.resume()
for id, obstacle in obstacles_to_move.items():
obstacle.ai_drive_in_lane(False)
obstacle.ai_set_line(
generate_node_list(cr_scenario.obstacle_by_id(id)))
ego_vehicle.ai_drive_in_lane(False)
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit')
speed = 65 / 3.6
ego_vehicle.ai_set_line([{
'pos': ego_vehicle.state['pos']
}, {
'pos': (129.739, 56.907, etk800_z_offset),
'speed': speed
}, {
'pos': (139.4, 62.3211, etk800_z_offset),
'speed': speed
}, {
'pos': (149.442, 64.3655, etk800_z_offset),
'speed': speed
}, {
'pos': (150.168, 63.3065, etk800_z_offset),
'speed': speed
}, {
'pos': (188.495, 78.8517, etk800_z_offset),
'speed': speed
}])
# FIXME Setting the speed does not work as expected
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set')
input("Press enter to end simulation...")
finally:
bng.close()
print(exc)
# get planning wrapper function
sys.path.append(os.getcwd() + os.path.dirname(settings['trajectory_planner_path']))
module = __import__(settings['trajectory_planner_module_name'])
function = getattr(module, settings['trajectory_planner_function_name'])
if __name__ == '__main__':
# iterate through scenarios
for filename in os.listdir(settings['input_path']):
if not filename.endswith('.xml'):
continue
fullname = os.path.join(settings['input_path'], filename)
print("Started processing scenario {}".format(filename))
scenario, planning_problem_set = CommonRoadFileReader(fullname).open()
# get settings for each scenario
if scenario.benchmark_id in settings.keys():
# specific
vehicle_model = parse_vehicle_model(settings[scenario.benchmark_id]['vehicle_model'])
vehicle_type,vehicle_type_id = parse_vehicle_type(settings[scenario.benchmark_id]['vehicle_type'])
cost_function = parse_cost_function(settings[scenario.benchmark_id]['cost_function'])
else:
# default
vehicle_model = parse_vehicle_model(settings['default']['vehicle_model'])
vehicle_type, vehicle_type_id = parse_vehicle_type(settings['default']['vehicle_type'])
cost_function = parse_cost_function(settings['default']['cost_function'])
queue = multiprocessing.Queue()
# create process, pass in required arguements
default="", type=str)
parser.add_argument('--solution_path', dest='solution_path',
help='path to solution file',
default="", type=str)
args = parser.parse_args()
return args
if __name__ == "__main__":
args = parse_args()
scenario_path = args.scenario_path
solution_path = args.solution_path
scenario, planning_problem_set = CommonRoadFileReader(scenario_path).open()
solution = CommonRoadSolutionReader().open(solution_path)
ego_vehicle_trajectory = solution.planning_problem_solutions[0].trajectory
initial_state = ego_vehicle_trajectory.state_at_time_step(0)
vehicle2 = parameters_vehicle2()
ego_vehicle_shape = Rectangle(length=vehicle2.l, width=vehicle2.w)
ego_vehicle_prediction = TrajectoryPrediction(
trajectory=ego_vehicle_trajectory, shape=ego_vehicle_shape)
ego_vehicle_type = ObstacleType.CAR
ego_vehicle = DynamicObstacle(obstacle_id=100,
obstacle_type=ego_vehicle_type,
obstacle_shape=ego_vehicle_shape,
initial_state=initial_state,
