class RRTStarPlanningOperator(erdos.Operator):
"""RRTStar Planning operator for Carla 0.9.x.
Args:
flags: Config flags.
goal_location: Goal pylot.utils.Location for planner to route to.
"""
def __init__(self,
can_bus_stream,
prediction_stream,
global_trajectory_stream,
open_drive_stream,
waypoints_stream,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
global_trajectory_stream.add_callback(self.on_global_trajectory)
open_drive_stream.add_callback(self.on_opendrive_map)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
self._vehicle_transform = None
self._map = None
self._waypoints = None
self._prev_waypoints = None
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
@staticmethod
def connect(can_bus_stream, prediction_stream, global_trajectory_stream,
open_drive_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def run(self):
# Run method is invoked after all operators finished initializing,
# including the CARLA operator, which reloads the world. Thus, if
# we get the map here we're sure it is up-to-date.
if not hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout))
self._logger.info('Planner running in stand-alone mode')
def on_can_bus_update(self, msg):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_prediction_update(self, msg):
self._logger.debug('@{}: received prediction message'.format(
msg.timestamp))
self._prediction_msgs.append(msg)
def on_global_trajectory(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
self._goal_location = self._vehicle_transform.location
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data))
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
else:
# haven't received waypoints from global trajectory stream
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# run rrt star
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
success, (path_x, path_y) = \
self._apply_rrt_star(obstacle_list, timestamp)
speeds = [0]
if success:
speeds = [self._flags.target_speed] * len(path_x)
self._logger.debug("@{}: RRT* Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: RRT* Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: RRT* Speeds: {}".format(
timestamp,
[self._flags.target_speed] * len(path_x))
)
# construct and send waypoint message
waypoint_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoint_message)
def _get_closest_index(self, start):
min_dist = np.infty
mindex = 0
for ind, wp in enumerate(self._waypoints):
dist = np.linalg.norm([start[0] - wp.location.x,
start[1] - wp.location.y])
if dist <= min_dist:
mindex = ind
min_dist = dist
return mindex
def _apply_rrt_star(self, obstacles, timestamp):
start = [
self._vehicle_transform.location.x,
self._vehicle_transform.location.y
]
# find the closest point to current location
mindex = self._get_closest_index(start)
end_ind = min(mindex + DEFAULT_TARGET_WAYPOINT, len(self._waypoints) - 1)
end = [
self._waypoints[end_ind].location.x,
self._waypoints[end_ind].location.y
]
# log initial conditions for debugging
initial_conditions = {
"start": start,
"end": end,
"obstacles": obstacles.tolist(),
"step_size": STEP_SIZE,
"max_iterations": MAX_ITERATIONS,
}
self._logger.debug("@{}: Initial conditions: {}".format(
timestamp, initial_conditions))
return apply_rrt_star(start, end, STEP_SIZE, MAX_ITERATIONS, obstacles)
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the rrt* path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.error("@{}: RRT* failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: RRT* succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
# RRT* does not take into account the driveable region
# it constructs search space as a top down, minimum bounding rectangle
# with padding in each dimension
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
@staticmethod
def _build_obstacle_list(vehicle_transform, prediction_msg):
"""
Construct an obstacle list of proximal objects given vehicle_transform.
"""
obstacle_list = []
# look over all predictions
for prediction in prediction_msg.predictions:
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
global_obstacle = vehicle_transform * transform
obstacle_origin = [
global_obstacle.location.x, global_obstacle.location.y
]
dist_to_ego = np.linalg.norm([
vehicle_transform.location.x - obstacle_origin[0],
vehicle_transform.location.y - obstacle_origin[1]
])
# TODO (@fangedward): Fix this hack
# Prediction also sends a prediction for ego vehicle
# This will always be the closest to the ego vehicle
# Filter out until this is removed from prediction
if dist_to_ego < 2: # this allows max vel to be 20m/s
break
elif dist_to_ego < DEFAULT_DISTANCE_THRESHOLD:
# use 3d bounding boxes if available, otherwise use default
if isinstance(prediction.bounding_box, BoundingBox3D):
start_location = \
prediction.bounding_box.transform.location - \
prediction.bounding_box.extent
end_location = \
prediction.bounding_box.transform.location + \
prediction.bounding_box.extent
start_transform = global_obstacle.transform_locations(
[start_location])
end_transform = global_obstacle.transform_locations(
[end_location])
else:
start_transform = [
Location(
obstacle_origin[0] - DEFAULT_OBSTACLE_SIZE,
obstacle_origin[1] - DEFAULT_OBSTACLE_SIZE,
0
)
]
end_transform = [
Location(
obstacle_origin[0] + DEFAULT_OBSTACLE_SIZE,
obstacle_origin[1] + DEFAULT_OBSTACLE_SIZE,
0
)
]
obstacle_list.append([min(start_transform[0].x,
end_transform[0].x),
min(start_transform[0].y,
end_transform[0].y),
max(start_transform[0].x,
end_transform[0].x),
max(start_transform[0].y,
end_transform[0].y)])
if len(obstacle_list) == 0:
return np.empty((0, 4))
return np.array(obstacle_list)
class FOTPlanningOperator(erdos.Operator):
""" Frenet Optimal Trajectory (FOT) Planning operator for Carla 0.9.x.
This planning operator uses a global route and listens for predictions
to produce a frenet optimal trajectory plan. Details can be found in
`~pylot.planning.frenet_optimal_trajectory.frenet_optimal_trajectory.py`.
Args:
flags(:absl.flags:): Object to be used to access absl flags
goal_location(:pylot.utils.Location:): Goal location for route planning
"""
def __init__(self,
can_bus_stream,
prediction_stream,
global_trajectory_stream,
open_drive_stream,
waypoints_stream,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
global_trajectory_stream.add_callback(self.on_global_trajectory)
open_drive_stream.add_callback(self.on_opendrive_map)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
self._vehicle_transform = None
self._map = None
self._waypoints = None
self._prev_waypoints = None
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
self.s0 = 0
@staticmethod
def connect(can_bus_stream, prediction_stream, global_trajectory_stream,
open_drive_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def run(self):
# Run method is invoked after all operators finished initializing,
# including the CARLA operator, which reloads the world. Thus, if
# we get the map here we're sure it is up-to-date.
if not hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout))
self._logger.info('Planner running in stand-alone mode')
def on_can_bus_update(self, msg):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_prediction_update(self, msg):
self._logger.debug('@{}: received prediction message'.format(
msg.timestamp))
self._prediction_msgs.append(msg)
def on_global_trajectory(self, msg):
"""Invoked whenever a message is received on the trajectory stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
a list of waypoints to the goal location.
"""
self._logger.debug('@{}: global trajectory has {} waypoints'.format(
msg.timestamp, len(msg.data)))
if len(msg.data) > 0:
# The last waypoint is the goal location.
self._goal_location = msg.data[-1][0].location
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
self._goal_location = self._vehicle_transform.location
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_opendrive_map(self, msg):
"""Invoked whenever a message is received on the open drive stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
the open drive string.
"""
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._logger.info('Initializing HDMap from open drive stream')
from pylot.map.hd_map import HDMap
self._map = HDMap(carla.Map('map', msg.data))
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# get ego info
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
self._vehicle_transform = vehicle_transform
# get obstacles
prediction_msg = self._prediction_msgs.popleft()
obstacle_list = self._build_obstacle_list(vehicle_transform,
prediction_msg)
# update waypoints
if not self._waypoints:
# running in CARLA
if self._map is not None:
self._waypoints = self._map.compute_waypoints(
vehicle_transform.location, self._goal_location)
# haven't received waypoints from global trajectory stream
else:
self._logger.debug("@{}: Sending target speed 0, haven't"
"received global trajectory"
.format(timestamp))
head_waypoints = deque([vehicle_transform])
target_speeds = deque([0])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
return
# compute optimal frenet trajectory
path_x, path_y, speeds, params, success, s0 = \
self._compute_optimal_frenet_trajectory(can_bus_msg, obstacle_list)
if success:
self._logger.debug("@{}: Frenet Path X: {}".format(
timestamp,
path_x.tolist())
)
self._logger.debug("@{}: Frenet Path Y: {}".format(
timestamp,
path_y.tolist())
)
self._logger.debug("@{}: Frenet Speeds: {}".format(
timestamp,
speeds.tolist())
)
# construct and send waypoint message
waypoints_message = self._construct_waypoints(
timestamp, path_x, path_y, speeds, success
)
waypoints_stream.send(waypoints_message)
def _compute_optimal_frenet_trajectory(self, can_bus_msg, obstacle_list):
"""
Compute the optimal frenet trajectory, given current environment info.
"""
# convert waypoints to frenet coordinates
wx = []
wy = []
for wp in itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS):
wx.append(wp.location.x)
wy.append(wp.location.y)
wx = np.array(wx)
wy = np.array(wy)
# compute frenet optimal trajectory
s0, c_speed, c_d, c_d_d, c_d_dd = \
self._compute_initial_conditions(can_bus_msg, wx, wy)
self.s0 = s0
target_speed = (c_speed + self._flags.target_speed) / 2
path_x, path_y, speeds, params, success = get_fot_frenet_space(
s0, c_speed, c_d, c_d_d, c_d_dd,
wx, wy, obstacle_list, target_speed
)
# log initial conditions for debugging
initial_conditions = {
"s0": s0,
"c_speed": c_speed,
"c_d": c_d,
"c_d_d": c_d_d,
"c_d_dd": c_d_dd,
"wx": wx.tolist(),
"wy": wy.tolist(),
"obstacle_list": obstacle_list.tolist(),
"x": can_bus_msg.data.transform.location.x,
"y": can_bus_msg.data.transform.location.y,
"vx": can_bus_msg.data.velocity_vector.x,
"vy": can_bus_msg.data.velocity_vector.y,
}
timestamp = can_bus_msg.timestamp
self._logger.debug("@{}: Initial conditions: {}".format(
timestamp, initial_conditions))
return path_x, path_y, speeds, params, success, s0
def _construct_waypoints(self, timestamp, path_x, path_y, speeds, success):
"""
Convert the optimal frenet path into a waypoints message.
"""
path_transforms = []
target_speeds = []
if not success:
self._logger.debug("@{}: Frenet Optimal Trajectory failed. "
"Sending emergency stop.".format(timestamp))
for wp in itertools.islice(self._prev_waypoints, 0,
DEFAULT_NUM_WAYPOINTS):
path_transforms.append(wp)
target_speeds.append(0)
else:
self._logger.debug("@{}: Frenet Optimal Trajectory succeeded."
.format(timestamp))
for point in zip(path_x, path_y, speeds):
if self._map is not None:
p_loc = self._map.get_closest_lane_waypoint(
Location(x=point[0], y=point[1], z=0)).location
else:
p_loc = Location(x=point[0], y=point[1], z=0)
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
target_speeds.append(point[2])
waypoints = deque(path_transforms)
self._prev_waypoints = waypoints
return WaypointsMessage(timestamp, waypoints, target_speeds)
def _compute_initial_conditions(self, can_bus_msg, wx, wy):
"""
Convert the initial conditions of vehicle into frenet frame parameters.
"""
x = can_bus_msg.data.transform.location.x
y = can_bus_msg.data.transform.location.y
vx = can_bus_msg.data.velocity_vector.x
vy = can_bus_msg.data.velocity_vector.y
return compute_initial_conditions(self.s0, x, y, vx, vy,
can_bus_msg.data.forward_speed, wx,
wy)
@staticmethod
def _build_obstacle_list(vehicle_transform, prediction_msg):
"""
Construct an obstacle list of proximal objects given vehicle_transform.
"""
obstacle_list = []
# look over all predictions
for prediction in prediction_msg.predictions:
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
global_obstacle = vehicle_transform * transform
obstacle_origin = [
global_obstacle.location.x, global_obstacle.location.y
]
dist_to_ego = np.linalg.norm([
vehicle_transform.location.x - obstacle_origin[0],
vehicle_transform.location.y - obstacle_origin[1]
])
# TODO (@fangedward): Fix this hack
# Prediction also sends a prediction for ego vehicle
# This will always be the closest to the ego vehicle
# Filter out until this is removed from prediction
if dist_to_ego < 2: # this allows max vel to be 20m/s
break
elif dist_to_ego < DEFAULT_DISTANCE_THRESHOLD:
obstacle_list.append(obstacle_origin)
if len(obstacle_list) == 0:
return np.empty((0, 2))
return np.array(obstacle_list)
