def test_distance(point_a, point_b, expected): """ Test the distance computed between two points is the same as expected""" location_a, location_b = Location(*point_a), Location(*point_b) assert np.isclose(location_a.distance(location_b), expected), "Distance " "between point_a and point_b is not the same as the expected distance." assert np.isclose(location_b.distance(location_a), expected), "Distance " "between point_b and point_a is not the same as the expected distance."
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 test_l1_distance(point_a, point_b, expected): """ Test the L1 distance computed between the two points. """ location_a, location_b = Location(*point_a), Location(*point_b) assert np.isclose(location_a.l1_distance(location_b), expected), "L1 " "Distance between point_a and point_b is not the same as expected." assert np.isclose(location_b.l1_distance(location_a), expected), "L1 " "Distance between point_a and point_b is not the same as expected."
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)
def test_negative_location_from_carla(): """ Test that Location throws a ValueError if incorrect carla_loc argument is passed. """ DummyType = namedtuple("DummyType", "x, y, z") dummy_instance = DummyType(10, 20, 30) with pytest.raises(ValueError): Location.from_carla_location(dummy_instance)
def test_as_numpy_array(): """ Test the as_carla_location instance method of Location """ location = Location(x=1, y=2, z=3) np_array = location.as_numpy_array() assert isinstance(np_array, np.ndarray), "Returned instance is " "not of the type np.ndarray" assert all(np.isclose(np_array, [1, 2, 3])), "Returned instance x, y, z " "values are not the same as the one in location."
def generate_waypoints(self, ego_transform, waypoint, road_option): # Keep on driving in the same lane/straight if the next waypoint is # far away. if (road_option == pylot.utils.RoadOption.STRAIGHT or road_option == pylot.utils.RoadOption.LANE_FOLLOW or ego_transform.location.distance( waypoint.location.distance) > 20): for lane in self.lanes: if lane.id == 0: return lane.get_lane_center_transforms() # Lane detector didn't find ego's lane. Keep on driving # in the same direction. output_wps = deque([]) for distance in range(1, 20): wp = (ego_transform * Transform(Location(x=distance), Rotation())) output_wps.append(wp) return output_wps elif road_option == pylot.utils.RoadOption.LEFT: output_wps = deque([]) wp = copy.deepcopy(ego_transform) for distance in range(1, 11): wp = wp * Transform(Location(x=1), Rotation(yaw=-9)) output_wps.append(wp) return output_wps elif road_option == pylot.utils.RoadOption.RIGHT: output_wps = deque([]) wp = copy.deepcopy(ego_transform) for distance in range(1, 11): wp = wp * Transform(Location(x=1), Rotation(yaw=9)) output_wps.append(wp) return output_wps elif road_option == pylot.utils.RoadOption.CHANGE_LANE_LEFT: for lane in self.lanes: if lane.id == -1: return lane.get_lane_center_transforms() # Lane detector didn't find left lane. output_wps = deque([]) wp = copy.deepcopy(ego_transform) for distance in range(1, 11): wp = wp * Transform(Location(x=1), Rotation(yaw=-4)) output_wps.append(wp) return output_wps elif road_option == pylot.utils.RoadOption.CHANGE_LANE_RIGHT: for lane in self.lanes: if lane.id == 1: return lane.get_lane_center_transforms() # Lane detector didn't find right lane. output_wps = deque([]) wp = copy.deepcopy(ego_transform) for distance in range(1, 11): wp = wp * Transform(Location(x=1), Rotation(yaw=4)) output_wps.append(wp) return output_wps self._logger.debug('Unexpected road option {}'.format(road_option)) return deque([ego_transform])
def test_as_carla_location(): """ Test the as_carla_location instance method of Location """ location = Location(x=1, y=2, z=3) carla_location = location.as_carla_location() assert isinstance(carla_location, carla.Location), "Returned instance is " "not of the type carla.Location" assert np.isclose(carla_location.x, location.x), "Returned instance x " "value is not the same as the one in location." assert np.isclose(carla_location.y, location.y), "Returned instance y " "value is not the same as the one in location." assert np.isclose(carla_location.z, location.z), "Returned instance z " "value is not the same as the one in location."
def test_addition(point_a, point_b, expected): """ Test the addition of the two locations. """ location_a, location_b = Location(*point_a), Location(*point_b) sum_location = location_a + location_b assert isinstance(sum_location, Location), "The sum was not of the type " "Location" assert np.isclose(expected[0], sum_location.x), "The x value of the sum " "was not the same as the expected value." assert np.isclose(expected[1], sum_location.y), "The y value of the sum " "was not the same as the expected value." assert np.isclose(expected[2], sum_location.z), "The z value of the sum " "was not the same as the expected value."
def test_vector_to_camera_view(location, expected): from pylot.drivers.sensor_setup import CameraSetup camera_setup = CameraSetup('test_camera', 'sensor.camera.rgb', width=1920, height=1080, transform=Transform(Location(), Rotation())) location = Location(*location) assert all( np.isclose( location.to_camera_view( camera_setup.get_extrinsic_matrix(), camera_setup.get_intrinsic_matrix()).as_numpy_array(), expected)), "The camera transformation was not as expected."
def get_pixel_location(self, pixel, camera_setup): """ Gets the 3D world location from pixel coordinates. Args: pixel: A pylot.utils.Vector2D denoting pixel coordinates. camera_setup: The setup of the camera. Returns: A pylot.utils.Location of the 3D world location, or None if all the point cloud points are behind. """ if len(self.points) == 0: return None intrinsic_mat = camera_setup.get_intrinsic_matrix() # Project our 2D pixel location into 3D space, onto the z=1 plane. p3d = np.dot(inv(intrinsic_mat), np.array([[pixel.x], [pixel.y], [1.0]])) location = self._get_closest_point_in_point_cloud( Vector2D(p3d[0], p3d[1])) # Normalize our point to have the same depth as our closest point. p3d *= np.array([location.z]) p3d_locations = [ Location(px, py, pz) for px, py, pz in np.asarray(p3d.transpose()) ] # Convert from camera to unreal coordinates. to_world_transform = camera_setup.get_unreal_transform() camera_point_cloud = to_world_transform.transform_points(p3d_locations) return camera_point_cloud[0]
def test_imu_setup_initialization(): # Set up the required parameters for the initialization. name = 'imu_sensor' transform = Transform(Location(), Rotation()) imu_setup = IMUSetup(name, transform) assert imu_setup.name == name, "The name in the setup is not the same." assert imu_setup.transform == transform, "The transform is not the same."
def from_carla_point_cloud(cls, carla_pc, transform): # Transform the raw_data into a point cloud. points = np.frombuffer(carla_pc.raw_data, dtype=np.dtype('f4')) points = copy.deepcopy(points) points = np.reshape(points, (int(points.shape[0] / 3), 3)) point_cloud = [Location(x, y, z) for x, y, z in np.asarray(points)] return cls(point_cloud, transform)
def build_output_waypoints(self, path_x, path_y, speeds): wps = deque() target_speeds = deque() 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) wps.append( Transform( location=Location(x=point[0], y=point[1], z=p_loc.z), rotation=Rotation(), )) target_speeds.append(point[2]) return Waypoints(wps, target_speeds)
def get_closest_point_in_point_cloud(fwd_points, pixel, normalized: bool = False): """Finds the closest point in the point cloud to the given point. Args: pixel (:py:class:`~pylot.utils.Vector2D`): Camera coordinates. Returns: :py:class:`~pylot.utils.Location`: Closest point cloud point. """ # Select x and y. pc_xy = fwd_points[:, 0:2] # Create an array from the x, y coordinates of the point. xy = np.array([pixel.x, pixel.y]).transpose() # Compute distance if normalized: # Select z pc_z = fwd_points[:, 2] # Divize x, y by z normalized_pc = pc_xy / pc_z[:, None] dist = np.sum((normalized_pc - xy)**2, axis=1) else: dist = np.sum((pc_xy - xy)**2, axis=1) # Select index of the closest point. closest_index = np.argmin(dist) # Return the closest point. return Location(fwd_points[closest_index][0], fwd_points[closest_index][1], fwd_points[closest_index][2])
def get_pixel_location(self, pixel, camera_setup): """ Gets the 3D world location from pixel coordinates. Args: pixel (:py:class:`~pylot.utils.Vector2D`): Pixel coordinates. camera_setup (:py:class:`~pylot.drivers.sensor_setup.CameraSetup`): The setup of the camera. Returns: :py:class:`~pylot.utils.Location`: The 3D world location, or None if all the point cloud points are behind. """ # Select only points that are in front. fwd_points = self.points[np.where(self.points[:, 2] > 0.0)] if len(fwd_points) == 0: return None intrinsic_mat = camera_setup.get_intrinsic_matrix() # Project our 2D pixel location into 3D space, onto the z=1 plane. p3d = np.dot(inv(intrinsic_mat), np.array([[pixel.x], [pixel.y], [1.0]])) location = PointCloud.get_closest_point_in_point_cloud( fwd_points, Vector2D(p3d[0], p3d[1])) # Normalize our point to have the same depth as our closest point. p3d *= np.array([location.z]) p3d = p3d.transpose() # Convert from camera to unreal coordinates. to_world_transform = camera_setup.get_unreal_transform() camera_point_cloud = to_world_transform.transform_points(p3d)[0] pixel_location = Location(camera_point_cloud[0], camera_point_cloud[1], camera_point_cloud[2]) return pixel_location
def run(self): # Read the vehicle ID from the vehicle ID stream. vehicle_id_msg = self._vehicle_id_stream.read() vehicle_id = vehicle_id_msg.data self._logger.debug("@{}: Received Vehicle ID: {}".format( vehicle_id_msg.timestamp, vehicle_id)) # Connect to the world. _, world = get_world(self._flags.simulator_host, self._flags.simulator_port, self._flags.simulator_timeout) self._vehicle = get_vehicle_handle(world, vehicle_id) # Install the collision sensor. collision_blueprint = world.get_blueprint_library().find( 'sensor.other.collision') self._logger.debug("Spawning a collision sensor.") self._collision_sensor = world.spawn_actor( collision_blueprint, Transform(Location(), Rotation()).as_simulator_transform(), attach_to=self._vehicle) # Register the callback on the collision sensor. self._collision_sensor.listen(self.process_collision)
def test_location_from_carla(x, y, z): """ Test that the Location is initialized correctly from a carla.Location instance """ location = Location.from_carla_location(carla.Location(x, y, z)) assert np.isclose(location.x, x), "X values are not the same." assert np.isclose(location.y, y), "Y values are not the same." assert np.isclose(location.z, z), "Z values are not the same."
def test_camera_unreal_transform(rotation, expected): """ Ensure that the camera space to unreal engine coordinate space conversion is correct. The camera space is defined as: +x to the right, +y to down, +z into the screen. The unreal engine coordinate space is defined as: +x into the screen, +y to the right, +z to up. """ camera_rotation = Rotation(*rotation) camera_setup = CameraSetup(name='camera_setup', camera_type='sensor.camera.rgb', width=1920, height=1080, transform=Transform(Location(), camera_rotation)) transformed_rotation = camera_setup.get_unreal_transform().rotation transformed_rotation = [ transformed_rotation.pitch, transformed_rotation.yaw, transformed_rotation.roll ] assert all(np.isclose(transformed_rotation, expected)), \ "The unreal transformation does not match the expected transform."
def test_lidar_unreal_transform(rotation, expected): """ Ensure that the LIDAR space to unreal engine coordinate space conversion is correct. The LIDAR space is defined as: +x to the right, +y out of the screen, +z is down. The unreal engine coordinate space is defined as: +x into the screen, +y to the right, +z to up. """ lidar_rotation = Rotation(*rotation) lidar_setup = LidarSetup(name='lidar_setup', lidar_type='sensor.lidar.ray_cast', transform=Transform(Location(), lidar_rotation), range=6000.0, rotation_frequency=3000.0, channels=3, upper_fov=90.0, lower_fov=90.0, points_per_second=100) transformed_rotation = lidar_setup.get_unreal_transform().rotation transformed_rotation = [ transformed_rotation.pitch, transformed_rotation.yaw, transformed_rotation.roll ] assert all(np.isclose(transformed_rotation, expected)), \ "The unreal transformation does not match the expected transform."
def get_lane_center_transforms(self): if len(self.left_markings) < len(self.right_markings): anchor_markings = self.left_markings other_markings = self.right_markings else: anchor_markings = self.right_markings other_markings = self.left_markings index_other = 0 center_markings = deque([]) for transform in anchor_markings: dist = transform.location.distance( other_markings[index_other].location) while (index_other + 1 < len(other_markings) and dist > transform.location.distance( other_markings[index_other + 1].location)): index_other += 1 dist = transform.location.distance( other_markings[index_other].location) if index_other < len(other_markings): other_loc = other_markings[index_other].location center_location = Location( (transform.location.x + other_loc.x) / 2.0, (transform.location.y + other_loc.y) / 2.0, (transform.location.z + other_loc.z) / 2.0) center_markings.append(Transform(center_location, Rotation())) return center_markings
def _postprocess_predictions(self, prediction_array, vehicle_trajectories, vehicle_ego_transforms): # The prediction_array consists of predictions with respect to each # vehicle. Transform each predicted trajectory to be in relation to the # ego-vehicle, then convert into an ObstaclePrediction. obstacle_predictions_list = [] num_predictions = len(vehicle_trajectories) for idx in range(num_predictions): cur_prediction = prediction_array[idx] obstacle_transform = vehicle_trajectories[idx].obstacle.transform predictions = [] # Because R2P2 only predicts (x,y) coordinates, we assume the # vehicle stays at the same height as its last location. for t in range(self._flags.prediction_num_future_steps): cur_point = vehicle_ego_transforms[idx].transform_points( np.array([[ cur_prediction[t][0], cur_prediction[t][1], vehicle_ego_transforms[idx].location.z ]]))[0] # R2P2 does not predict vehicle orientation, so we use our # estimated orientation of the vehicle at its latest location. predictions.append( Transform(location=Location(cur_point[0], cur_point[1], cur_point[2]), rotation=vehicle_ego_transforms[idx].rotation)) # Probability; currently a filler value because we are taking # just one sample from distribution obstacle_predictions_list.append( ObstaclePrediction(vehicle_trajectories[idx], obstacle_transform, 1.0, predictions)) return obstacle_predictions_list
def get_pixel_location(self, pixel, camera_setup: CameraSetup): """ Gets the 3D world location from pixel coordinates. Args: pixel (:py:class:`~pylot.utils.Vector2D`): Pixel coordinates. camera_setup (:py:class:`~pylot.drivers.sensor_setup.CameraSetup`): The setup of the camera with its transform in the world frame of reference. Returns: :py:class:`~pylot.utils.Location`: The 3D world location, or None if all the point cloud points are behind. """ # Select only points that are in front. # Setting the threshold to 0.1 because super close points cause # floating point errors. fwd_points = self.points[np.where(self.points[:, 2] > 0.1)] if len(fwd_points) == 0: return None intrinsic_mat = camera_setup.get_intrinsic_matrix() # Project our 2D pixel location into 3D space, onto the z=1 plane. p3d = np.dot(inv(intrinsic_mat), np.array([[pixel.x], [pixel.y], [1.0]])) if self._lidar_setup.lidar_type == 'sensor.lidar.ray_cast': location = PointCloud.get_closest_point_in_point_cloud( fwd_points, Vector2D(p3d[0], p3d[1]), normalized=True) # Use the depth from the retrieved location. p3d *= np.array([location.z]) p3d = p3d.transpose() # Convert from camera to unreal coordinates if the lidar type is # sensor.lidar.ray_cast to_world_transform = camera_setup.get_unreal_transform() camera_point_cloud = to_world_transform.transform_points(p3d)[0] pixel_location = Location(camera_point_cloud[0], camera_point_cloud[1], camera_point_cloud[2]) elif self._lidar_setup.lidar_type == 'velodyne': location = PointCloud.get_closest_point_in_point_cloud( fwd_points, Vector2D(p3d[0], p3d[1]), normalized=False) # Use the depth from the retrieved location. p3d[2] = location.z p3d = p3d.transpose() pixel_location = Location(p3d[0, 0], p3d[0, 1], p3d[0, 2]) return pixel_location
def _construct_waypoints(self, timestamp, pose_msg, path_x, path_y, speeds, success): """ Convert the rrt* path into a waypoints message. """ path_transforms = [] target_speeds = deque() if not success: self._logger.error("@{}: RRT* failed. " "Sending emergency stop.".format(timestamp)) x = pose_msg.data.transform.location.x y = pose_msg.data.transform.location.y current_index = 0 min_dist = np.infty for i, wp in enumerate(self._prev_waypoints): dist = np.linalg.norm([wp.location.x - x, wp.location.y - y]) if dist <= min_dist: current_index = i min_dist = dist for wp in itertools.islice( self._prev_waypoints, current_index, current_index + self._flags.num_waypoints_ahead): path_transforms.append(wp) target_speeds.append(0) waypoints = deque(path_transforms) 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)
def test_imu_setup_failed_initialization(): # Set up the required parameters for the initialization. name = 'imu_sensor' transform = Transform(Location(), Rotation()) with pytest.raises(AssertionError): imu_setup = IMUSetup(name=1, transform=transform) with pytest.raises(AssertionError): imu_setup = IMUSetup(name=name, transform=None)
def test_point_cloud_get_pixel_location(lidar_points, pixel, expected): camera_setup = CameraSetup( 'test_setup', 'test_type', 801, 601, # width, height Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)), fov=90) point_cloud = PointCloud(lidar_points, Transform(Location(), Rotation())) location = point_cloud.get_pixel_location(pixel, camera_setup) assert np.isclose(location.x, expected.x), 'Returned x value is not the same ' 'as expected' assert np.isclose(location.y, expected.y), 'Returned y value is not the same ' 'as expected' assert np.isclose(location.z, expected.z), 'Returned z value is not the same ' 'as expected'
def create_left_right_camera_setups(camera_name_prefix, location, width, height, camera_offset, fov=90): """ Creates a dual-RGB-camera setup with the center at the given location, and the two cameras on either side of the center at a distance specified by the camera_offset. The Rotation is set to (pitch=0, yaw=0, roll=0). Args: camera_name_prefix (str): The name of the camera instance. A suffix of "_left" and "_right" is appended to the name. location (:py:class:`~pylot.utils.Location`): The location of the center of the cameras with respect to the center of the vehicle. width (int): The width of the image returned by the cameras. height (int): The height of the image returned by the cameras. camera_offset (float): The offset of the two cameras from the center. fov (float): The field of view of the image returned by the cameras. Returns: tuple: A tuple containing two instances of :py:class:`~pylot.drivers.sensor_setup.RGBCameraSetup` for the left and right camera setups with the given parameters. """ rotation = Rotation() left_loc = location + Location(0, -camera_offset, 0) right_loc = location + Location(0, camera_offset, 0) left_transform = Transform(left_loc, rotation) right_transform = Transform(right_loc, rotation) left_camera_setup = RGBCameraSetup(camera_name_prefix + '_left', width, height, left_transform, fov=fov) right_camera_setup = RGBCameraSetup(camera_name_prefix + '_right', width, height, right_transform, fov=fov) return (left_camera_setup, right_camera_setup)
def _construct_waypoints(self, timestamp, pose_msg, path_x, path_y, speeds, success): """ Convert the optimal frenet path into a waypoints message. """ path_transforms = [] target_speeds = deque() if not success: self._logger.debug("@{}: Frenet Optimal Trajectory failed. " "Sending emergency stop.".format(timestamp)) x = pose_msg.data.transform.location.x y = pose_msg.data.transform.location.y current_index = 0 min_dist = np.infty for i, wp in enumerate(self._prev_waypoints): dist = np.linalg.norm([wp.location.x - x, wp.location.y - y]) if dist <= min_dist: current_index = i min_dist = dist for wp in itertools.islice( self._prev_waypoints, current_index, current_index + self._flags.num_waypoints_ahead): 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 _get_waypoint(self, location: Location, project_to_road: bool, lane_type): try: waypoint = self._map.get_waypoint(location.as_simulator_location(), project_to_road=project_to_road, lane_type=lane_type) except RuntimeError as err: self._logger.error('get_waypoint call failed: {}'.format(err)) waypoint = None return waypoint
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))