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)
Exemple #2
0
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
Exemple #3
0
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)
Exemple #4
0
class MPCAgentOperator(erdos.Operator):
    def __init__(self,
                 can_bus_stream,
                 ground_obstacles_stream,
                 ground_traffic_lights_stream,
                 waypoints_stream,
                 control_stream,
                 name,
                 flags,
                 log_file_name=None,
                 csv_file_name=None):
        can_bus_stream.add_callback(self.on_can_bus_update)
        ground_obstacles_stream.add_callback(self.on_obstacles_update)
        ground_traffic_lights_stream.add_callback(
            self.on_traffic_lights_update)
        waypoints_stream.add_callback(self.on_waypoints_update)
        erdos.add_watermark_callback([
            can_bus_stream, ground_obstacles_stream,
            ground_traffic_lights_stream, waypoints_stream
        ], [control_stream], self.on_watermark)
        self._name = name
        self._logger = erdos.utils.setup_logging(name, log_file_name)
        self._csv_logger = erdos.utils.setup_csv_logging(
            name + '-csv', csv_file_name)
        self._flags = flags
        self._config = global_config
        self._map = HDMap(
            get_map(self._flags.carla_host, self._flags.carla_port,
                    self._flags.carla_timeout), log_file_name)
        _, self._world = get_world(self._flags.carla_host,
                                   self._flags.carla_port,
                                   self._flags.carla_timeout)
        self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
        self._can_bus_msgs = deque()
        self._obstacles_msgs = deque()
        self._traffic_light_msgs = deque()
        self._waypoint_msgs = deque()

    @staticmethod
    def connect(can_bus_stream, ground_obstacles_stream,
                ground_traffic_lights_stream, waypoints_stream):
        control_stream = erdos.WriteStream()
        return [control_stream]

    def on_waypoints_update(self, msg):
        self._logger.debug('@{}: waypoints update'.format(msg.timestamp))
        self._waypoint_msgs.append(msg)

    def on_can_bus_update(self, msg):
        self._logger.debug('@{}: can bus update'.format(msg.timestamp))
        self._can_bus_msgs.append(msg)

    def on_obstacles_update(self, msg):
        self._logger.debug('@{}: obstacles update'.format(msg.timestamp))
        self._obstacles_msgs.append(msg)

    def on_traffic_lights_update(self, msg):
        self._logger.debug('@{}: traffic lights update'.format(msg.timestamp))
        self._traffic_light_msgs.append(msg)

    def on_watermark(self, timestamp, control_stream):
        self._logger.debug('Received watermark {}'.format(timestamp))
        # Get hero vehicle info.
        can_bus_msg = self._can_bus_msgs.popleft()
        vehicle_transform = can_bus_msg.data.transform
        vehicle_speed = can_bus_msg.data.forward_speed

        # Get waypoints.
        waypoint_msg = self._waypoint_msgs.popleft()
        wp_angle = waypoint_msg.wp_angle
        wp_vector = waypoint_msg.wp_vector
        wp_angle_speed = waypoint_msg.wp_angle_speed

        waypoints = deque(itertools.islice(waypoint_msg.waypoints, 0,
                                           50))  # only take 50 meters

        # Get ground obstacles info.
        obstacles = self._obstacles_msgs.popleft().obstacles

        # Get ground traffic lights info.
        traffic_lights = self._traffic_light_msgs.popleft().traffic_lights

        speed_factor, state = self.stop_for_agents(vehicle_transform.location,
                                                   wp_angle, wp_vector,
                                                   wp_angle_speed, obstacles,
                                                   traffic_lights)

        control_msg = self.get_control_message(waypoints, vehicle_transform,
                                               vehicle_speed, speed_factor,
                                               timestamp)
        self._logger.debug("Throttle: {}".format(control_msg.throttle))
        self._logger.debug("Steer: {}".format(control_msg.steer))
        self._logger.debug("Brake: {}".format(control_msg.brake))
        self._logger.debug("State: {}".format(state))

        control_stream.send(control_msg)

    def stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
                        wp_angle_speed, obstacles, traffic_lights):
        speed_factor = 1
        speed_factor_tl = 1
        speed_factor_p = 1
        speed_factor_v = 1

        for obstacle in obstacles:
            if obstacle.label == 'vehicle' and self._flags.stop_for_vehicles:
                # Only brake for vehicles that are in ego vehicle's lane.
                if self._map.are_on_same_lane(ego_vehicle_location,
                                              obstacle.transform.location):
                    new_speed_factor_v = pylot.control.utils.stop_vehicle(
                        ego_vehicle_location, obstacle.transform.location,
                        wp_vector, speed_factor_v, self._flags)
                    speed_factor_v = min(speed_factor_v, new_speed_factor_v)
            if obstacle.label == 'person' and \
               self._flags.stop_for_people:
                # Only brake for people that are on the road.
                if self._map.is_on_lane(obstacle.transform.location):
                    new_speed_factor_p = pylot.control.utils.stop_person(
                        ego_vehicle_location, obstacle.transform.location,
                        wp_vector, speed_factor_p, self._flags)
                    speed_factor_p = min(speed_factor_p, new_speed_factor_p)

        if self._flags.stop_for_traffic_lights:
            for tl in traffic_lights:
                if (self._map.must_obbey_traffic_light(ego_vehicle_location,
                                                       tl.transform.location)
                        and self._is_traffic_light_visible(
                            ego_vehicle_location, tl.transform.location)):
                    new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
                        ego_vehicle_location, tl.transform.location, tl.state,
                        wp_vector, wp_angle, speed_factor_tl, self._flags)
                    speed_factor_tl = min(speed_factor_tl, new_speed_factor_tl)

        speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)

        # slow down around corners
        if math.fabs(wp_angle_speed) < 0.1:
            speed_factor = 0.3 * speed_factor

        state = {
            'stop_person': speed_factor_p,
            'stop_vehicle': speed_factor_v,
            'stop_traffic_lights': speed_factor_tl
        }

        return speed_factor, state

    def setup_mpc(self, waypoints):
        path = np.array([[wp.location.x, wp.location.y] for wp in waypoints])

        # convert target waypoints into spline
        spline = CubicSpline2D(path[:, 0], path[:, 1], self._logger)
        ss = []
        vels = []
        xs = []
        ys = []
        yaws = []
        ks = []
        for s in spline.s[:-2]:
            x, y = spline.calc_position(s)
            yaw = np.abs(spline.calc_yaw(s))
            k = spline.calc_curvature(s)
            xs.append(x)
            ys.append(y)
            yaws.append(yaw)
            ks.append(k)
            ss.append(s)
            vels.append(18)

        self._config["reference"] = {
            't_list': [],  # Time [s]
            's_list': ss,  # Arc distance [m]
            'x_list': xs,  # Desired X coordinates [m]
            'y_list': ys,  # Desired Y coordinates [m]
            'k_list': ks,  # Curvatures [1/m]
            'vel_list': vels,  # Desired tangential velocities [m/s]
            'yaw_list': yaws,  # Yaws [rad]
        }

        # initialize mpc controller
        self.mpc = ModelPredictiveController(config=self._config)

    def get_control_message(self, waypoints, vehicle_transform, current_speed,
                            speed_factor, timestamp):
        ego_location = vehicle_transform.location.as_carla_location()
        ego_yaw = np.deg2rad(zero_to_2_pi(vehicle_transform.rotation.yaw))

        # step the controller
        self.setup_mpc(waypoints)
        self.mpc.vehicle.x = ego_location.x
        self.mpc.vehicle.y = ego_location.y
        self.mpc.vehicle.yaw = ego_yaw

        try:
            self.mpc.step()
        except Exception as e:
            self._logger.info("Failed to solve MPC.")
            self._logger.info(e)
            return ControlMessage(0, 0, 1, False, False, timestamp)

        # compute pid controls
        target_speed = self.mpc.solution.vel_list[0] * speed_factor
        target_yaw = self.mpc.solution.yaw_list[0]
        target_steer_rad = self.mpc.horizon_steer[0]  # in rad
        steer = self.__rad2steer(target_steer_rad)  # [-1.0, 1.0]
        throttle, brake = self.__get_throttle_brake_without_factor(
            current_speed, target_speed)

        # send controls
        return ControlMessage(steer, throttle, brake, False, False, timestamp)

    def _is_traffic_light_visible(self, ego_vehicle_location, tl_location):
        _, tl_dist = pylot.control.utils.get_world_vec_dist(
            ego_vehicle_location.x, ego_vehicle_location.y, tl_location.x,
            tl_location.y)
        return tl_dist > self._flags.traffic_light_min_dist_thres

    def __rad2steer(self, rad):
        """
        Converts radians to steer input.

        :return: float [-1.0, 1.0]
        """
        steer = self._flags.steer_gain * rad
        if steer > 0:
            steer = min(steer, 1)
        else:
            steer = max(steer, -1)
        return steer

    def __steer2rad(self, steer):
        """
        Converts radians to steer input. Assumes max steering angle is -45, 45 degrees

        :return: float [-1.0, 1.0]
        """
        rad = steer / self._flags.steer_gain
        if rad > 0:
            rad = min(rad, np.pi / 2)
        else:
            rad = max(rad, -np.pi / 2)
        return rad

    def __get_throttle_brake_without_factor(self, current_speed, target_speed):
        self._pid.target = target_speed
        pid_gain = self._pid(feedback=current_speed)
        throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
                       self._flags.throttle_max)
        if pid_gain > 0.5:
            brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
        else:
            brake = 0
        return throttle, brake
