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 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 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 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 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
