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