class GroundAgentOperator(Op):
    def __init__(self, name, flags, log_file_name=None, csv_file_name=None):
        super(GroundAgentOperator, self).__init__(name)
        self._logger = setup_logging(self.name, log_file_name)
        self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
        self._flags = flags
        self._map = HDMap(
            get_map(self._flags.carla_host, self._flags.carla_port,
                    self._flags.carla_timeout), log_file_name)
        self._pid = PID(p=flags.pid_p, i=flags.pid_i, d=flags.pid_d)
        self._can_bus_msgs = deque()
        self._pedestrian_msgs = deque()
        self._vehicle_msgs = deque()
        self._traffic_light_msgs = deque()
        self._speed_limit_sign_msgs = deque()
        self._waypoint_msgs = deque()
        self._lock = threading.Lock()

    @staticmethod
    def setup_streams(input_streams):
        input_streams.filter(pylot.utils.is_can_bus_stream).add_callback(
            GroundAgentOperator.on_can_bus_update)
        input_streams.filter(
            pylot.utils.is_ground_pedestrians_stream).add_callback(
                GroundAgentOperator.on_pedestrians_update)
        input_streams.filter(
            pylot.utils.is_ground_vehicles_stream).add_callback(
                GroundAgentOperator.on_vehicles_update)
        input_streams.filter(
            pylot.utils.is_ground_traffic_lights_stream).add_callback(
                GroundAgentOperator.on_traffic_lights_update)
        input_streams.filter(
            pylot.utils.is_ground_speed_limit_signs_stream).add_callback(
                GroundAgentOperator.on_speed_limit_signs_update)
        input_streams.filter(pylot.utils.is_waypoints_stream).add_callback(
            GroundAgentOperator.on_waypoints_update)
        input_streams.add_completion_callback(
            GroundAgentOperator.on_notification)
        # Set no watermark on the output stream so that we do not
        # close the watermark loop with the carla operator.
        return [pylot.utils.create_control_stream()]

    def on_notification(self, msg):
        # Get hero vehicle info.
        can_bus_msg = self._can_bus_msgs.popleft()
        vehicle_transform = can_bus_msg.data.transform
        vehicle_speed = can_bus_msg.data.forward_speed
        # Get waypoints.
        waypoint_msg = self._waypoint_msgs.popleft()
        wp_angle = waypoint_msg.wp_angle
        wp_vector = waypoint_msg.wp_vector
        wp_angle_speed = waypoint_msg.wp_angle_speed
        # Get ground pedestrian info.
        pedestrians_msg = self._pedestrian_msgs.popleft()
        pedestrians = pedestrians_msg.pedestrians
        # Get ground vehicle info.
        vehicles_msg = self._vehicle_msgs.popleft()
        vehicles = vehicles_msg.vehicles
        # Get ground traffic lights info.
        traffic_lights_msg = self._traffic_light_msgs.popleft()
        traffic_lights = traffic_lights_msg.traffic_lights
        # Get ground traffic signs info.
        speed_limit_signs_msg = self._speed_limit_sign_msgs.popleft()
        speed_limit_signs = speed_limit_signs_msg.speed_signs
        # TODO(ionel): Use traffic signs info as well.

        speed_factor, state = self.stop_for_agents(vehicle_transform.location,
                                                   wp_angle, wp_vector,
                                                   vehicles, pedestrians,
                                                   traffic_lights)
        control_msg = self.get_control_message(wp_angle, wp_angle_speed,
                                               speed_factor, vehicle_speed,
                                               msg.timestamp)
        self.get_output_stream('control_stream').send(control_msg)

    def on_waypoints_update(self, msg):
        with self._lock:
            self._waypoint_msgs.append(msg)

    def on_can_bus_update(self, msg):
        with self._lock:
            self._can_bus_msgs.append(msg)

    def on_pedestrians_update(self, msg):
        with self._lock:
            self._pedestrian_msgs.append(msg)

    def on_vehicles_update(self, msg):
        with self._lock:
            self._vehicle_msgs.append(msg)

    def on_traffic_lights_update(self, msg):
        with self._lock:
            self._traffic_light_msgs.append(msg)

    def on_speed_limit_signs_update(self, msg):
        with self._lock:
            self._speed_limit_sign_msgs.append(msg)

    def stop_for_agents(self, ego_vehicle_location, wp_angle, wp_vector,
                        vehicles, pedestrians, traffic_lights):
        speed_factor = 1
        speed_factor_tl = 1
        speed_factor_p = 1
        speed_factor_v = 1

        if self._flags.stop_for_vehicles:
            for obs_vehicle in vehicles:
                # Only brake for vehicles that are in ego vehicle's lane.
                if self._map.are_on_same_lane(ego_vehicle_location,
                                              obs_vehicle.transform.location):
                    new_speed_factor_v = pylot.control.utils.stop_vehicle(
                        ego_vehicle_location, obs_vehicle.transform.location,
                        wp_vector, speed_factor_v, self._flags)
                    speed_factor_v = min(speed_factor_v, new_speed_factor_v)

        if self._flags.stop_for_pedestrians:
            for pedestrian in pedestrians:
                # Only brake for pedestrians that are on the road.
                if self._map.is_on_lane(pedestrian.transform.location):
                    new_speed_factor_p = pylot.control.utils.stop_pedestrian(
                        ego_vehicle_location, pedestrian.transform.location,
                        wp_vector, speed_factor_p, self._flags)
                    speed_factor_p = min(speed_factor_p, new_speed_factor_p)

        if self._flags.stop_for_traffic_lights:
            for tl in traffic_lights:
                if (self._map.must_obbey_traffic_light(ego_vehicle_location,
                                                       tl.transform.location)
                        and self._is_traffic_light_visible(
                            ego_vehicle_location, tl.transform.location)):
                    new_speed_factor_tl = pylot.control.utils.stop_traffic_light(
                        ego_vehicle_location, tl.transform.location, tl.state,
                        wp_vector, wp_angle, speed_factor_tl, self._flags)
                    speed_factor_tl = min(speed_factor_tl, new_speed_factor_tl)

        speed_factor = min(speed_factor_tl, speed_factor_p, speed_factor_v)
        state = {
            'stop_pedestrian': speed_factor_p,
            'stop_vehicle': speed_factor_v,
            'stop_traffic_lights': speed_factor_tl
        }

        return speed_factor, state

    def execute(self):
        self.spin()

    def get_control_message(self, wp_angle, wp_angle_speed, speed_factor,
                            current_speed, timestamp):
        current_speed = max(current_speed, 0)
        steer = self._flags.steer_gain * wp_angle
        if steer > 0:
            steer = min(steer, 1)
        else:
            steer = max(steer, -1)

        # Don't go to fast around corners
        if math.fabs(wp_angle_speed) < 0.1:
            target_speed_adjusted = self._flags.target_speed * speed_factor / 2
        else:
            target_speed_adjusted = self._flags.target_speed * speed_factor

        self._pid.target = target_speed_adjusted
        pid_gain = self._pid(feedback=current_speed)
        throttle = min(max(self._flags.default_throttle - 1.3 * pid_gain, 0),
                       self._flags.throttle_max)

        if pid_gain > 0.5:
            brake = min(0.35 * pid_gain * self._flags.brake_strength, 1)
        else:
            brake = 0

        return ControlMessage(steer, throttle, brake, False, False, timestamp)

    def _is_traffic_light_visible(self, ego_vehicle_location, tl_location):
        _, tl_dist = pylot.control.utils.get_world_vec_dist(
            ego_vehicle_location.x, ego_vehicle_location.y, tl_location.x,
            tl_location.y)
        return tl_dist > self._flags.traffic_light_min_dist_thres