def __init__(self,
can_bus_stream,
ground_obstacles_stream,
ground_traffic_lights_stream,
waypoints_stream,
control_stream,
name,
flags,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
ground_obstacles_stream.add_callback(self.on_obstacles_update)
ground_traffic_lights_stream.add_callback(
self.on_traffic_lights_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([
can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream
], [control_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._obstacle_msgs = deque()
self._traffic_light_msgs = deque()
self._waypoint_msgs = deque()
def on_open_drive_map(self, msg):
self._logger.debug('@{}: open drive update'.format(msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._map = HDMap(carla.Map('challenge', msg.data),
self._log_file_name)
def __init__(self,
can_bus_stream,
open_drive_stream,
global_trajectory_stream,
waypoints_stream,
name,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update, [waypoints_stream])
open_drive_stream.add_callback(self.on_opendrive_map)
global_trajectory_stream.add_callback(self.on_global_trajectory)
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
# Initialize the state of the behaviour planner.
# XXX(ionel): The behaviour planner is not ready yet.
self.__initialize_behaviour_planner()
# We do not know yet the vehicle's location.
self._vehicle_transform = None
# Deque of waypoints the vehicle must follow. The waypoints are either
# received on the global trajectory stream when running using the
# scenario runner, or computed using the Carla global planner when
# running in stand-alone mode. The waypoints are Pylot transforms.
self._waypoints = collections.deque()
# The operator picks the wp_num_steer-th waypoint to compute the angle
# it has to steer by when taking a turn.
self._wp_num_steer = 9 # use 10th waypoint for steering
# The operator picks the wp_num_speed-th waypoint to compute the angle
# it has to steer by when driving straight.
self._wp_num_speed = 4 # use tth waypoint for speed
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Planner running in stand-alone mode')
assert goal_location, 'Planner has not received a goal location'
# Transform goal location to carla.Location
self._goal_location = goal_location
# Recompute waypoints upon each run.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
# TODO(ionel): HACK! In Carla challenge track 1 and 2 the waypoints
# are coarse grained (30 meters apart). We pick the first waypoint
# to compute the angles. However, we should instead implement
# trajectory planning.
if self._flags.track == 1 or self._flags == 2:
self._wp_num_steer = 1
self._wp_num_speed = 1
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 run(self):
# Run method is invoked after all operators finished initializing.
# Thus, we're sure the world is up-to-date here.
if self._flags.execution_mode == 'simulation':
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.simulator_host, self._flags.simulator_port,
self._flags.simulator_timeout),
self.config.log_file_name)
from pylot.simulation.utils import get_world
_, self._world = get_world(self._flags.simulator_host,
self._flags.simulator_port,
self._flags.simulator_timeout)
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 world here we're sure it is up-to-date.
if self._flags.execution_mode == 'simulation':
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.config.log_file_name)
from pylot.simulation.utils import get_world
_, self._world = get_world(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
def __init__(self,
can_bus_stream,
waypoints_stream,
traffic_lights_stream,
obstacles_stream,
point_cloud_stream,
open_drive_stream,
control_stream,
name,
flags,
camera_setup,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
waypoints_stream.add_callback(self.on_waypoints_update)
traffic_lights_stream.add_callback(self.on_traffic_lights_update)
obstacles_stream.add_callback(self.on_obstacles_update)
point_cloud_stream.add_callback(self.on_point_cloud_update)
open_drive_stream.add_callback(self.on_open_drive_map)
erdos.add_watermark_callback([
can_bus_stream, waypoints_stream, traffic_lights_stream,
obstacles_stream, point_cloud_stream
], [control_stream], self.on_watermark)
self._name = name
self._flags = flags
self._camera_setup = camera_setup
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
if not hasattr(self._flags, 'track'):
# The agent is not used in the Carla challenge. It has access to
# the simulator, and to the town map.
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.debug('Agent running using map')
else:
self._map = None
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
# Queues in which received messages are stored.
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
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))
def map_from_opendrive(opendrive: str, log_file_name: str = None):
try:
from carla import Map
except ImportError:
raise Exception('Error importing CARLA.')
from pylot.map.hd_map import HDMap
return HDMap(Map('map', opendrive), log_file_name)
def __init__(self, name, flags, log_file_name=None, csv_file_name=None):
super(GroundAgentOperator, self).__init__(name)
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._pedestrian_msgs = deque()
self._vehicle_msgs = deque()
self._traffic_light_msgs = deque()
self._speed_limit_sign_msgs = deque()
self._waypoint_msgs = deque()
self._lock = threading.Lock()
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.
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
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')
# Recompute waypoints every RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
self._watermark_cnt = 0
def __init__(self,
name,
flags,
bgr_camera_setup,
log_file_name=None,
csv_file_name=None):
super(PylotAgentOperator, self).__init__(name)
self._flags = flags
self._log_file_name = log_file_name
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._bgr_camera_setup = bgr_camera_setup
self._map = None
if '0.9' in self._flags.carla_version:
from pylot.map.hd_map import HDMap
from pylot.simulation.carla_utils import get_map
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Agent running using map')
elif hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._depth_camera_msgs = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
self._lock = threading.Lock()
self._last_traffic_light_game_time = -100000
self._last_moving_time = 0
# Num of control commands to override to ensure the agent doesn't get
# stuck.
self._num_control_override = 0
def __init__(self,
can_bus_stream,
prediction_stream,
waypoints_stream,
name,
flags,
goal_location,
log_file_name=None,
csv_file_name=None):
"""
Initialize the RRT* planner. Setup logger and map attributes.
Args:
name: Name of the operator.
flags: Config flags.
goal_location: Global goal carla.Location for planner to route to.
"""
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._wp_index = 9
self._waypoints = None
self._carla_map = get_map(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
self._hd_map = HDMap(self._carla_map, log_file_name)
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
class PylotAgentOperator(Op):
def __init__(self,
name,
flags,
bgr_camera_setup,
log_file_name=None,
csv_file_name=None):
super(PylotAgentOperator, self).__init__(name)
self._flags = flags
self._log_file_name = log_file_name
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._bgr_camera_setup = bgr_camera_setup
self._map = None
if '0.9' in self._flags.carla_version:
from pylot.map.hd_map import HDMap
from pylot.simulation.carla_utils import get_map
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Agent running using map')
elif hasattr(self._flags, 'track'):
from pylot.map.hd_map import HDMap
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._depth_camera_msgs = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
self._lock = threading.Lock()
self._last_traffic_light_game_time = -100000
self._last_moving_time = 0
# Num of control commands to override to ensure the agent doesn't get
# stuck.
self._num_control_override = 0
@staticmethod
def setup_streams(input_streams):
input_streams.filter(pylot.utils.is_can_bus_stream).add_callback(
PylotAgentOperator.on_can_bus_update)
input_streams.filter(pylot.utils.is_waypoints_stream).add_callback(
PylotAgentOperator.on_waypoints_update)
input_streams.filter(
pylot.utils.is_traffic_lights_stream).add_callback(
PylotAgentOperator.on_traffic_lights_update)
input_streams.filter(pylot.utils.is_obstacles_stream).add_callback(
PylotAgentOperator.on_obstacles_update)
input_streams.filter(pylot.utils.is_lidar_stream).add_callback(
PylotAgentOperator.on_lidar_update)
input_streams.filter(pylot.utils.is_open_drive_stream).add_callback(
PylotAgentOperator.on_opendrive_map)
input_streams.filter(pylot.utils.is_depth_camera_stream).add_callback(
PylotAgentOperator.on_depth_camera_update)
input_streams.filter(
pylot.utils.is_segmented_camera_stream).add_callback(
PylotAgentOperator.on_segmented_frame)
input_streams.filter(pylot.utils.is_detected_lane_stream).add_callback(
PylotAgentOperator.on_detected_lane_update)
# Set no watermark on the output stream so that we do not
# close the watermark loop with the carla operator.
return [pylot.utils.create_control_stream()]
def compute_command(self, can_bus_msg, waypoint_msg, tl_msg, obstacles_msg,
pc_msg, depth_msg, timestamp):
start_time = time.time()
vehicle_transform = can_bus_msg.data.transform
vehicle_speed = can_bus_msg.data.forward_speed
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
target_speed = waypoint_msg.target_speed
# Transform point cloud to camera coordinates.
point_cloud = None
if pc_msg:
point_cloud = pylot.simulation.utils.lidar_point_cloud_to_camera_coordinates(
pc_msg.point_cloud)
depth_frame = None
if depth_msg:
depth_frame = depth_msg.frame
traffic_lights = self.__transform_tl_output(tl_msg, vehicle_transform,
point_cloud, depth_frame)
game_time = timestamp.coordinates[0]
if len(traffic_lights) > 0:
self._last_traffic_light_game_time = game_time
(pedestrians,
vehicles) = self.__transform_detector_output(obstacles_msg,
vehicle_transform,
point_cloud, depth_frame)
# if self._map.is_on_opposite_lane(vehicle_transform):
# # Ignore obstacles
# self._logger.info('Ego-vehicle {} on opposite lange'.format(
# vehicle_transform))
# pedestrians = []
# vehicles = []
# traffic_lights = []
self._logger.info('{} Current speed {} and location {}'.format(
timestamp, vehicle_speed, vehicle_transform))
self._logger.info('{} Pedestrians {}'.format(timestamp, pedestrians))
self._logger.info('{} Vehicles {}'.format(timestamp, vehicles))
speed_factor, _ = self.__stop_for_agents(vehicle_transform, wp_angle,
wp_vector, vehicles,
pedestrians, traffic_lights,
timestamp)
new_target_speed = self.reduce_speed_when_approaching_intersection(
vehicle_transform, vehicle_speed, target_speed, game_time)
if new_target_speed != target_speed:
self._logger.info(
'Proximity to intersection, reducing speed from {} to {}'.
format(target_speed, new_target_speed))
target_speed = new_target_speed
self._logger.info('{} Current speed factor {}'.format(
timestamp, speed_factor))
control_msg = self.get_control_message(wp_angle, wp_angle_speed,
speed_factor, vehicle_speed,
target_speed, timestamp)
if control_msg.throttle > 0.001:
self._last_moving_time = game_time
self._num_control_override = 0
if self._num_control_override > 0:
self._num_control_override -= 1
control_msg.throttle = 0.75
# Might be stuck because of a faulty detector.
# Override control message if we haven't been moving for a while.
if game_time - self._last_moving_time > 30000:
self._num_control_override = 6
control_msg = ControlMessage(0, 0.75, 0, False, False, timestamp)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, timestamp,
runtime))
self.get_output_stream('control_stream').send(control_msg)
def synchronize_msg_buffers(self, timestamp, buffers):
for buffer in buffers:
while (len(buffer) > 0 and buffer[0].timestamp < timestamp):
buffer.popleft()
if len(buffer) == 0:
return False
assert buffer[0].timestamp == timestamp
return True
def run_if_you_can(self):
streams = [
self._can_bus_msgs, self._waypoint_msgs, self._traffic_lights_msgs,
self._obstacles_msgs
]
for stream in streams:
if len(stream) == 0:
return
can_bus_msg = self._can_bus_msgs.popleft()
waypoint_msg = self._waypoint_msgs.popleft()
tl_msg = self._traffic_lights_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
self._logger.info('Timestamps {} {} {} {}'.format(
can_bus_msg.timestamp, waypoint_msg.timestamp, tl_msg.timestamp,
obstacles_msg.timestamp))
assert (can_bus_msg.timestamp == waypoint_msg.timestamp ==
tl_msg.timestamp == obstacles_msg.timestamp)
if len(self._point_clouds) == 0 and len(self._depth_camera_msgs) == 0:
# No point clouds or depth frame msgs available.
return
pc_msg = None
if len(self._point_clouds) > 0:
pc_msg = self._point_clouds.popleft()
depth_msg = None
if len(self._depth_camera_msgs) > 0:
depth_msg = self._depth_camera_msgs.popleft()
self.compute_command(can_bus_msg, waypoint_msg, tl_msg, obstacles_msg,
pc_msg, depth_msg, can_bus_msg.timestamp)
def on_waypoints_update(self, msg):
self._logger.info('Waypoints update at {}'.format(msg.timestamp))
with self._lock:
self._waypoint_msgs.append(msg)
self.run_if_you_can()
def on_can_bus_update(self, msg):
self._logger.info('Can bus update at {}'.format(msg.timestamp))
with self._lock:
self._can_bus_msgs.append(msg)
self.run_if_you_can()
def on_traffic_lights_update(self, msg):
self._logger.info('Traffic light update at {}'.format(msg.timestamp))
with self._lock:
self._traffic_lights_msgs.append(msg)
self.run_if_you_can()
def on_obstacles_update(self, msg):
self._logger.info('Obstacle update at {}'.format(msg.timestamp))
with self._lock:
self._obstacles_msgs.append(msg)
self.run_if_you_can()
def on_lidar_update(self, msg):
self._logger.info('Lidar update at {}'.format(msg.timestamp))
with self._lock:
self._point_clouds.append(msg)
self.run_if_you_can()
def on_opendrive_map(self, msg):
self._map = HDMap(carla.Map('challenge', msg.data),
self._log_file_name)
def on_depth_camera_update(self, msg):
self._logger.info('Depth camera frame at {}'.format(msg.timestamp))
with self._lock:
self._depth_camera_msgs.append(msg)
self.run_if_you_can()
def on_segmented_frame(self, msg):
self._logger.info('Received segmented frame at {}'.format(
msg.timestamp))
# TODO(ionel): Implement!
def on_detected_lane_update(self, msg):
# TODO(ionel): Implement!
pass
def execute(self):
self.spin()
def reduce_speed_when_approaching_intersection(self, vehicle_transform,
vehicle_speed, target_speed,
game_time):
if not self._map:
return target_speed
intersection_dist = self._map.distance_to_intersection(
vehicle_transform.location, max_distance_to_check=30)
if not intersection_dist or intersection_dist < 4:
# We are not close to an intersection or we're already
# too close.
return target_speed
# Reduce the speed because we're getting close to an intersection.
# In this way, we can stop even if we detect the traffic light
# very late.
if intersection_dist < 30:
target_speed = min(target_speed, 5)
# We assume that we are at a stop sign.
if (intersection_dist < 10
and game_time - self._last_traffic_light_game_time > 4000):
if vehicle_speed < 0.09:
# We've already stopped at the intersection.
target_speed = min(target_speed, 12)
else:
# Stop at the intersection.
target_speed = min(target_speed, 0)
return target_speed
def __transform_to_3d(self, x, y, vehicle_transform, point_cloud,
depth_frame):
pos = None
if depth_frame:
pos = get_3d_world_position_with_depth_map(
x, y, depth_frame, self._bgr_camera_setup.width,
self._bgr_camera_setup.height, self._bgr_camera_setup.fov,
vehicle_transform * self._bgr_camera_setup.transform)
elif point_cloud is not None:
pos = get_3d_world_position_with_point_cloud(
x, y, point_cloud,
vehicle_transform * self._bgr_camera_setup.transform,
self._bgr_camera_setup.width, self._bgr_camera_setup.height,
self._bgr_camera_setup.fov)
if pos is None:
self._logger.error('Could not find lidar point for {} {}'.format(
x, y))
return pos
def __transform_tl_output(self, tls, vehicle_transform, point_cloud,
depth_frame):
traffic_lights = []
for tl in tls.detected_objects:
x = (tl.corners[0] + tl.corners[1]) / 2
y = (tl.corners[2] + tl.corners[3]) / 2
pos = self.__transform_to_3d(x, y, vehicle_transform, point_cloud,
depth_frame)
if pos:
traffic_lights.append((pos, tl.label))
return traffic_lights
def __transform_detector_output(self, obstacles_msg, vehicle_transform,
point_cloud, depth_frame):
vehicles = []
pedestrians = []
for detected_obj in obstacles_msg.detected_objects:
x = (detected_obj.corners[0] + detected_obj.corners[1]) / 2
y = (detected_obj.corners[2] + detected_obj.corners[3]) / 2
if detected_obj.label == 'person':
pos = self.__transform_to_3d(x, y, vehicle_transform,
point_cloud, depth_frame)
if pos:
pedestrians.append(pos)
elif (detected_obj.label in self._vehicle_labels):
pos = self.__transform_to_3d(x, y, vehicle_transform,
point_cloud, depth_frame)
if pos:
vehicles.append(pos)
return (pedestrians, vehicles)
def __stop_for_agents(self, vehicle_transform, wp_angle, wp_vector,
vehicles, pedestrians, traffic_lights, timestamp):
speed_factor = 1
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
for obs_vehicle_loc in vehicles:
if (not self._map or self._map.are_on_same_lane(
vehicle_transform.location, obs_vehicle_loc)):
self._logger.info(
'Ego {} and vehicle {} are on the same lane'.format(
vehicle_transform.location, obs_vehicle_loc))
new_speed_factor_v = pylot.control.utils.stop_vehicle(
vehicle_transform, obs_vehicle_loc, wp_vector,
speed_factor_v, self._flags)
if new_speed_factor_v < speed_factor_v:
speed_factor_v = new_speed_factor_v
self._logger.info(
'Vehicle {} reduced speed factor to {}'.format(
obs_vehicle_loc, speed_factor_v))
for obs_ped_loc in pedestrians:
if (not self._map or self._map.are_on_same_lane(
vehicle_transform.location, obs_ped_loc)):
self._logger.info(
'Ego {} and pedestrian {} are on the same lane'.format(
vehicle_transform.location, obs_ped_loc))
new_speed_factor_p = pylot.control.utils.stop_pedestrian(
vehicle_transform, obs_ped_loc, wp_vector, speed_factor_p,
self._flags)
if new_speed_factor_p < speed_factor_p:
speed_factor_p = new_speed_factor_p
self._logger.info(
'Pedestrian {} reduced speed factor to {}'.format(
obs_ped_loc, speed_factor_p))
for tl in traffic_lights:
if (not self._map or self._map.must_obbey_traffic_light(
vehicle_transform.location, tl[0])):
self._logger.info('Ego is obbeying traffic light {}'.format(
vehicle_transform.location, tl[0]))
tl_state = tl[1]
new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
vehicle_transform, tl[0], tl_state, wp_vector, wp_angle,
speed_factor_tl, self._flags)
if new_speed_factor_tl < speed_factor_tl:
speed_factor_tl = new_speed_factor_tl
self._logger.info(
'Traffic light {} reduced speed factor to {}'.format(
tl[0], speed_factor_tl))
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
state = {
'stop_pedestrian': speed_factor_p,
'stop_vehicle': speed_factor_v,
'stop_traffic_lights': speed_factor_tl
}
self._logger.info('{}: Agent speed factors {}'.format(
timestamp, state))
return speed_factor, state
def __get_steer(self, wp_angle):
steer = self._flags.steer_gain * wp_angle
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
return steer
def __get_throttle_brake_without_factor(self, current_speed, target_speed):
self._pid.target = target_speed
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return throttle, brake
def __get_throttle_brake(self, current_speed, target_speed, wp_angle_speed,
speed_factor):
# TODO(ionel): DO NOT HARDCODE VALUES!
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = target_speed * speed_factor
elif math.fabs(wp_angle_speed) < 0.5:
target_speed_adjusted = 6 * speed_factor
else:
target_speed_adjusted = 3 * speed_factor
self._pid.target = target_speed_adjusted
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return throttle, brake
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, target_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self.__get_steer(wp_angle)
throttle, brake = self.__get_throttle_brake(current_speed,
target_speed,
wp_angle_speed,
speed_factor)
return ControlMessage(steer, throttle, brake, False, False, timestamp)
class PerfectLaneDetectionOperator(erdos.Operator):
"""Operator that uses the Carla world to perfectly detect lanes.
Args:
pose_stream (:py:class:`erdos.ReadStream`): Stream on which pose
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
detected_lane_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator writes
:py:class:`~pylot.perception.messages.LanesMessage` messages.
flags (absl.flags): Object to be used to access absl flags.
"""
def __init__(self, pose_stream: ReadStream, open_drive_stream: ReadStream,
detected_lane_stream: WriteStream, flags):
pose_stream.add_callback(self.on_position_update,
[detected_lane_stream])
self._flags = flags
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
@staticmethod
def connect(pose_stream: ReadStream, open_drive_stream: ReadStream):
detected_lane_stream = erdos.WriteStream()
return [detected_lane_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 world here we're sure it is up-to-date.
if self._flags.execution_mode == 'simulation':
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.config.log_file_name)
from pylot.simulation.utils import get_world
_, self._world = get_world(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
def on_opendrive_map(self, msg: Message):
"""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_position_update(self, pose_msg: Message,
detected_lane_stream: WriteStream):
"""Invoked on the receipt of an update to the position of the vehicle.
Uses the position of the vehicle to get future waypoints and draw
lane markings using those waypoints.
Args:
pose_msg: Contains the current location of the ego vehicle.
"""
self._logger.debug('@{}: received pose message'.format(
pose_msg.timestamp))
vehicle_location = pose_msg.data.transform.location
if self._map:
lanes = [self._map.get_lane(vehicle_location)]
lanes[0].draw_on_world(self._world)
else:
self._logger.debug('@{}: map is not ready yet'.format(
pose_msg.timestamp))
lanes = []
output_msg = LanesMessage(pose_msg.timestamp, lanes)
detected_lane_stream.send(output_msg)
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)
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 PerfectLaneDetectionOperator(erdos.Operator):
"""Operator that uses the simulator to perfectly detect lanes.
Args:
pose_stream (:py:class:`erdos.ReadStream`): Stream on which pose
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
detected_lane_stream (:py:class:`erdos.WriteStream`): Stream on which
the operator writes
:py:class:`~pylot.perception.messages.LanesMessage` messages.
flags (absl.flags): Object to be used to access absl flags.
"""
def __init__(self, pose_stream: ReadStream, open_drive_stream: ReadStream,
center_camera_stream: ReadStream,
detected_lane_stream: WriteStream, flags):
pose_stream.add_callback(self.on_pose_update)
center_camera_stream.add_callback(self.on_bgr_camera_update)
erdos.add_watermark_callback([pose_stream, center_camera_stream],
[detected_lane_stream],
self.on_position_update)
self._flags = flags
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._logger_lane = erdos.utils.setup_logging(
self.config.name + "_lane", self.config.log_file_name + "_lane")
self._bgr_msgs = deque()
self._pose_msgs = deque()
self._frame_cnt = 0
@staticmethod
def connect(pose_stream: ReadStream, open_drive_stream: ReadStream,
center_camera_stream: ReadStream):
detected_lane_stream = erdos.WriteStream()
return [detected_lane_stream]
def destroy(self):
self._logger.warn('destroying {}'.format(self.config.name))
def run(self):
# Run method is invoked after all operators finished initializing.
# Thus, we're sure the world is up-to-date here.
if self._flags.execution_mode == 'simulation':
from pylot.map.hd_map import HDMap
from pylot.simulation.utils import get_map
self._map = HDMap(
get_map(self._flags.simulator_host, self._flags.simulator_port,
self._flags.simulator_timeout),
self.config.log_file_name)
from pylot.simulation.utils import get_world
_, self._world = get_world(self._flags.simulator_host,
self._flags.simulator_port,
self._flags.simulator_timeout)
def on_opendrive_map(self, msg: Message):
"""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))
from pylot.simulation.utils import map_from_opendrive
self._map = map_from_opendrive(msg.data)
def on_bgr_camera_update(self, msg: Message):
self._logger.debug('@{}: received BGR frame'.format(msg.timestamp))
self._bgr_msgs.append(msg)
def on_pose_update(self, msg: Message):
self._logger.debug('@{}: received pose message'.format(msg.timestamp))
self._pose_msgs.append(msg)
@erdos.profile_method()
def on_position_update(self, timestamp: Timestamp,
detected_lane_stream: WriteStream):
"""Invoked on the receipt of an update to the position of the vehicle.
Uses the position of the vehicle to get future waypoints and draw
lane markings using those waypoints.
Args:
pose_msg: Contains the current location of the ego vehicle.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
bgr_msg = self._bgr_msgs.popleft()
pose_msg = self._pose_msgs.popleft()
vehicle_location = pose_msg.data.transform.location
self._frame_cnt += 1
if self._map:
lanes = self._map.get_all_lanes(vehicle_location)
if self._flags.log_lane_detection_camera:
camera_setup = bgr_msg.frame.camera_setup
frame = np.zeros((camera_setup.height, camera_setup.width),
dtype=np.dtype("uint8"))
binary_frame = frame.copy()
for lane in lanes:
lane.collect_frame_data(frame,
binary_frame,
camera_setup,
inverse_transform=pose_msg.data.
transform.inverse_transform())
self._logger.debug('@{}: detected {} lanes'.format(
bgr_msg.timestamp, len(lanes)))
if self._frame_cnt % self._flags.log_every_nth_message == 0:
instance_file_name = os.path.join(
self._flags.data_path,
'{}-{}.png'.format("lane",
bgr_msg.timestamp.coordinates[0]))
binary_file_name = os.path.join(
self._flags.data_path,
'{}-{}.png'.format("binary_lane",
bgr_msg.timestamp.coordinates[0]))
instance_img = Image.fromarray(frame)
binary_img = Image.fromarray(binary_frame)
instance_img.save(instance_file_name)
binary_img.save(binary_file_name)
self._logger_lane.debug(
'@{}: Created binary lane and lane images in {}'.
format(pose_msg.timestamp, self._flags.data_path))
else:
for lane in lanes:
lane.draw_on_world(self._world)
else:
self._logger.debug('@{}: map is not ready yet'.format(
pose_msg.timestamp))
lanes = []
output_msg = LanesMessage(pose_msg.timestamp, lanes)
detected_lane_stream.send(output_msg)
def on_open_drive_map(self, msg):
self._logger.debug('@{}: open drive update'.format(msg.timestamp))
self._map = HDMap(carla.Map('challenge', msg.data),
self._log_file_name)
class GroundAgentOperator(erdos.Operator):
def __init__(self,
can_bus_stream,
ground_obstacles_stream,
ground_traffic_lights_stream,
waypoints_stream,
control_stream,
name,
flags,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
ground_obstacles_stream.add_callback(self.on_obstacles_update)
ground_traffic_lights_stream.add_callback(
self.on_traffic_lights_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([
can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream
], [control_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._obstacle_msgs = deque()
self._traffic_light_msgs = deque()
self._waypoint_msgs = deque()
@staticmethod
def connect(can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream):
control_stream = erdos.WriteStream()
return [control_stream]
def on_watermark(self, timestamp, control_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
# Get hero vehicle info.
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
vehicle_speed = can_bus_msg.data.forward_speed
# Get waypoints.
waypoint_msg = self._waypoint_msgs.popleft()
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
# Get ground obstacle info.
obstacles = self._obstacle_msgs.popleft().obstacles
# Get ground traffic lights info.
traffic_lights = self._traffic_light_msgs.popleft().traffic_lights
speed_factor, state = self.stop_for_agents(vehicle_transform.location,
wp_angle, wp_vector,
obstacles, traffic_lights)
control_stream.send(
self.get_control_message(wp_angle, wp_angle_speed, speed_factor,
vehicle_speed, timestamp))
def on_waypoints_update(self, msg):
self._logger.debug('@{}: received waypoints message'.format(
msg.timestamp))
self._waypoint_msgs.append(msg)
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_obstacles_update(self, msg):
self._logger.debug('@{}: received obstacles message'.format(
msg.timestamp))
self._obstacle_msgs.append(msg)
def on_traffic_lights_update(self, msg):
self._logger.debug('@{}: received traffic lights message'.format(
msg.timestamp))
self._traffic_light_msgs.append(msg)
def stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
obstacles, traffic_lights):
speed_factor = 1
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
for obstacle in obstacles:
if obstacle.label == 'vehicle' and self._flags.stop_for_vehicles:
# Only brake for vehicles that are in ego vehicle's lane.
if self._map.are_on_same_lane(ego_vehicle_location,
obstacle.transform.location):
new_speed_factor_v = pylot.control.utils.stop_vehicle(
ego_vehicle_location, obstacle.transform.location,
wp_vector, speed_factor_v, self._flags)
speed_factor_v = min(speed_factor_v, new_speed_factor_v)
if obstacle.label == 'person' and \
self._flags.stop_for_people:
# Only brake for people that are on the road.
if self._map.is_on_lane(obstacle.transform.location):
new_speed_factor_p = pylot.control.utils.stop_person(
ego_vehicle_location, obstacle.transform.location,
wp_vector, speed_factor_p, self._flags)
speed_factor_p = min(speed_factor_p, new_speed_factor_p)
if self._flags.stop_for_traffic_lights:
for tl in traffic_lights:
if (self._map.must_obbey_traffic_light(ego_vehicle_location,
tl.transform.location)
and self._is_traffic_light_visible(
ego_vehicle_location, tl.transform.location)):
new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
ego_vehicle_location, tl.transform.location, tl.state,
wp_vector, wp_angle, speed_factor_tl, self._flags)
speed_factor_tl = min(speed_factor_tl, new_speed_factor_tl)
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
state = {
'stop_person': speed_factor_p,
'stop_vehicle': speed_factor_v,
'stop_traffic_lights': speed_factor_tl
}
return speed_factor, state
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self._flags.steer_gain * wp_angle
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self._flags.target_speed * speed_factor / 2
else:
target_speed_adjusted = self._flags.target_speed * speed_factor
self._pid.target = target_speed_adjusted
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def _is_traffic_light_visible(self, ego_vehicle_location, tl_location):
_, tl_dist = pylot.control.utils.get_world_vec_dist(
ego_vehicle_location.x, ego_vehicle_location.y, tl_location.x,
tl_location.y)
return tl_dist > self._flags.traffic_light_min_dist_thres
class MPCAgentOperator(erdos.Operator):
def __init__(self,
can_bus_stream,
ground_obstacles_stream,
ground_traffic_lights_stream,
waypoints_stream,
control_stream,
name,
flags,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
ground_obstacles_stream.add_callback(self.on_obstacles_update)
ground_traffic_lights_stream.add_callback(
self.on_traffic_lights_update)
waypoints_stream.add_callback(self.on_waypoints_update)
erdos.add_watermark_callback([
can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream
], [control_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._config = global_config
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
_, self._world = get_world(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._obstacles_msgs = deque()
self._traffic_light_msgs = deque()
self._waypoint_msgs = deque()
@staticmethod
def connect(can_bus_stream, ground_obstacles_stream,
ground_traffic_lights_stream, waypoints_stream):
control_stream = erdos.WriteStream()
return [control_stream]
def on_waypoints_update(self, msg):
self._logger.debug('@{}: waypoints update'.format(msg.timestamp))
self._waypoint_msgs.append(msg)
def on_can_bus_update(self, msg):
self._logger.debug('@{}: can bus update'.format(msg.timestamp))
self._can_bus_msgs.append(msg)
def on_obstacles_update(self, msg):
self._logger.debug('@{}: obstacles update'.format(msg.timestamp))
self._obstacles_msgs.append(msg)
def on_traffic_lights_update(self, msg):
self._logger.debug('@{}: traffic lights update'.format(msg.timestamp))
self._traffic_light_msgs.append(msg)
def on_watermark(self, timestamp, control_stream):
self._logger.debug('Received watermark {}'.format(timestamp))
# Get hero vehicle info.
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
vehicle_speed = can_bus_msg.data.forward_speed
# Get waypoints.
waypoint_msg = self._waypoint_msgs.popleft()
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
waypoints = deque(itertools.islice(waypoint_msg.waypoints, 0,
50)) # only take 50 meters
# Get ground obstacles info.
obstacles = self._obstacles_msgs.popleft().obstacles
# Get ground traffic lights info.
traffic_lights = self._traffic_light_msgs.popleft().traffic_lights
speed_factor, state = self.stop_for_agents(vehicle_transform.location,
wp_angle, wp_vector,
wp_angle_speed, obstacles,
traffic_lights)
control_msg = self.get_control_message(waypoints, vehicle_transform,
vehicle_speed, speed_factor,
timestamp)
self._logger.debug("Throttle: {}".format(control_msg.throttle))
self._logger.debug("Steer: {}".format(control_msg.steer))
self._logger.debug("Brake: {}".format(control_msg.brake))
self._logger.debug("State: {}".format(state))
control_stream.send(control_msg)
def stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
wp_angle_speed, obstacles, traffic_lights):
speed_factor = 1
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
for obstacle in obstacles:
if obstacle.label == 'vehicle' and self._flags.stop_for_vehicles:
# Only brake for vehicles that are in ego vehicle's lane.
if self._map.are_on_same_lane(ego_vehicle_location,
obstacle.transform.location):
new_speed_factor_v = pylot.control.utils.stop_vehicle(
ego_vehicle_location, obstacle.transform.location,
wp_vector, speed_factor_v, self._flags)
speed_factor_v = min(speed_factor_v, new_speed_factor_v)
if obstacle.label == 'person' and \
self._flags.stop_for_people:
# Only brake for people that are on the road.
if self._map.is_on_lane(obstacle.transform.location):
new_speed_factor_p = pylot.control.utils.stop_person(
ego_vehicle_location, obstacle.transform.location,
wp_vector, speed_factor_p, self._flags)
speed_factor_p = min(speed_factor_p, new_speed_factor_p)
if self._flags.stop_for_traffic_lights:
for tl in traffic_lights:
if (self._map.must_obbey_traffic_light(ego_vehicle_location,
tl.transform.location)
and self._is_traffic_light_visible(
ego_vehicle_location, tl.transform.location)):
new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
ego_vehicle_location, tl.transform.location, tl.state,
wp_vector, wp_angle, speed_factor_tl, self._flags)
speed_factor_tl = min(speed_factor_tl, new_speed_factor_tl)
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
# slow down around corners
if math.fabs(wp_angle_speed) < 0.1:
speed_factor = 0.3 * speed_factor
state = {
'stop_person': speed_factor_p,
'stop_vehicle': speed_factor_v,
'stop_traffic_lights': speed_factor_tl
}
return speed_factor, state
def setup_mpc(self, waypoints):
path = np.array([[wp.location.x, wp.location.y] for wp in waypoints])
# convert target waypoints into spline
spline = CubicSpline2D(path[:, 0], path[:, 1], self._logger)
ss = []
vels = []
xs = []
ys = []
yaws = []
ks = []
for s in spline.s[:-2]:
x, y = spline.calc_position(s)
yaw = np.abs(spline.calc_yaw(s))
k = spline.calc_curvature(s)
xs.append(x)
ys.append(y)
yaws.append(yaw)
ks.append(k)
ss.append(s)
vels.append(18)
self._config["reference"] = {
't_list': [], # Time [s]
's_list': ss, # Arc distance [m]
'x_list': xs, # Desired X coordinates [m]
'y_list': ys, # Desired Y coordinates [m]
'k_list': ks, # Curvatures [1/m]
'vel_list': vels, # Desired tangential velocities [m/s]
'yaw_list': yaws, # Yaws [rad]
}
# initialize mpc controller
self.mpc = ModelPredictiveController(config=self._config)
def get_control_message(self, waypoints, vehicle_transform, current_speed,
speed_factor, timestamp):
ego_location = vehicle_transform.location.as_carla_location()
ego_yaw = np.deg2rad(zero_to_2_pi(vehicle_transform.rotation.yaw))
# step the controller
self.setup_mpc(waypoints)
self.mpc.vehicle.x = ego_location.x
self.mpc.vehicle.y = ego_location.y
self.mpc.vehicle.yaw = ego_yaw
try:
self.mpc.step()
except Exception as e:
self._logger.info("Failed to solve MPC.")
self._logger.info(e)
return ControlMessage(0, 0, 1, False, False, timestamp)
# compute pid controls
target_speed = self.mpc.solution.vel_list[0] * speed_factor
target_yaw = self.mpc.solution.yaw_list[0]
target_steer_rad = self.mpc.horizon_steer[0] # in rad
steer = self.__rad2steer(target_steer_rad) # [-1.0, 1.0]
throttle, brake = self.__get_throttle_brake_without_factor(
current_speed, target_speed)
# send controls
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def _is_traffic_light_visible(self, ego_vehicle_location, tl_location):
_, tl_dist = pylot.control.utils.get_world_vec_dist(
ego_vehicle_location.x, ego_vehicle_location.y, tl_location.x,
tl_location.y)
return tl_dist > self._flags.traffic_light_min_dist_thres
def __rad2steer(self, rad):
"""
Converts radians to steer input.
:return: float [-1.0, 1.0]
"""
steer = self._flags.steer_gain * rad
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
return steer
def __steer2rad(self, steer):
"""
Converts radians to steer input. Assumes max steering angle is -45, 45 degrees
:return: float [-1.0, 1.0]
"""
rad = steer / self._flags.steer_gain
if rad > 0:
rad = min(rad, np.pi / 2)
else:
rad = max(rad, -np.pi / 2)
return rad
def __get_throttle_brake_without_factor(self, current_speed, target_speed):
self._pid.target = target_speed
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return throttle, brake
class WaypointPlanningOperator(erdos.Operator):
""" Planning operator for Carla 0.9.x.
The operator either receives all the waypoints from the scenario runner
agent (on the global trajectory stream), or computes waypoints using the
HD Map.
"""
def __init__(self,
can_bus_stream,
open_drive_stream,
global_trajectory_stream,
waypoints_stream,
name,
flags,
goal_location=None,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update, [waypoints_stream])
open_drive_stream.add_callback(self.on_opendrive_map)
global_trajectory_stream.add_callback(self.on_global_trajectory)
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
# Initialize the state of the behaviour planner.
# XXX(ionel): The behaviour planner is not ready yet.
self.__initialize_behaviour_planner()
# We do not know yet the vehicle's location.
self._vehicle_transform = None
# Deque of waypoints the vehicle must follow. The waypoints are either
# received on the global trajectory stream when running using the
# scenario runner, or computed using the Carla global planner when
# running in stand-alone mode. The waypoints are Pylot transforms.
self._waypoints = collections.deque()
# The operator picks the wp_num_steer-th waypoint to compute the angle
# it has to steer by when taking a turn.
self._wp_num_steer = 9 # use 10th waypoint for steering
# The operator picks the wp_num_speed-th waypoint to compute the angle
# it has to steer by when driving straight.
self._wp_num_speed = 4 # use tth waypoint for speed
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
if not hasattr(self._flags, 'track'):
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.info('Planner running in stand-alone mode')
assert goal_location, 'Planner has not received a goal location'
# Transform goal location to carla.Location
self._goal_location = goal_location
# Recompute waypoints upon each run.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
# TODO(ionel): HACK! In Carla challenge track 1 and 2 the waypoints
# are coarse grained (30 meters apart). We pick the first waypoint
# to compute the angles. However, we should instead implement
# trajectory planning.
if self._flags.track == 1 or self._flags == 2:
self._wp_num_steer = 1
self._wp_num_speed = 1
@staticmethod
def connect(can_bus_stream, open_drive_stream, global_trajectory_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
def on_opendrive_map(self, msg):
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
self._logger.info('Initializing HDMap from open drive stream')
self._map = HDMap(carla.Map('map', msg.data), self._log_file_name)
def on_global_trajectory(self, msg):
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.as_carla_location()
else:
# Trajectory does not contain any waypoints. We assume we have
# arrived at destionation.
goal_loc = self._vehicle_transform.location
self._goal_location = goal_loc.as_carla_location()
assert self._goal_location, 'Planner does not have a goal'
self._waypoints = collections.deque()
for waypoint_option in msg.data:
self._waypoints.append(waypoint_option[0])
def on_can_bus_update(self, msg, waypoints_stream):
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._vehicle_transform = msg.data.transform
self.__update_waypoints()
next_waypoint_steer = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_steer)]
next_waypoint_speed = self._waypoints[min(
len(self._waypoints) - 1, self._wp_num_speed)]
# Get vectors and angles to corresponding speed and steer waypoints.
wp_steer_vector, wp_steer_angle = get_waypoint_vector_and_angle(
next_waypoint_steer, self._vehicle_transform)
wp_speed_vector, wp_speed_angle = get_waypoint_vector_and_angle(
next_waypoint_speed, self._vehicle_transform)
head_waypoints = collections.deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
output_msg = WaypointsMessage(msg.timestamp,
waypoints=head_waypoints,
target_speed=self._flags.target_speed,
wp_angle=wp_steer_angle,
wp_vector=wp_steer_vector,
wp_angle_speed=wp_speed_angle)
waypoints_stream.send(output_msg)
def __update_waypoints(self):
""" Updates the waypoints.
Depending on setup, the method either recomputes the waypoints
between the ego vehicle and the goal location, or just garbage collects
waypoints that have already been achieved.
Returns:
(wp_steer, wp_speed): The waypoints to be used to compute steer and
speed angles.
"""
if self._recompute_waypoints:
ego_location = self._vehicle_transform.location.as_carla_location()
self._waypoints = self._map.compute_waypoints(
ego_location, self._goal_location)
self.__remove_completed_waypoints()
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = collections.deque([self._vehicle_transform])
def __remove_completed_waypoints(self):
""" Removes waypoints that the ego vehicle has already completed.
The method first finds the closest waypoint, removes all waypoints
that are before the closest waypoint, and finally removes the
closest waypoint if the ego vehicle is very close to it
(i.e., close to completion)."""
min_dist = 10000000
min_index = 0
index = 0
for waypoint in self._waypoints:
# XXX(ionel): We only check the first 10 waypoints.
if index > 10:
break
dist = waypoint.location.distance(self._vehicle_transform.location)
if dist < min_dist:
min_dist = dist
min_index = index
# Remove waypoints that are before the closest waypoint. The ego
# vehicle already completed them.
while min_index > 0:
self._waypoints.popleft()
min_index -= 1
# The closest waypoint is almost complete, remove it.
if min_dist < WAYPOINT_COMPLETION_THRESHOLD:
self._waypoints.popleft()
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.READY
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_speed,
pylot.planning.cost_functions.cost_lane_change,
pylot.planning.cost_functions.cost_inefficiency
]
reach_speed_weight = 10**5
reach_goal_weight = 10**6
efficiency_weight = 10**4
# How important a cost function is.
self._function_weights = [
reach_speed_weight, reach_goal_weight, efficiency_weight
]
def __best_transition(self, vehicle_transform, predictions):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = 10000000
for state in possible_next_states:
# Generate trajectory for next state.
vehicle_info, trajectory_for_state = self.__generate_trajectory(
state, vehicle_transform, predictions)
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](vehicle_info, predictions,
trajectory_for_state)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if best_next_state is None or state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
def __generate_trajectory(self, next_state, vehicle_transform,
predictions):
raise NotImplementedError
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.READY:
return [BehaviorPlannerState.READY, BehaviorPlannerState.KEEP_LANE]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
class WaypointPlanningOperator(erdos.Operator):
"""Computes waypoints the ego vehicle must follow.
If the operator is running in CARLA challenge mode, then it receives all
the waypoints from the scenario runner agent (on the global trajectory
stream). Otherwise, it computes waypoints using the HD Map.
The planner reduces speed/stops whenever it encounters an obstacle,
and waits for the obstacle to move. It does not implement an obstacle
avoidance policy.
Args:
can_bus_stream (:py:class:`erdos.ReadStream`): Stream on which can bus
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
global_trajectory_stream (:py:class:`erdos.ReadStream`): Stream on
which the scenario runner publishes waypoints.
waypoints_stream (:py:class:`erdos.WriteStream`): Stream on which the
operator sends waypoints the ego vehicle must follow.
flags (absl.flags): Object to be used to access absl flags.
goal_location (:py:class:`~pylot.utils.Location`): The goal location of
the ego vehicle.
"""
def __init__(self,
can_bus_stream,
open_drive_stream,
global_trajectory_stream,
obstacles_stream,
traffic_lights_stream,
waypoints_stream,
flags,
goal_location=None):
can_bus_stream.add_callback(self.on_can_bus_update)
open_drive_stream.add_callback(self.on_opendrive_map)
global_trajectory_stream.add_callback(self.on_global_trajectory)
obstacles_stream.add_callback(self.on_obstacles_update)
traffic_lights_stream.add_callback(self.on_traffic_lights_update)
erdos.add_watermark_callback(
[can_bus_stream, obstacles_stream, traffic_lights_stream],
[waypoints_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
# Initialize the state of the behaviour planner.
# XXX(ionel): The behaviour planner is not ready yet.
self.__initialize_behaviour_planner()
# We do not know yet the vehicle's location.
self._vehicle_transform = None
self._goal_location = goal_location
self._map = None
# Deque of waypoints the vehicle must follow. The waypoints are either
# received on the global trajectory stream when running using the
# scenario runner, or computed using the Carla global planner when
# running in stand-alone mode. The waypoints are Pylot transforms.
self._waypoints = deque()
self._can_bus_msgs = deque()
self._obstacles_msgs = deque()
self._traffic_light_msgs = deque()
@staticmethod
def connect(can_bus_stream, open_drive_stream, global_trajectory_stream,
obstacles_stream, traffic_lights_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.
# We're not running in challenge mode if no track flag is present.
# Thus, we can directly get the map from the simulator.
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')
# Recompute waypoints every RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS.
self._recompute_waypoints = True
else:
# Do not recompute waypoints upon each run.
self._recompute_waypoints = False
self._watermark_cnt = 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))
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 destination.
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_can_bus_update(self, msg):
"""Invoked whenever a message is received on the can bus stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
info about the ego vehicle.
"""
self._logger.debug('@{}: received can bus message'.format(
msg.timestamp))
self._can_bus_msgs.append(msg)
def on_obstacles_update(self, msg):
self._logger.debug('@{}: obstacles update'.format(msg.timestamp))
self._obstacles_msgs.append(msg)
def on_traffic_lights_update(self, msg):
self._logger.debug('@{}: traffic lights update'.format(msg.timestamp))
self._traffic_light_msgs.append(msg)
@erdos.profile_method()
def on_watermark(self, timestamp, waypoints_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
self._watermark_cnt += 1
# Get hero vehicle info.
can_bus_msg = self._can_bus_msgs.popleft()
self._vehicle_transform = can_bus_msg.data.transform
tl_msg = self._traffic_light_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
if (self._recompute_waypoints
and self._watermark_cnt % RECOMPUTE_WAYPOINT_EVERY_N_WATERMARKS
== 0):
self._waypoints = self._map.compute_waypoints(
self._vehicle_transform.location, self._goal_location)
self.__remove_completed_waypoints()
if not self._waypoints or len(self._waypoints) == 0:
# If waypoints are empty (e.g., reached destination), set waypoint
# to current vehicle location.
self._waypoints = deque([self._vehicle_transform])
wp_vector, wp_angle = \
pylot.planning.utils.compute_waypoint_vector_and_angle(
self._vehicle_transform, self._waypoints,
DEFAULT_TARGET_WAYPOINT)
speed_factor, _ = pylot.planning.utils.stop_for_agents(
self._vehicle_transform.location, wp_angle, wp_vector,
obstacles_msg.obstacles, tl_msg.obstacles, self._flags,
self._logger, self._map, timestamp)
target_speed = speed_factor * self._flags.target_speed
self._logger.debug('@{}: computed speed factor: {}'.format(
timestamp, speed_factor))
self._logger.debug('@{}: computed target speed: {}'.format(
timestamp, target_speed))
head_waypoints = deque(
itertools.islice(self._waypoints, 0, DEFAULT_NUM_WAYPOINTS))
target_speeds = deque(
[target_speed for _ in range(len(head_waypoints))])
waypoints_stream.send(
WaypointsMessage(timestamp, head_waypoints, target_speeds))
def __remove_completed_waypoints(self):
"""Removes waypoints that the ego vehicle has already completed.
The method first finds the closest waypoint, removes all waypoints
that are before the closest waypoint, and finally removes the closest
waypoint if the ego vehicle is very close to it (i.e., close to
completion).
"""
min_dist = 10000000
min_index = 0
index = 0
for waypoint in self._waypoints:
# XXX(ionel): We only check the first 10 waypoints.
if index > 10:
break
dist = waypoint.location.distance(self._vehicle_transform.location)
if dist < min_dist:
min_dist = dist
min_index = index
# Remove waypoints that are before the closest waypoint. The ego
# vehicle already completed them.
while min_index > 0:
self._waypoints.popleft()
min_index -= 1
# The closest waypoint is almost complete, remove it.
if min_dist < WAYPOINT_COMPLETION_THRESHOLD:
self._waypoints.popleft()
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.READY
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_speed,
pylot.planning.cost_functions.cost_lane_change,
pylot.planning.cost_functions.cost_inefficiency
]
reach_speed_weight = 10**5
reach_goal_weight = 10**6
efficiency_weight = 10**4
# How important a cost function is.
self._function_weights = [
reach_speed_weight, reach_goal_weight, efficiency_weight
]
def __best_transition(self, vehicle_transform, predictions):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = 10000000
for state in possible_next_states:
# Generate trajectory for next state.
vehicle_info, trajectory_for_state = self.__generate_trajectory(
state, vehicle_transform, predictions)
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](vehicle_info, predictions,
trajectory_for_state)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if best_next_state is None or state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
def __generate_trajectory(self, next_state, vehicle_transform,
predictions):
raise NotImplementedError
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.READY:
return [BehaviorPlannerState.READY, BehaviorPlannerState.KEEP_LANE]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
def on_opendrive_map(self, msg):
self._logger.debug('@{}: received open drive message'.format(
msg.timestamp))
self._logger.info('Initializing HDMap from open drive stream')
self._map = HDMap(carla.Map('map', msg.data), self._log_file_name)
def on_opendrive_map(self, msg):
self._map = HDMap(carla.Map('challenge', msg.data),
self._log_file_name)
def on_opendrive_map(self, msg):
self._logger.info('Planner running in scenario runner mode')
self._map = HDMap(carla.Map('map', msg.data), self._log_file_name)
class BehaviorPlanningOperator(erdos.Operator):
"""Behavior planning operator.
Args:
pose_stream (:py:class:`erdos.ReadStream`): Stream on which pose
info is received.
open_drive_stream (:py:class:`erdos.ReadStream`): Stream on which open
drive string representations are received. The operator can
construct HDMaps out of the open drive strings.
route_stream (:py:class:`erdos.ReadStream`): Stream on which the
scenario runner publishes waypoints.
trajectory_stream (:py:class:`erdos.WriteStream`): Stream on which the
operator sends waypoints the ego vehicle must follow.
flags (absl.flags): Object to be used to access absl flags.
goal_location (:py:class:`~pylot.utils.Location`): The goal location of
the ego vehicle.
"""
def __init__(self,
pose_stream,
open_drive_stream,
route_stream,
trajectory_stream,
flags,
goal_location=None):
pose_stream.add_callback(self.on_pose_update)
open_drive_stream.add_callback(self.on_opendrive_map)
route_stream.add_callback(self.on_route_msg)
erdos.add_watermark_callback(
[pose_stream, open_drive_stream, route_stream],
[trajectory_stream], self.on_watermark)
self._logger = erdos.utils.setup_logging(self.config.name,
self.config.log_file_name)
self._flags = flags
# Do not set the goal location here so that the behavior planner
# issues an initial message.
self._goal_location = None
# Initialize the state of the behaviour planner.
self.__initialize_behaviour_planner()
self._pose_msgs = deque()
self._ego_info = EgoInfo()
if goal_location:
self._route = Waypoints(
deque([
pylot.utils.Transform(goal_location,
pylot.utils.Rotation())
]))
else:
self._route = None
self._map = None
@staticmethod
def connect(pose_stream, open_drive_stream, route_stream):
trajectory_stream = erdos.WriteStream()
return [trajectory_stream]
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),
self.config.log_file_name)
def on_route_msg(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.waypoints.waypoints)))
self._route = msg.waypoints
def on_pose_update(self, msg):
"""Invoked whenever a message is received on the pose stream.
Args:
msg (:py:class:`~erdos.message.Message`): Message that contains
info about the ego vehicle.
"""
self._logger.debug('@{}: received pose message'.format(msg.timestamp))
self._pose_msgs.append(msg)
def on_watermark(self, timestamp, trajectory_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
pose_msg = self._pose_msgs.popleft()
ego_transform = pose_msg.data.transform
self._ego_info.update(self._state, pose_msg)
old_state = self._state
self._state = self.__best_state_transition(self._ego_info)
self._logger.debug('@{}: agent transitioned from {} to {}'.format(
timestamp, old_state, self._state))
# Remove the waypoint from the route if we're close to it.
if (old_state != self._state
and self._state == BehaviorPlannerState.OVERTAKE):
self._route.remove_waypoint_if_close(ego_transform.location, 10)
else:
if not self._map.is_intersection(ego_transform.location):
self._route.remove_waypoint_if_close(ego_transform.location, 5)
else:
self._route.remove_waypoint_if_close(ego_transform.location,
3.5)
new_goal_location = None
if len(self._route.waypoints) > 1:
new_goal_location = self._route.waypoints[1].location
elif len(self._route.waypoints) == 1:
new_goal_location = self._route.waypoints[0].location
else:
new_goal_location = ego_transform.location
if new_goal_location != self._goal_location:
self._goal_location = new_goal_location
if self._map:
# Use the map to compute more fine-grained waypoints.
waypoints = self._map.compute_waypoints(
ego_transform.location, self._goal_location)
road_options = deque([
pylot.utils.RoadOption.LANE_FOLLOW
for _ in range(len(waypoints))
])
waypoints = Waypoints(waypoints, road_options=road_options)
else:
# Map is not available, send the route.
waypoints = self._route
if not waypoints or waypoints.is_empty():
# If waypoints are empty (e.g., reached destination), set
# waypoints to current vehicle location.
waypoints = Waypoints(
deque([ego_transform]),
road_options=deque([pylot.utils.RoadOption.LANE_FOLLOW]))
trajectory_stream.send(
WaypointsMessage(timestamp, waypoints, self._state))
elif old_state != self._state:
# Send the state update.
trajectory_stream.send(
WaypointsMessage(timestamp, None, self._state))
trajectory_stream.send(erdos.WatermarkMessage(timestamp))
def __initialize_behaviour_planner(self):
# State the planner is in.
self._state = BehaviorPlannerState.FOLLOW_WAYPOINTS
# Cost functions. Output between 0 and 1.
self._cost_functions = [
pylot.planning.cost_functions.cost_overtake,
]
# How important a cost function is.
self._function_weights = [1]
def __successor_states(self):
""" Returns possible state transitions from current state."""
if self._state == BehaviorPlannerState.FOLLOW_WAYPOINTS:
# Can transition to OVERTAKE if the ego vehicle has been stuck
# behind an obstacle for a while.
return [
BehaviorPlannerState.FOLLOW_WAYPOINTS,
BehaviorPlannerState.OVERTAKE
]
elif self._state == BehaviorPlannerState.OVERTAKE:
return [
BehaviorPlannerState.OVERTAKE,
BehaviorPlannerState.FOLLOW_WAYPOINTS
]
elif self._state == BehaviorPlannerState.KEEP_LANE:
# 1) keep_lane -> prepare_lane_change_left
# 2) keep_lane -> prepare_lane_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT:
# 1) prepare_lane_change_left -> keep_lane
# 2) prepare_lane_change_left -> lange_change_left
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_LEFT,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_LEFT:
# 1) lange_change_left -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_LEFT
]
elif self._state == BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT:
# 1) prepare_lane_change_right -> keep_lane
# 2) prepare_lane_change_right -> lange_change_right
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.PREPARE_LANE_CHANGE_RIGHT,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
elif self._state == BehaviorPlannerState.LANE_CHANGE_RIGHT:
# 1) lane_change_right -> keep_lane
return [
BehaviorPlannerState.KEEP_LANE,
BehaviorPlannerState.LANE_CHANGE_RIGHT
]
else:
raise ValueError('Unexpected vehicle state {}'.format(self._state))
def __best_state_transition(self, ego_info):
""" Computes most likely state transition from current state."""
# Get possible next state machine states.
possible_next_states = self.__successor_states()
best_next_state = None
min_state_cost = np.infty
for state in possible_next_states:
state_cost = 0
# Compute the cost of the trajectory.
for i in range(len(self._cost_functions)):
cost_func = self._cost_functions[i](self._state, state,
ego_info)
state_cost += self._function_weights[i] * cost_func
# Check if it's the best trajectory.
if state_cost < min_state_cost:
best_next_state = state
min_state_cost = state_cost
return best_next_state
class GroundAgentOperator(Op):
def __init__(self, name, flags, log_file_name=None, csv_file_name=None):
super(GroundAgentOperator, self).__init__(name)
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._flags = flags
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
self._can_bus_msgs = deque()
self._pedestrian_msgs = deque()
self._vehicle_msgs = deque()
self._traffic_light_msgs = deque()
self._speed_limit_sign_msgs = deque()
self._waypoint_msgs = deque()
self._lock = threading.Lock()
@staticmethod
def setup_streams(input_streams):
input_streams.filter(pylot.utils.is_can_bus_stream).add_callback(
GroundAgentOperator.on_can_bus_update)
input_streams.filter(
pylot.utils.is_ground_pedestrians_stream).add_callback(
GroundAgentOperator.on_pedestrians_update)
input_streams.filter(
pylot.utils.is_ground_vehicles_stream).add_callback(
GroundAgentOperator.on_vehicles_update)
input_streams.filter(
pylot.utils.is_ground_traffic_lights_stream).add_callback(
GroundAgentOperator.on_traffic_lights_update)
input_streams.filter(
pylot.utils.is_ground_speed_limit_signs_stream).add_callback(
GroundAgentOperator.on_speed_limit_signs_update)
input_streams.filter(pylot.utils.is_waypoints_stream).add_callback(
GroundAgentOperator.on_waypoints_update)
input_streams.add_completion_callback(
GroundAgentOperator.on_notification)
# Set no watermark on the output stream so that we do not
# close the watermark loop with the carla operator.
return [pylot.utils.create_control_stream()]
def on_notification(self, msg):
# Get hero vehicle info.
can_bus_msg = self._can_bus_msgs.popleft()
vehicle_transform = can_bus_msg.data.transform
vehicle_speed = can_bus_msg.data.forward_speed
# Get waypoints.
waypoint_msg = self._waypoint_msgs.popleft()
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
# Get ground pedestrian info.
pedestrians_msg = self._pedestrian_msgs.popleft()
pedestrians = pedestrians_msg.pedestrians
# Get ground vehicle info.
vehicles_msg = self._vehicle_msgs.popleft()
vehicles = vehicles_msg.vehicles
# Get ground traffic lights info.
traffic_lights_msg = self._traffic_light_msgs.popleft()
traffic_lights = traffic_lights_msg.traffic_lights
# Get ground traffic signs info.
speed_limit_signs_msg = self._speed_limit_sign_msgs.popleft()
speed_limit_signs = speed_limit_signs_msg.speed_signs
# TODO(ionel): Use traffic signs info as well.
speed_factor, state = self.stop_for_agents(vehicle_transform.location,
wp_angle, wp_vector,
vehicles, pedestrians,
traffic_lights)
control_msg = self.get_control_message(wp_angle, wp_angle_speed,
speed_factor, vehicle_speed,
msg.timestamp)
self.get_output_stream('control_stream').send(control_msg)
def on_waypoints_update(self, msg):
with self._lock:
self._waypoint_msgs.append(msg)
def on_can_bus_update(self, msg):
with self._lock:
self._can_bus_msgs.append(msg)
def on_pedestrians_update(self, msg):
with self._lock:
self._pedestrian_msgs.append(msg)
def on_vehicles_update(self, msg):
with self._lock:
self._vehicle_msgs.append(msg)
def on_traffic_lights_update(self, msg):
with self._lock:
self._traffic_light_msgs.append(msg)
def on_speed_limit_signs_update(self, msg):
with self._lock:
self._speed_limit_sign_msgs.append(msg)
def stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
vehicles, pedestrians, traffic_lights):
speed_factor = 1
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
if self._flags.stop_for_vehicles:
for obs_vehicle in vehicles:
# Only brake for vehicles that are in ego vehicle's lane.
if self._map.are_on_same_lane(ego_vehicle_location,
obs_vehicle.transform.location):
new_speed_factor_v = pylot.control.utils.stop_vehicle(
ego_vehicle_location, obs_vehicle.transform.location,
wp_vector, speed_factor_v, self._flags)
speed_factor_v = min(speed_factor_v, new_speed_factor_v)
if self._flags.stop_for_pedestrians:
for pedestrian in pedestrians:
# Only brake for pedestrians that are on the road.
if self._map.is_on_lane(pedestrian.transform.location):
new_speed_factor_p = pylot.control.utils.stop_pedestrian(
ego_vehicle_location, pedestrian.transform.location,
wp_vector, speed_factor_p, self._flags)
speed_factor_p = min(speed_factor_p, new_speed_factor_p)
if self._flags.stop_for_traffic_lights:
for tl in traffic_lights:
if (self._map.must_obbey_traffic_light(ego_vehicle_location,
tl.transform.location)
and self._is_traffic_light_visible(
ego_vehicle_location, tl.transform.location)):
new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
ego_vehicle_location, tl.transform.location, tl.state,
wp_vector, wp_angle, speed_factor_tl, self._flags)
speed_factor_tl = min(speed_factor_tl, new_speed_factor_tl)
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
state = {
'stop_pedestrian': speed_factor_p,
'stop_vehicle': speed_factor_v,
'stop_traffic_lights': speed_factor_tl
}
return speed_factor, state
def execute(self):
self.spin()
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self._flags.steer_gain * wp_angle
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self._flags.target_speed * speed_factor / 2
else:
target_speed_adjusted = self._flags.target_speed * speed_factor
self._pid.target = target_speed_adjusted
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return ControlMessage(steer, throttle, brake, False, False, timestamp)
def _is_traffic_light_visible(self, ego_vehicle_location, tl_location):
_, tl_dist = pylot.control.utils.get_world_vec_dist(
ego_vehicle_location.x, ego_vehicle_location.y, tl_location.x,
tl_location.y)
return tl_dist > self._flags.traffic_light_min_dist_thres
class PylotAgentOperator(erdos.Operator):
def __init__(self,
can_bus_stream,
waypoints_stream,
traffic_lights_stream,
obstacles_stream,
point_cloud_stream,
open_drive_stream,
control_stream,
name,
flags,
camera_setup,
log_file_name=None,
csv_file_name=None):
can_bus_stream.add_callback(self.on_can_bus_update)
waypoints_stream.add_callback(self.on_waypoints_update)
traffic_lights_stream.add_callback(self.on_traffic_lights_update)
obstacles_stream.add_callback(self.on_obstacles_update)
point_cloud_stream.add_callback(self.on_point_cloud_update)
open_drive_stream.add_callback(self.on_open_drive_map)
erdos.add_watermark_callback([
can_bus_stream, waypoints_stream, traffic_lights_stream,
obstacles_stream, point_cloud_stream
], [control_stream], self.on_watermark)
self._name = name
self._flags = flags
self._camera_setup = camera_setup
self._log_file_name = log_file_name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
if not hasattr(self._flags, 'track'):
# The agent is not used in the Carla challenge. It has access to
# the simulator, and to the town map.
self._map = HDMap(
get_map(self._flags.carla_host, self._flags.carla_port,
self._flags.carla_timeout), log_file_name)
self._logger.debug('Agent running using map')
else:
self._map = None
self._pid = PID(p=self._flags.pid_p,
i=self._flags.pid_i,
d=self._flags.pid_d)
# Queues in which received messages are stored.
self._waypoint_msgs = deque()
self._can_bus_msgs = deque()
self._traffic_lights_msgs = deque()
self._obstacles_msgs = deque()
self._point_clouds = deque()
self._vehicle_labels = {'car', 'bicycle', 'motorcycle', 'bus', 'truck'}
@staticmethod
def connect(can_bus_stream, waypoints_stream, traffic_lights_stream,
obstacles_stream, point_cloud_stream, open_drive_stream):
control_stream = erdos.WriteStream()
return [control_stream]
def on_watermark(self, timestamp, control_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
start_time = time.time()
can_bus_msg = self._can_bus_msgs.popleft()
waypoint_msg = self._waypoint_msgs.popleft()
tl_msg = self._traffic_lights_msgs.popleft()
obstacles_msg = self._obstacles_msgs.popleft()
point_cloud_msg = self._point_clouds.popleft()
vehicle_transform = can_bus_msg.data.transform
# Vehicle sped in m/s
vehicle_speed = can_bus_msg.data.forward_speed
wp_angle = waypoint_msg.wp_angle
wp_vector = waypoint_msg.wp_vector
wp_angle_speed = waypoint_msg.wp_angle_speed
target_speed = waypoint_msg.target_speed
transformed_camera_setup = copy.deepcopy(self._camera_setup)
transformed_camera_setup.set_transform(
vehicle_transform * transformed_camera_setup.transform)
traffic_lights = self.__transform_tl_output(
tl_msg, point_cloud_msg.point_cloud, transformed_camera_setup)
(people, vehicles) = self.__transform_detector_output(
obstacles_msg, point_cloud_msg.point_cloud,
transformed_camera_setup)
self._logger.debug('@{}: speed {} and location {}'.format(
timestamp, vehicle_speed, vehicle_transform))
self._logger.debug('@{}: people {}'.format(timestamp, people))
self._logger.debug('@{}: vehicles {}'.format(timestamp, vehicles))
speed_factor, _ = self.__stop_for_agents(vehicle_transform.location,
wp_angle, wp_vector, vehicles,
people, traffic_lights,
timestamp)
control_msg = self.get_control_message(wp_angle, wp_angle_speed,
speed_factor, vehicle_speed,
target_speed, timestamp)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, timestamp,
runtime))
control_stream.send(control_msg)
def on_waypoints_update(self, msg):
self._logger.debug('@{}: waypoints update'.format(msg.timestamp))
self._waypoint_msgs.append(msg)
def on_can_bus_update(self, msg):
self._logger.debug('@{}: can bus update'.format(msg.timestamp))
self._can_bus_msgs.append(msg)
def on_traffic_lights_update(self, msg):
self._logger.debug('@{}: traffic lights update'.format(msg.timestamp))
self._traffic_lights_msgs.append(msg)
def on_obstacles_update(self, msg):
self._logger.debug('@{}: obstacles update'.format(msg.timestamp))
self._obstacles_msgs.append(msg)
def on_point_cloud_update(self, msg):
self._logger.debug('@{}: point cloud update'.format(msg.timestamp))
self._point_clouds.append(msg)
def on_open_drive_map(self, msg):
self._logger.debug('@{}: open drive update'.format(msg.timestamp))
try:
import carla
except ImportError:
raise Exception('Error importing carla.')
self._map = HDMap(carla.Map('challenge', msg.data),
self._log_file_name)
def __transform_tl_output(self, tls, point_cloud, camera_setup):
""" Transforms traffic light bounding boxes to world coordinates.
Args:
tls: A list of traffic light detected obstacles.
point_cloud: The Lidar point cloud. Must be taken captured at the
same time as the frame on which the traffic lights
were detected.
Returns:
A list of traffic light locations.
"""
traffic_lights = []
for tl in tls.obstacles:
location = point_cloud.get_pixel_location(
tl.bounding_box.get_center_point(), camera_setup)
if location is not None:
traffic_lights.append((location, tl.label))
else:
self._logger.error(
'Could not find location for traffic light {}'.format(tl))
return traffic_lights
def __transform_detector_output(self, obstacles_msg, point_cloud,
camera_setup):
""" Transforms detected obstacles to world coordinates.
Args:
obstacles_msg: A list of detected obstacles.
point_cloud: The Lidar point cloud. Must be taken captured at the
same time as the frame on which the obstacles were
detected.
Returns:
A list of 3D world locations.
"""
vehicles = []
people = []
for obstacle in obstacles_msg.obstacles:
if obstacle.label == 'person':
location = point_cloud.get_pixel_location(
obstacle.bounding_box.get_center_point(), camera_setup)
if location is not None:
people.append(location)
else:
self._logger.error(
'Could not find location for person {}'.format(
obstacle))
elif (obstacle.label in self._vehicle_labels):
location = point_cloud.get_pixel_location(
obstacle.bounding_box.get_center_point(), camera_setup)
if location is not None:
vehicles.append(location)
else:
self._logger.error(
'Could not find location for vehicle {}'.format(
obstacle))
return (people, vehicles)
def __stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
vehicles, people, traffic_lights, timestamp):
speed_factor = 1
speed_factor_tl = 1
speed_factor_p = 1
speed_factor_v = 1
for obs_vehicle_loc in vehicles:
if (not self._map or self._map.are_on_same_lane(
ego_vehicle_location, obs_vehicle_loc)):
self._logger.debug(
'@{}: ego {} and vehicle {} are on the same lane'.format(
timestamp, ego_vehicle_location, obs_vehicle_loc))
new_speed_factor_v = pylot.control.utils.stop_vehicle(
ego_vehicle_location, obs_vehicle_loc, wp_vector,
speed_factor_v, self._flags)
if new_speed_factor_v < speed_factor_v:
speed_factor_v = new_speed_factor_v
self._logger.debug(
'@{}: vehicle {} reduced speed factor to {}'.format(
timestamp, obs_vehicle_loc, speed_factor_v))
for obs_ped_loc in people:
if (not self._map or self._map.are_on_same_lane(
ego_vehicle_location, obs_ped_loc)):
self._logger.debug(
'@{}: ego {} and person {} are on the same lane'.format(
timestamp, ego_vehicle_location, obs_ped_loc))
new_speed_factor_p = pylot.control.utils.stop_person(
ego_vehicle_location, obs_ped_loc, wp_vector,
speed_factor_p, self._flags)
if new_speed_factor_p < speed_factor_p:
speed_factor_p = new_speed_factor_p
self._logger.debug(
'@{}: person {} reduced speed factor to {}'.format(
timestamp, obs_ped_loc, speed_factor_p))
for tl in traffic_lights:
if (not self._map or self._map.must_obbey_traffic_light(
ego_vehicle_location, tl[0])):
self._logger.debug(
'@{}: ego is obbeying traffic light {}'.format(
timestamp, ego_vehicle_location, tl[0]))
tl_state = tl[1]
new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
ego_vehicle_location, tl[0], tl_state, wp_vector, wp_angle,
speed_factor_tl, self._flags)
if new_speed_factor_tl < speed_factor_tl:
speed_factor_tl = new_speed_factor_tl
self._logger.debug(
'@{}: traffic light {} reduced speed factor to {}'.
format(timestamp, tl[0], speed_factor_tl))
speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
state = {
'stop_person': speed_factor_p,
'stop_vehicle': speed_factor_v,
'stop_traffic_lights': speed_factor_tl
}
self._logger.debug('@{}: agent speed factors {}'.format(
timestamp, state))
return speed_factor, state
def __get_steer(self, wp_angle):
steer = self._flags.steer_gain * wp_angle
if steer > 0:
steer = min(steer, 1)
else:
steer = max(steer, -1)
return steer
def __get_throttle_brake_without_factor(self, current_speed, target_speed):
self._pid.target = target_speed
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return throttle, brake
def __get_throttle_brake(self, current_speed, target_speed, wp_angle_speed,
speed_factor):
# TODO(ionel): DO NOT HARDCODE VALUES!
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = target_speed * speed_factor
elif math.fabs(wp_angle_speed) < 0.5:
target_speed_adjusted = 6 * speed_factor
else:
target_speed_adjusted = 3 * speed_factor
self._pid.target = target_speed_adjusted
pid_gain = self._pid(feedback=current_speed)
throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
self._flags.throttle_max)
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
else:
brake = 0
return throttle, brake
def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, target_speed, timestamp):
current_speed = max(current_speed, 0)
steer = self.__get_steer(wp_angle)
throttle, brake = self.__get_throttle_brake(current_speed,
target_speed,
wp_angle_speed,
speed_factor)
return ControlMessage(steer, throttle, brake, False, False, timestamp)
class RRTStarPlanningOperator(erdos.Operator):
""" RRTStar Planning operator for Carla 0.9.x."""
def __init__(self,
can_bus_stream,
prediction_stream,
waypoints_stream,
name,
flags,
goal_location,
log_file_name=None,
csv_file_name=None):
"""
Initialize the RRT* planner. Setup logger and map attributes.
Args:
name: Name of the operator.
flags: Config flags.
goal_location: Global goal carla.Location for planner to route to.
"""
can_bus_stream.add_callback(self.on_can_bus_update)
prediction_stream.add_callback(self.on_prediction_update)
erdos.add_watermark_callback([can_bus_stream, prediction_stream],
[waypoints_stream], self.on_watermark)
self._name = name
self._logger = erdos.utils.setup_logging(name, log_file_name)
self._csv_logger = erdos.utils.setup_csv_logging(
name + '-csv', csv_file_name)
self._flags = flags
self._wp_index = 9
self._waypoints = None
self._carla_map = get_map(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
self._hd_map = HDMap(self._carla_map, log_file_name)
self._goal_location = goal_location
self._can_bus_msgs = deque()
self._prediction_msgs = deque()
@staticmethod
def connect(can_bus_stream, prediction_stream):
waypoints_stream = erdos.WriteStream()
return [waypoints_stream]
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_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
# get obstacles
obstacle_map = self._build_obstacle_map(vehicle_transform)
# compute goals
target_location = self._compute_target_location(vehicle_transform)
# run rrt*
path, cost = self._run_rrt_star(vehicle_transform, target_location,
obstacle_map)
# convert to waypoints if path found, else use default waypoints
if cost is not None:
path_transforms = []
for point in path:
p_loc = self._carla_map.get_waypoint(
carla.Location(x=point[0], y=point[1], z=0),
project_to_road=True,
).transform.location # project to road to approximate Z
path_transforms.append(
Transform(
location=Location(x=point[0], y=point[1], z=p_loc.z),
rotation=Rotation(),
))
waypoints = deque(path_transforms)
waypoints.extend(
itertools.islice(self._waypoints, self._wp_index,
len(self._waypoints))
) # add the remaining global route for future
else:
waypoints = self._waypoints
# construct waypoints message
waypoints = collections.deque(
itertools.islice(waypoints, 0,
DEFAULT_NUM_WAYPOINTS)) # only take 50 meters
next_waypoint = waypoints[self._wp_index]
wp_steer_speed_vector, wp_steer_speed_angle = \
get_waypoint_vector_and_angle(
next_waypoint, vehicle_transform
)
output_msg = WaypointsMessage(timestamp,
waypoints=waypoints,
wp_angle=wp_steer_speed_angle,
wp_vector=wp_steer_speed_vector,
wp_angle_speed=wp_steer_speed_angle)
# send waypoints message
waypoints_stream.send(output_msg)
waypoints_stream.send(erdos.WatermarkMessage(timestamp))
def _build_obstacle_map(self, vehicle_transform):
"""
Construct an obstacle map given vehicle_transform.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
Returns:
an obstacle map that maps
{id_time: (obstacle_origin, obstacle_range)}
only obstacles within DEFAULT_DISTANCE_THRESHOLD in front of the
ego vehicle are considered to save computation cost.
"""
obstacle_map = {}
prediction_msg = self._prediction_msgs.popleft()
# look over all predictions
for prediction in prediction_msg.predictions:
time = 0
# use all prediction times as potential obstacles
for transform in prediction.trajectory:
if vehicle_transform.location.is_within_distance_ahead(
transform.location, DEFAULT_DISTANCE_THRESHOLD):
# compute the obstacle origin and range of the obstacle
obstacle_origin = ((transform.location.x -
DEFAULT_OBSTACLE_LENGTH / 2,
transform.location.y -
DEFAULT_OBSTACLE_WIDTH / 2),
(DEFAULT_OBSTACLE_LENGTH,
DEFAULT_OBSTACLE_WIDTH))
obs_id = str("{}_{}".format(prediction.id, time))
obstacle_map[obs_id] = obstacle_origin
time += 1
return obstacle_map
def _compute_target_location(self, vehicle_transform):
"""
Update the global waypoint route and compute the target location for
RRT* search to plan for.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
Returns:
target location
"""
ego_location = vehicle_transform.location.as_carla_location()
self._waypoints = self._hd_map.compute_waypoints(
ego_location, self._goal_location)
target_waypoint = self._waypoints[self._wp_index]
target_location = target_waypoint.location
return target_location
@staticmethod
def _run_rrt_star(vehicle_transform, target_location, obstacle_map):
"""
Run the RRT* algorithm given the vehicle_transform, target_location,
and obstacle_map.
Args:
vehicle_transform: pylot.utils.Transform of vehicle from can_bus
stream
target_location: Location target
obstacle_map: an obstacle map that maps
{id_time: (obstacle_origin, obstacle_range)}
Returns:
np.ndarray, float
return the path in form [[x0, y0],...] and final cost
if solution not found, returns the path to the closest point to the
target space and final cost is none
"""
starting_state = (vehicle_transform.location.x,
vehicle_transform.location.y)
target_space = ((target_location.x - DEFAULT_TARGET_LENGTH / 2,
target_location.y - DEFAULT_TARGET_WIDTH / 2),
(DEFAULT_TARGET_LENGTH, DEFAULT_TARGET_WIDTH))
state_space = start_target_to_space(starting_state, target_space,
DEFAULT_TARGET_LENGTH,
DEFAULT_TARGET_WIDTH)
path, cost = apply_rrt_star(state_space=state_space,
starting_state=starting_state,
target_space=target_space,
obstacle_map=obstacle_map)
return path, cost
