def print_info(self):
kwarg = {
'position': np.array([self.startState.x, self.startState.y]),
'velocity': self.startState.velocity,
'steering_angle': self.startState.steering_angle,
'orientation': self.startState.orientation,
'time_step': self.startState.time_step
}
state_start = State(**kwarg)
# add final state into the trajectory
kwarg = {
'position': np.array([self.finalState.x, self.finalState.y]),
'velocity': self.finalState.velocity,
'steering_angle': self.finalState.steering_angle,
'orientation': self.finalState.orientation,
'time_step': self.finalState.time_step
}
state_final = State(**kwarg)
print(state_start)
for state in self.trajectory.state_list:
print(state)
print(state_final)
def gen_vehicle(vehicle_id=None, init_position=None, init_velocity=None):
"""
Generates obstacle with given id, initial position and initial velocity;
Default obstacle type: CAR, default obstacle shape: Rectangle
:param vehicle_id:
:return: DynamicObstacle
"""
assert all(v is not None for v in [vehicle_id, init_velocity, init_position]), \
'<utils/gen_vehicle> vehicle id, initial position and initial velocity need to be given'
obstacle_shape = Rectangle(length=params.DEFAULT_VEHICLE_LENGTH,
width=params.DEFAULT_VEHICLE_WIDTH)
initial_state = State(position=np.array([init_position, 0.]),
velocity=init_velocity,
orientation=0.,
acceleration=0.,
time_step=1)
trajectory = Trajectory(state_list=[initial_state], initial_time_step=1)
prediction = TrajectoryPrediction(trajectory, obstacle_shape)
return DynamicObstacle(obstacle_id=vehicle_id,
obstacle_type=ObstacleType.CAR,
obstacle_shape=obstacle_shape,
initial_state=initial_state,
prediction=prediction)
def create_from_vertices(cls, vertices: np.ndarray, steering_angles: np.ndarray, velocities: np.ndarray,
orientations: np.ndarray, time_steps: np.ndarray) -> Trajectory:
"""
Creates a trajectory from a set of given positions, orientations, velocities and a starting time step.
:param vertices: a set of positions for every state of the trajectory
:param t0: starting time step of the trajectory
:param orientation: a set of orientations for every state of the trajectory
:param velocity: a set of velocities for every state of the trajectory
"""
assert len(vertices) == len(steering_angles) == len(velocities) == len(orientations) == len(time_steps), "The sizes of state elements should be equal!"
list_states = []
for i in range(len(vertices)):
kwarg = {'position': np.array([vertices[i][0], vertices[i][1]]),
'velocity': velocities[i],
'steering_angle': steering_angles[i],
'orientation': orientations[i],
'time_step': time_steps[i]}
# append state
list_states.append(State(**kwarg))
# create trajectory
trajectory = Trajectory(initial_time_step=int(time_steps[0]), state_list=list_states)
return trajectory
def reset(self, initial_state: State, dt: float) -> None:
"""
Reset vehicle parameters.
:param initial_state: The initial state of the vehicle
:param dt: Simulation dt of the scenario
:return: None
"""
self.current_time_step = 0
self.dt = dt
self._friction_violation = False
problem_init_state = initial_state
init_state = State(
**{
"position":
problem_init_state.position,
"steering_angle":
problem_init_state.steering_angle if hasattr(
problem_init_state, "steering_angle") else 0.0,
"orientation":
problem_init_state.orientation,
"yaw_rate":
problem_init_state.yaw_rate,
"time_step":
problem_init_state.time_step,
"velocity":
problem_init_state.velocity,
"acceleration":
0.0,
})
self._reset_car_state(init_state)
def add_simple_object(scenario, id, position_x, position_y, oritentation,
velocity, *args, **kwargs):
init_state = State(position=np.array([position_x, position_y]),
orientation=oritentation,
velocity=velocity)
new_obstacle = DynamicObstacle(id, ObstacleType.PEDESTRIAN, Circle(10.0),
init_state)
scenario.add_objects(new_obstacle)
def _get_new_state(self, action: np.ndarray) -> State:
"""
Get the new state using the kinematic single track model.
:param action: rescaled action from the CommonroadEnv.
:return: new state
Actions:
Type: Box(2)
Num Action Min Max
0 ego vehicle acceleration -vehicle.params.longitudinal.a_max vehicle.params.longitudinal.a_max
1 ego vehicle steering angular velocity -vehicle.params.steering.v_max vehicle.params.steering.v_max
Forward simulation of states
ref: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf
x1 = x-position in a global coordinate system
x2 = y-position in a global coordinate system
x3 = steering angle of front wheels
x4 = velocity in x-direction
x5 = yaw angle
u1 = steering angle velocity of front wheels
u2 = longitudinal acceleration
"""
current_state = self.state
x_current = np.array([
current_state.position[0],
current_state.position[1],
current_state.steering_angle,
current_state.velocity,
current_state.orientation,
])
x_dot = None
u_input = np.array([action[1], action[0]])
for _ in range(N_INTEGRATION_STEPS):
# simulate state transition
x_dot = np.array(
vehicle_dynamics_ks(x_current, u_input, self.params))
# update state
x_current = x_current + x_dot * (self.dt / N_INTEGRATION_STEPS)
# feed in required slots
kwarg = {
"position": np.array([x_current[0], x_current[1]]),
"velocity": x_current[3],
"steering_angle": x_current[2],
"orientation": make_valid_orientation(x_current[4]),
"acceleration": u_input[1],
"yaw_rate": x_dot[4],
"time_step": self.current_time_step,
}
# append state
new_state = State(**kwarg)
return new_state
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 _get_new_state(self, action: np.ndarray) -> State:
"""
Get the new state using the kinematic single track model.
:param action: rescaled action from the CommonroadEnv.
:return: new state
"""
if self.current_time_step == 1:
old_state = self.initial_state
else:
old_state = self.state_list[-1]
theta = old_state.orientation
v = old_state.velocity
x = old_state.position[0]
y = old_state.position[1]
theta_dot = action[1]
a = action[0]
delta_t = self.dt / N_INTEGRATION_STEPS
for _ in range(N_INTEGRATION_STEPS):
x += v * np.cos(theta) * delta_t
y += v * np.sin(theta) * delta_t
theta += theta_dot * delta_t
v += a * delta_t
self.acceleration = a
self.turn_rate = theta_dot
theta = shift_orientation(theta)
assert np.pi >= theta >= -np.pi
new_state = State(
**{
"position": np.array([x, y]),
"orientation": theta,
"time_step": self.current_time_step,
"yaw_rate": theta_dot,
"steering_angle": np.arctan(theta_dot * self.l_wb / v),
"velocity": v,
"acceleration": a,
})
return new_state
def _propagate(self, veh_id: int, a: float):
time_step = self.t
dt = self.scenario.dt
obstacle = self.scenario.obstacle_by_id(veh_id)
last_state = obstacle.prediction.trajectory.state_list[
-1] # state_at_time_step(time_step - 1) # #.
s = last_state.position[0]
v = last_state.velocity
next_s = s + v * dt + a * dt * dt / 2
next_v = v + a * dt
next_state = State(position=np.array([next_s, 0.]),
velocity=next_v,
orientation=0.,
time_step=time_step,
acceleration=a)
# obstacle_shape = obstacle.prediction.shape
# occupied_region = obstacle_shape.rotate_translate_local(next_state.position, next_state.orientation)
self.scenario.obstacle_by_id(
veh_id).prediction.trajectory.state_list.append(next_state)
def execute_search_batch(scenario,
planning_problem_set,
veh_type_id=2,
planning_problem_idx=0,
planner_id=3,
max_tree_depth=100):
# append main directory
import sys
sys.path.append("../../GSMP/tools/")
sys.path.append("../../GSMP/tools/commonroad-collision-checker")
sys.path.append("../../GSMP/tools/commonroad-road-boundary")
sys.path.append("../../GSMP/motion_automata")
sys.path.append("../../GSMP/motion_automata/vehicle_model")
import os
import time
from multiprocessing import Manager, Process
import numpy as np
from math import atan2
import matplotlib.pyplot as plt
from IPython import display
from ipywidgets import widgets
# import CommonRoad-io modules
from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.scenario.trajectory import Trajectory, State
from commonroad.geometry.shape import Rectangle
from commonroad.prediction.prediction import TrajectoryPrediction
from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object
from commonroad_cc.visualization.draw_dispatch import draw_object as draw_it
from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleType, CostFunction
# import Motion Automata modules
from automata.MotionAutomata import MotionAutomata
from automata.MotionPrimitive import MotionPrimitive
from automata.States import FinalState
# load necessary modules and functions
from automata.HelperFunctions import generate_automata, add_initial_state_to_automata
# 1: Greedy BFS 2: A* 3: Your own algorithm
if planner_id == 1:
from automata.MotionPlanner_gbfs import MotionPlanner
elif planner_id == 2:
from automata.MotionPlanner_Astar import MotionPlanner
else:
from automata.MotionPlanner import MotionPlanner
automata = generate_automata(veh_type_id)
# retrieve planning problem with given index (for cooperative scenario:0, 1, 2, ..., otherwise: 0)
planning_problem = list(planning_problem_set.planning_problem_dict.values(
))[planning_problem_idx]
planning_problem_id = list(planning_problem_set.planning_problem_dict.keys(
))[planning_problem_idx]
# add initial state of planning problem to automata
automata, initial_motion_primitive = add_initial_state_to_automata(
automata, planning_problem, flag_print_states=False)
# construct motion planner.
motion_planner = MotionPlanner(scenario, planning_problem, automata)
print("Start search..")
time_start = time.process_time()
result = motion_planner.search_alg(initial_motion_primitive.Successors,
max_tree_depth)
time_end = time.process_time()
print("Solving this scenario took {} seconds".format(
round(time_end - time_start, 2)))
dict_result = {}
# result is in form of (final path, used_primitives)
if result is not None:
result_path = result[0]
list_state = list()
for state in result_path:
kwarg = {
'position': state.position,
'velocity': state.velocity,
'steering_angle': state.steering_angle,
'orientation': state.orientation,
'time_step': state.time_step
}
list_state.append(State(**kwarg))
trajectory = Trajectory(initial_time_step=list_state[0].time_step,
state_list=list_state)
dict_result[planning_problem_id] = trajectory
return dict_result
def set_variable_to(state: State, j: int, value: Any):
# FIXME Is there a way to use MyState.variables?
if j == 0:
state.velocity = value
else:
raise Exception("No state with index " + str(j) + " defined.")