class PathDecider:
def __init__(self, enable_routing_aid, enable_nudge, enable_change_lane):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
self.enable_routing_aid = enable_routing_aid
self.enable_nudge = enable_nudge
self.enable_change_lane = enable_change_lane
def get_path_by_lm(self, mobileye, chassis):
return self.ref.get_ref_path_by_lm(mobileye, chassis)
def get_path_by_lmr(self, perception, routing, localization, chassis):
path_x, path_y, path_len = self.ref.get_ref_path_by_lmr(
perception, routing, localization, chassis)
if self.enable_nudge:
path_x, path_y, path_len = self.nudge_process(
path_x, path_y, path_len)
return path_x, path_y, path_len
def nudge_process(self, path_x, path_y, path_len):
return path_x, path_y, path_len
def get(self, perception, routing, localization, chassis):
if self.enable_routing_aid:
return self.get_path_by_lmr(perception, routing, localization,
chassis)
else:
return self.get_path_by_lm(perception, chassis)
def __init__(self, enable_routing_aid, enable_nudge, enable_change_lane):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
self.enable_routing_aid = enable_routing_aid
self.enable_nudge = enable_nudge
self.enable_change_lane = enable_change_lane
class PathDecider:
def __init__(self):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
def get_path_by_lm(self, mobileye, chassis):
return self.ref.get_ref_path_by_lm(mobileye, chassis)
def get_path_by_lmr(self, perception, routing, localization, chassis):
return self.ref.get_ref_path_by_lmr(perception,
routing,
localization,
chassis)
def __init__(self, enable_routing_aid, enable_nudge, enable_change_lane):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
self.enable_routing_aid = enable_routing_aid
self.enable_nudge = enable_nudge
self.enable_change_lane = enable_change_lane
self.path_range = 10
def environment_setup(map_file, use_obstacles=True):
# Load map file
map = Map(file_path=map_file, origin=[-1, -2], resolution=0.005)
# Specify waypoints
wp_x = [
-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, 1.25,
-0.75, -0.75, -0.25
]
wp_y = [
-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5,
-1.5
]
# Specify path resolution
path_resolution = 0.05 # m / wp
# Create smoothed reference path
reference_path = ReferencePath(
map,
wp_x,
wp_y,
path_resolution,
smoothing_distance=5,
max_width=0.23,
circular=True,
)
# Add obstacles
if use_obstacles:
obs1 = Obstacle(cx=0.0, cy=0.05, radius=0.05)
obs2 = Obstacle(cx=-0.85, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.75, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.35, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.35, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06)
map.add_obstacles(
[obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9])
return reference_path
class PathDecider:
def __init__(self, enable_routing_aid, enable_nudge, enable_change_lane):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
self.enable_routing_aid = enable_routing_aid
self.enable_nudge = enable_nudge
self.enable_change_lane = enable_change_lane
self.path_range = 10
def get_path_by_lm(self, mobileye, adv):
return self.ref.get_ref_path_by_lm(mobileye, adv)
def get_path_by_lmr(self, perception, routing, adv):
path_x, path_y, path_len = self.ref.get_ref_path_by_lmr(perception,
routing,
adv)
if self.enable_nudge:
path_x, path_y, path_len = self.nudge_process(path_x, path_y,
path_len)
return path_x, path_y, path_len
def nudge_process(self, final_path, obstacle_decider):
obstacle_decider.process_path_obstacle(final_path)
left_dist = 999
right_dist = 999
for obs_id, lat_dist in obstacle_decider.obstacle_lat_dist.items():
if lat_dist < 0:
left_dist = lat_dist
else:
right_dist = lat_dist
print left_dist, right_dist
return final_path
def get(self, perception, routing, adv):
if self.enable_routing_aid:
return self.get_path_by_lmr(perception, routing, adv)
else:
return self.get_path_by_lm(perception, adv)
def get_path(self, perception, routing, adv, obstacle_decider):
self.path_range = self._get_path_range(adv.speed_mps)
if self.enable_routing_aid and adv.is_ready():
return self.get_routing_path(perception, routing, adv,
obstacle_decider)
else:
return self.get_lane_marker_path(perception)
def get_routing_path(self, perception, routing, adv, obstacle_decider):
routing_path = routing.get_local_path(adv, self.path_range + 1)
perception_path = perception.get_lane_marker_middle_path(
self.path_range + 1)
quality = perception.right_lm_quality + perception.left_lm_quality
quality = quality / 2.0
if routing_path.range() >= self.path_range \
and routing.human \
and routing_path.init_y() <= 3:
# "routing only"
init_y_routing = routing_path.init_y()
init_y = self._smooth_init_y(init_y_routing)
routing_path.shift(init_y - routing_path.init_y())
if self.enable_nudge:
obstacle_decider.process_path_obstacle(routing_path)
left_nudgable, right_nudgable = \
obstacle_decider.get_adv_left_right_nudgable_dist(
routing_path)
nudge_dist = obstacle_decider.get_nudge_distance(left_nudgable,
right_nudgable)
smoothed_nudge_dist = self._smooth_init_y(nudge_dist)
if smoothed_nudge_dist != 0:
print smoothed_nudge_dist
routing_path.shift(smoothed_nudge_dist)
return routing_path
init_y = self._smooth_init_y(perception_path.init_y())
if routing_path.range() < self.path_range:
# "perception only"
perception_path.shift(init_y - perception_path.init_y())
return perception_path
# "hybrid"
init_y = perception_path.init_y()
routing_path.shift(init_y - routing_path.init_y())
perception_path.shift(init_y - routing_path.init_y())
routing_path.merge(perception_path, quality)
return routing_path
def get_lane_marker_path(self, perception):
path = perception.get_lane_marker_middle_path(perception,
self.path_range)
init_y = path.init_y()
smoothed_init_y = self._smooth_init_y(init_y)
path.shift(smoothed_init_y - init_y)
return path
def _get_path_range(self, speed_mps):
path_length = self.MINIMUM_PATH_LENGTH
current_speed = speed_mps
if current_speed is not None:
if path_length < current_speed * 2:
path_length = math.ceil(current_speed * 2)
return int(path_length)
def _smooth_init_y(self, init_y):
if init_y > 0.2:
init_y = 0.2
if init_y < -0.2:
init_y = -0.2
return init_y
-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
1.25, -0.75, -0.75, -0.25
]
wp_y = [
-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5,
-1.5
]
# Specify path resolution
path_resolution = 0.05 # m / wp
# Create smoothed reference path
reference_path = ReferencePath(map,
wp_x,
wp_y,
path_resolution,
smoothing_distance=5,
max_width=0.23,
circular=True)
# Add obstacles
use_obstacles = True
if use_obstacles:
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
class PathDecider:
def __init__(self, enable_routing_aid, enable_nudge, enable_change_lane):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
self.enable_routing_aid = enable_routing_aid
self.enable_nudge = enable_nudge
self.enable_change_lane = enable_change_lane
self.path_range = 10
def get_path_by_lm(self, mobileye, adv):
return self.ref.get_ref_path_by_lm(mobileye, adv)
def get_path_by_lmr(self, perception, routing, adv):
path_x, path_y, path_len = self.ref.get_ref_path_by_lmr(
perception, routing, adv)
if self.enable_nudge:
path_x, path_y, path_len = self.nudge_process(
path_x, path_y, path_len)
return path_x, path_y, path_len
def nudge_process(self, final_path, obstacle_decider):
obstacle_decider.process_path_obstacle(final_path)
left_dist = 999
right_dist = 999
for obs_id, lat_dist in obstacle_decider.obstacle_lat_dist.items():
if lat_dist < 0:
left_dist = lat_dist
else:
right_dist = lat_dist
print(left_dist, right_dist)
return final_path
def get(self, perception, routing, adv):
if self.enable_routing_aid:
return self.get_path_by_lmr(perception, routing, adv)
else:
return self.get_path_by_lm(perception, adv)
def get_path(self, perception, routing, adv, obstacle_decider):
self.path_range = self._get_path_range(adv.speed_mps)
if self.enable_routing_aid and adv.is_ready():
return self.get_routing_path(perception, routing, adv,
obstacle_decider)
else:
return self.get_lane_marker_path(perception)
def get_routing_path(self, perception, routing, adv, obstacle_decider):
routing_path = routing.get_local_path(adv, self.path_range + 1)
perception_path = perception.get_lane_marker_middle_path(
self.path_range + 1)
quality = perception.right_lm_quality + perception.left_lm_quality
quality = quality / 2.0
if routing_path.range() >= self.path_range \
and routing.human \
and routing_path.init_y() <= 3:
# "routing only"
init_y_routing = routing_path.init_y()
init_y = self._smooth_init_y(init_y_routing)
routing_path.shift(init_y - routing_path.init_y())
if self.enable_nudge:
obstacle_decider.process_path_obstacle(routing_path)
left_nudgable, right_nudgable = \
obstacle_decider.get_adv_left_right_nudgable_dist(
routing_path)
nudge_dist = obstacle_decider.get_nudge_distance(
left_nudgable, right_nudgable)
smoothed_nudge_dist = self._smooth_init_y(nudge_dist)
if smoothed_nudge_dist != 0:
print(smoothed_nudge_dist)
routing_path.shift(smoothed_nudge_dist)
return routing_path
init_y = self._smooth_init_y(perception_path.init_y())
if routing_path.range() < self.path_range:
# "perception only"
perception_path.shift(init_y - perception_path.init_y())
return perception_path
# "hybrid"
init_y = perception_path.init_y()
routing_path.shift(init_y - routing_path.init_y())
perception_path.shift(init_y - routing_path.init_y())
routing_path.merge(perception_path, quality)
return routing_path
def get_lane_marker_path(self, perception):
path = perception.get_lane_marker_middle_path(perception,
self.path_range)
init_y = path.init_y()
smoothed_init_y = self._smooth_init_y(init_y)
path.shift(smoothed_init_y - init_y)
return path
def _get_path_range(self, speed_mps):
path_length = self.MINIMUM_PATH_LENGTH
current_speed = speed_mps
if current_speed is not None:
if path_length < current_speed * 2:
path_length = math.ceil(current_speed * 2)
return int(path_length)
def _smooth_init_y(self, init_y):
if init_y > 0.2:
init_y = 0.2
if init_y < -0.2:
init_y = -0.2
return init_y
def __init__(self):
self.MINIMUM_PATH_LENGTH = 5
self.MAX_LAT_CHANGE = 0.1
self.last_init_lat = None
self.ref = ReferencePath()
wp_x = [
-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, 1.25, -0.75,
-0.75, -0.25
]
wp_y = [
-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5, -1.5
]
# Specify path resolution
path_resolution = 0.05 # m / wp
# Create smoothed reference path
reference_path = ReferencePath(map,
wp_x,
wp_y,
path_resolution,
smoothing_distance=5,
max_width=0.23,
circular=True)
# Add obstacles
use_obstacles = False
if use_obstacles:
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
def setupMPC():
global returnUDPMessage
global mpc
global car
global x_log
global y_log
global v_log
global t
global reference_path
global map
print("go")
# Select Simulation Mode | 'Sim_Track' or 'Real_Track'
sim_mode = 'Sim_Track'
# Simulation Environment. Mini-Car on track specifically designed to show-
# case time-optimal driving.
if sim_mode == 'Sim_Track':
# Load map file
map = Map(file_path='maps/sim_map5.png', origin=[-1, -2],
resolution=0.005) #picture is 35x35m
#500x500px
"""
Constructor for map object. Map contains occupancy grid map data of
environment as well as meta information.
:param file_path: path to image of map
:param threshold_occupied: threshold value for binarization of map
image
:param origin: x and y coordinates of map origin in world coordinates
[m]
:param resolution: resolution in m/px
"""
# Specify waypoints
wp_x = [-0.75, -0.25,-0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, 1.25, -0.75, -0.75, -0.25]
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, -1.5, -1.5]
# Specify path resolution
path_resolution = 0.05 # m / wp
# Create smoothed reference path
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=0.23,
circular=True)
# Add obstacles
use_obstacles = False
if use_obstacles:
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
obs4 = Obstacle(cx=-0.3, cy=-1.0, radius=0.08)
obs5 = Obstacle(cx=0.27, cy=-1.0, radius=0.05)
obs6 = Obstacle(cx=0.78, cy=-1.47, radius=0.05)
obs7 = Obstacle(cx=0.73, cy=-0.9, radius=0.07)
obs8 = Obstacle(cx=1.2, cy=0.0, radius=0.08)
obs9 = Obstacle(cx=0.67, cy=-0.05, radius=0.06)
map.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7,
obs8, obs9])
#map.remove_obstacle(obs4)
"""
Simplified Spatial Bicycle Model. Spatial Reformulation of Kinematic
Bicycle Model. Uses Simplified Spatial State.
:param reference_path: reference path model is supposed to follow
:param length: length of the car in m
:param width: with of the car in m
:param Ts: sampling time of model in s
"""
# Instantiate motion model
car = BicycleModel(length=0.12, width=0.06, #12cm. 6cm
reference_path=reference_path, Ts=0.008333333333)
else:
print('Invalid Simulation Mode!')
map, wp_x, wp_y, path_resolution, reference_path, car \
= None, None, None, None, None, None
exit(1)
##############
# Controller #
##############
"""
Constructor for the Model Predictive Controller.
:param model: bicycle model object to be controlled
:param N: time horizon | int
:param Q: state cost matrix
:param R: input cost matrix
:param QN: final state cost matrix
:param StateConstraints: dictionary of state constraints
:param InputConstraints: dictionary of input constraints
:param ay_max: maximum allowed lateral acceleration in curves
"""
N = 30
Q = sparse.diags([1.0, 0.0, 0.0])
R = sparse.diags([0.5, 0.0])
QN = sparse.diags([1.0, 0.0, 0.0])
v_max = 1 # m/s
delta_max = .66 # rad
ay_max = 4000 # m/s^2 #max laterial accel (how far it's actually capable of going)
InputConstraints = {'umin': np.array([0.0, -np.tan(delta_max)/car.length]),
'umax': np.array([v_max, np.tan(delta_max)/car.length])}
StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),
'xmax': np.array([np.inf, np.inf, np.inf])}
mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, ay_max)
# Compute speed profile
a_min = -0.1 # m/s^2
a_max = 0.5 # m/s^2 ##################what inputs the car can actually accept
"""
Compute a speed profile for the path. Assign a reference velocity
to each waypoint based on its curvature.
:param Constraints: constraints on acceleration and velocity
curvature of the path
"""
SpeedProfileConstraints = {'a_min': a_min, 'a_max': a_max,
'v_min': 0.0, 'v_max': v_max, 'ay_max': ay_max}
car.reference_path.compute_speed_profile(SpeedProfileConstraints)
##############
# Simulation #
##############
# Set simulation time to zero
t = 0.0
# Logging containers
x_log = [car.temporal_state.x]
y_log = [car.temporal_state.y]
v_log = [0.0]
returnUDPMessage="DONE SETUP"
print(returnUDPMessage)