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 draw_object(obj, plot_limits=None, ax=None, draw_params=None,
draw_func=None, handles=None, call_stack=None):
"""
Main function for drawing objects from the commonroad-collision-checker.
:param obj: the object or list of objects (with all the same type) to be plotted
:param plot_limits: list of [x_min, x_max, y_min, y_max] that defines the plotted area
:param ax: axes object from matplotlib
:param draw_params: parameters for plotting given by a nested dict that recreates the structure of an object,
see documentation for full overview over the structure. If parameters are not set,
the default setting are used.
:param draw_func: specify the drawing function (usually not necessary to change default)
:param handles: dict that assign to every object_id of all plotted obstacles the corresponding patch handles
:param call_stack: tuple of string containing the call stack, which allows for differentiation of plotting styles
depending on the call stack of draw_object, (usually 'None'!)
:return: Returns matplotlib patch object for draw_funcs that actually draw a patch (used internally for creating handles dict)
"""
if handles is None:
handles = dict()
if ax is None:
ax = plt.gca()
if plot_limits is not None:
ax.set_xlim(plot_limits[0], plot_limits[1])
ax.set_ylim(plot_limits[2], plot_limits[3])
if draw_func is None:
draw_func_pycrcc = _create_drawers_dict_pycrcc()
else:
draw_func_pycrcc = draw_func
if draw_params is None:
draw_params = _create_default_draw_params_pycrcc()
if call_stack is None:
call_stack = tuple()
if type(obj) is list:
if len(obj)==0:
return []
for o in obj:
draw_object(o, None, ax, draw_params, draw_func, handles, call_stack)
return
draw_func_core = draw_dispatch_core._create_drawers_dict()
if type(obj) in draw_func_pycrcc.keys():
draw_func_pycrcc[type(obj)](obj, plot_limits, ax, draw_params, draw_func_pycrcc, handles, call_stack)
elif type(obj) in draw_func_core.keys():
draw_dispatch_core.draw_object(obj, plot_limits, ax, draw_params, draw_func, handles, call_stack)
else:
print("Cannot dispatch to plot " + str(type(obj)))
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 render(self, mode="rgb array", *args, **kwargs):
# occupancies are not calculated in step function for efficiency, but calculated here for visualization purpose
obs_state = self.obs_state
ego_state = self.ego_state
obs_occupancy = Occupancy(
time_step=1,
shape=self.obs_veh.prediction.shape.rotate_translate_local(
obs_state.position, obs_state.orientation))
ego_occupancy = Occupancy(
time_step=1,
shape=self.ego_veh.prediction.shape.rotate_translate_local(
ego_state.position, ego_state.orientation))
plt.figure(figsize=(25, 10))
x = ego_state.position[0] - 15.
y = ego_state.position[1]
plt.text(x, y + 7., "vel={}".format(np.round(ego_state.velocity, 2)))
accel = str(np.round(ego_state.acceleration, 2))
plt.text(x, y + 14., "accel={}".format(accel))
x = obs_state.position[0] + 15.
y = obs_state.position[1]
plt.text(x, y + 7., "vel={}".format(np.round(obs_state.velocity, 2)))
plt.text(x, y + 14.,
"accel={}".format(np.round(obs_state.acceleration, 2)))
s_safe = abs((obs_state.velocity**2 - ego_state.velocity**2) /
(2 * params.MIN_ACCEL))
x = (ego_state.position[0] + obs_state.position[0]) / 2.
plt.text(x, y + 28., "safe distance {}".format(np.round(s_safe)))
plt.text(
x, y + 21., "distance {}".format(
np.round(obs_state.position[0] - ego_state.position[0], 2)))
draw_object(self.scenario.lanelet_network,
draw_params={'time_begin': self.t},
plot_limits=self.plot_limits)
draw_object(ego_occupancy,
draw_params=utils.get_draw_params(ego=True),
plot_limits=self.plot_limits)
draw_object(obs_occupancy,
draw_params=utils.get_draw_params(ego=False),
plot_limits=self.plot_limits)
plt.gca().set_aspect('equal')
if mode == "human":
plt.show()
else:
viz_path = os.path.join(self.viz_dir,
"episode_{}".format(self.epid))
if not os.path.exists(viz_path):
os.mkdir(viz_path)
figpath = os.path.join(viz_path, "step_{}.png".format(self.t))
plt.savefig(figpath)
print("Rendering saved in {}".format(figpath))
plt.close()
def draw(to_draw: drawable_types) -> Union[Optional[Artist], list]:
"""
Converts the given object to a drawable object, draws and returns it.
:param to_draw: The object to draw.
:return The object drawn on the current plot or None if it could not be drawn.
"""
if not to_draw:
artist = None
elif isinstance(to_draw,
(Scenario, LaneletNetwork, List, GoalRegion,
StaticObstacle, commonroad.geometry.shape.Rectangle
)): # FIXME Use List[plottable_types]
artist = draw_object(to_draw, draw_params=DrawConfig.draw_params)
elif isinstance(to_draw, DynamicObstacle):
error(
"Pass an occupancy of a DynamicObstacle instead of the obstacle itself."
)
artist = None
elif isinstance(to_draw, MultiLineString):
artist = [] # In case the MultiLineString had no boundary
for line_string in to_draw.boundary:
artist.append(DrawHelp.draw(line_string))
elif isinstance(to_draw, LineString):
coords = to_draw.coords
x_data: List[Tuple[float,
float]] = list(map(lambda l: l[0], coords))
y_data: List[Tuple[float,
float]] = list(map(lambda l: l[1], coords))
artist = gca().add_line(Line2D(x_data, y_data, color='orange'))
elif isinstance(to_draw, MultiPolygon):
artist = [] # In case the polygon had no boundary
for line_string in to_draw.boundary:
artist.append(DrawHelp.draw(line_string))
elif isinstance(to_draw, Polygon):
artist = DrawHelp.draw(to_draw.boundary)
elif isinstance(to_draw, Patch):
artist = gca().add_patch(to_draw)
else:
error("Drawing " + str(type(to_draw)) +
" is not implemented, yet.")
artist = None
if isinstance(artist, list) and artist == []:
artist = None
return artist
def start_search(scenario,
planning_problem,
automata,
motion_planner,
initial_motion_primitive,
flag_plot_intermediate_results=True,
flag_plot_planning_problem=True):
# create Manager object to manage shared variables between different Processes
process_manager = Manager()
# create shared variables between Processes
path_shared = process_manager.Value('path_shared', None)
dict_status_shared = process_manager.Value('dict_status_shared', None)
# initialize status with a dictionary.
# IMPORTANT NOTE: if the shared variable contains complex object types (list, dict, ...), to change its value,
# we need to do it via another object (here dict_status) and pass it to the shared variable at the end
dict_status = {'cost_current': 0, 'path_current': []}
dict_status_shared.value = dict_status
# maximum number of concatenated primitives
max_tree_depth = 100
# create and start the Process for search
process_search = Process(target=execute_search,
args=(motion_planner, initial_motion_primitive,
path_shared, dict_status_shared,
max_tree_depth))
process_search.start()
if flag_plot_intermediate_results:
# create a figure, plot the scenario and planning problem
fig = plt.figure(figsize=(9, 9))
plt.clf()
draw_object(scenario)
if flag_plot_planning_problem:
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0, 0)
plt.show()
refresh_rate_plot = 5.0
while True:
time.sleep(1 / refresh_rate_plot)
# break the loop if process_search is not alive anymore
if not process_search.is_alive():
break
# print current status
string_status = "Cost so far:", round(
dict_status_shared.value['cost_current'], 2)
string_status += "Time step:", round(
dict_status_shared.value['path_current'][-1].time_step, 2)
string_status += "Orientation:", round(
dict_status_shared.value['path_current'][-1].orientation, 2)
string_status += "Velocity:", round(
dict_status_shared.value['path_current'][-1].velocity, 2)
print(string_status, end='\r')
# if there is a path to be visualized
if len(dict_status_shared.value['path_current']) > 0:
if flag_plot_intermediate_results:
plt.clf()
draw_object(scenario)
if flag_plot_planning_problem:
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0, 0)
plt.show()
# create a Trajectory from the current states of the path
# Trajectory is essentially a list of states
trajectory = Trajectory(
initial_time_step=0,
state_list=dict_status_shared.value['path_current'])
# create an occupancy Prediction via the generated trajectory and shape of ego vehicle
# the Prediction includes a Trajectory and Shape of the object
prediction = TrajectoryPrediction(trajectory=trajectory,
shape=automata.egoShape)
# create a colission object from prediction
# here it is used for visualization purpose
collision_object = create_collision_object(prediction)
# draw this collision object
draw_it(collision_object,
draw_params={'collision': {
'facecolor': 'magenta'
}})
# visualize current trajectory
fig.canvas.draw()
# wait till process_search terminates
process_search.join()
time.sleep(0.5)
print("Search finished.")
if path_shared.value is not None and len(path_shared.value) > 1:
print("Solution successfully found :)")
else:
print("Finding solution failed :(")
return path_shared.value, dict_status_shared.value
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,
prediction=ego_vehicle_prediction)
for i in range(0, 40):
display.clear_output(wait=True)
plt.figure()
draw_object(scenario, draw_params={'time_begin': i})
draw_object(ego_vehicle,
draw_params={'time_begin': i, 'facecolor': 'g'})
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.xlim(-10, 40)
plt.ylim(-15, 15)
plt.savefig('solution{}.png'.format(i))
break
def parse_args():
parser = argparse.ArgumentParser(description='Plot CommonRoad scenario')
parser.add_argument('--file_path',
dest='file_path',
help='path to scenario file',
default="",
type=str)
args = parser.parse_args()
return args
if __name__ == "__main__":
args = parse_args()
file_path = args.file_path
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()
for i in range(0, 20):
display.clear_output(wait=True)
plt.figure()
draw_object(scenario, draw_params={'time_begin': i})
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.xlim(-10, 40)
plt.ylim(-15, 15)
plt.savefig('scenario{}.png'.format(i))
break
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)
cc = create_collision_checker(scenario)
co = create_collision_object(ego_vehicle_prediction)
if args.check_all and cc.collide(co):
print(plot_name)
collision_num += 1
else:
print('Does collision exist? ', cc.collide(co))
plt.figure()
draw_object(scenario.lanelet_network)
draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})
draw_object(co, draw_params={'collision': {'facecolor': 'green'}})
plt.autoscale()
plt.axis('equal')
plt.xlim(-10, 40)
plt.ylim(-15, 15)
plt.savefig('plot/{}.png'.format(plot_name))
if args.check_all:
print("collision rate: ", collision_num / len(solutions))
# record the number of collision case
with open('collision.txt', 'w') as f:
f.write(str(collision_num))
