def process_imu(self, imu_msg): """Invoked when an IMU message is received from the simulator. Sends IMU measurements to downstream operators. Args: imu_msg (carla.IMUMeasurement): IMU reading. """ game_time = int(imu_msg.timestamp * 1000) timestamp = erdos.Timestamp(coordinates=[game_time]) watermark_msg = erdos.WatermarkMessage(timestamp) with erdos.profile(self.config.name + '.process_imu', self, event_data={'timestamp': str(timestamp)}): with self._lock: msg = IMUMessage( timestamp, Transform.from_simulator_transform(imu_msg.transform), Vector3D.from_simulator_vector(imu_msg.accelerometer), Vector3D.from_simulator_vector(imu_msg.gyroscope), imu_msg.compass) self._imu_stream.send(msg) # Note: The operator is set not to automatically propagate # watermarks received on input streams. Thus, we can issue # watermarks only after the simulator callback is invoked. self._imu_stream.send(watermark_msg)
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 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 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 _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 _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 create_segmented_camera_setup(camera_name_prefix, camera_location, width, height, fov=90): """ Creates a SegmentedCameraSetup instance with the given values. 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 "_segmented" is appended to the name. camera_location (:py:class:`~pylot.utils.Location`): The location of the camera with respect to the center of the vehicle. width (int): The width of the image returned by the camera. height (int): The height of the image returned by the camera. fov (float): The field of view of the image returned by the camera. Returns: :py:class:`~pylot.drivers.sensor_setup.SegmentedCameraSetup`: A camera setup with the given parameters. """ transform = Transform(camera_location, Rotation()) return SegmentedCameraSetup(camera_name_prefix + '_segmented', width, height, transform, fov=fov)
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 compute_waypoints(self, source_loc: Location, destination_loc: Location): """Computes waypoints between two locations. Assumes that the ego vehicle has the same orientation as the lane on whch it is on. Args: source_loc (:py:class:`~pylot.utils.Location`): Source location in world coordinates. destination_loc (:py:class:`~pylot.utils.Location`): Destination location in world coordinates. Returns: list(:py:class:`~pylot.utils.Transform`): List of waypoint transforms. """ start_waypoint = self._get_waypoint(source_loc, project_to_road=True, lane_type=carla.LaneType.Driving) end_waypoint = self._get_waypoint(destination_loc, project_to_road=True, lane_type=carla.LaneType.Driving) assert start_waypoint and end_waypoint, 'Map could not find waypoints' route = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location) # TODO(ionel): The planner returns several options in intersections. # We always take the first one, but this is not correct. return deque([ Transform.from_carla_transform(waypoint[0].transform) for waypoint in route ])
def get_right_lane(self, location: Location): waypoint = self._get_waypoint(location, project_to_road=False) if waypoint: right_lane_waypoint = waypoint.get_right_lane() if right_lane_waypoint: return Transform.from_carla_transform(waypoint.transform) return None
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().as_simulator_transform(), attach_to=self._vehicle) # Register the callback on the collision sensor. self._collision_sensor.listen(self.process_collision)
def create_rgb_camera_setup(camera_name, camera_location, width, height, fov=90): transform = Transform(camera_location, Rotation()) return RGBCameraSetup(camera_name, width, height, transform, fov)
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 create_center_lidar_setup(location, rotation_frequency=20): """ Creates a LidarSetup instance with the given location. The Rotation is set to (pitch=0, roll=0, yaw=0). Args: location (:py:class:`~pylot.utils.Location`): The location of the LIDAR with respect to the center of the vehicle. Returns: :py:class:`~pylot.drivers.sensor_setup.LidarSetup`: A LidarSetup with the given location. """ rotation = Rotation() # Place the lidar in the same position as the camera. lidar_transform = Transform(location, rotation) return LidarSetup( name='front_center_lidar', lidar_type='sensor.lidar.ray_cast', transform=lidar_transform, range=5000, # in centimeters rotation_frequency=rotation_frequency, channels=32, upper_fov=15, lower_fov=-30, points_per_second=250000)
def get_right_lane(self, location: Location): waypoint = self._get_waypoint(location) if waypoint: right_lane_waypoint = waypoint.get_right_lane() if right_lane_waypoint: return Transform.from_simulator_transform( right_lane_waypoint.transform) return None
def _to_camera_coordinates(self, points): # Converts points in lidar coordinates to points in camera coordinates. # See CameraSetup in pylot/drivers/sensor_setup.py for coordinate # axis orientations. # # The Velodyne coordinate space is defined as: # +x into the screen, +y to the left, and +z up. # # Note: We're using the ROS velodyne driver coordinate # system, not the one specified in the Velodyne manual. # Link to the ROS coordinate system: # https://www.ros.org/reps/rep-0103.html#axis-orientation if self._lidar_setup.lidar_type == 'sensor.lidar.ray_cast': if self._lidar_setup.legacy: # The legacy CARLA Lidar coordinate space is defined as: # +x to right, +y out of the screen, +z down. to_camera_transform = Transform(matrix=np.array( [[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]])) else: # The latest coordiante space is the unreal space. to_camera_transform = Transform(matrix=np.array( [[0, 1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0], [0, 0, 0, 1]])) elif self._lidar_setup.lidar_type == 'velodyne': to_camera_transform = Transform(matrix=np.array( [[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0], [0, 0, 0, 1]])) else: raise ValueError('Unexpected lidar type {}'.format( self._lidar_setup.lidar_type)) transformed_points = to_camera_transform.transform_points(points) return transformed_points
def __create_unreal_transform(transform): """ Takes in a Transform that occurs in camera coordinates, and converts it into a Transform that goes from lidar coordinates to camera coordinates. """ to_camera_transform = Transform(matrix=np.array( [[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1]])) return transform * to_camera_transform
def get_right_lane(self, location: Location): waypoint = self._get_waypoint(location, project_to_road=False, lane_type=carla.LaneType.Any) if waypoint: right_lane_waypoint = waypoint.get_right_lane() if right_lane_waypoint: return Transform.from_simulator_transform( right_lane_waypoint.transform) return None
def get_left_lane(self, location: Location): waypoint = self._get_waypoint(location, project_to_road=False, lane_type=carla.LaneType.Any) if waypoint: left_lane_waypoint = waypoint.get_left_lane() if left_lane_waypoint: return Transform.from_carla_transform( left_lane_waypoint.transform) return None
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 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 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 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 create_depth_camera_setup(camera_name_prefix, camera_location, width, height, fov=90): transform = Transform(camera_location, Rotation()) return DepthCameraSetup(camera_name_prefix + '_depth', width, height, transform, fov=fov)
def generate_predicted_trajectories(self, msg: Message, linear_prediction_stream: WriteStream): self._logger.debug('@{}: received trajectories message'.format( msg.timestamp)) obstacle_predictions_list = [] nearby_obstacle_trajectories, nearby_obstacles_ego_transforms = \ msg.get_nearby_obstacles_info(self._flags.prediction_radius) num_predictions = len(nearby_obstacle_trajectories) self._logger.info( '@{}: Getting linear predictions for {} obstacles'.format( msg.timestamp, num_predictions)) for idx in range(len(nearby_obstacle_trajectories)): obstacle_trajectory = nearby_obstacle_trajectories[idx] # Time step matrices used in regression. num_steps = min(self._flags.prediction_num_past_steps, len(obstacle_trajectory.trajectory)) ts = np.zeros((num_steps, 2)) future_ts = np.zeros((self._flags.prediction_num_future_steps, 2)) for t in range(num_steps): ts[t][0] = -t ts[t][1] = 1 for i in range(self._flags.prediction_num_future_steps): future_ts[i][0] = i + 1 future_ts[i][1] = 1 xy = np.zeros((num_steps, 2)) for t in range(num_steps): # t-th most recent step transform = obstacle_trajectory.trajectory[-(t + 1)] xy[t][0] = transform.location.x xy[t][1] = transform.location.y linear_model_params = np.linalg.lstsq(ts, xy, rcond=None)[0] # Predict future steps and convert to list of locations. predict_array = np.matmul(future_ts, linear_model_params) predictions = [] for t in range(self._flags.prediction_num_future_steps): # Linear prediction does not predict vehicle orientation, so we # use our estimated orientation of the vehicle at its latest # location. predictions.append( Transform(location=Location(x=predict_array[t][0], y=predict_array[t][1]), rotation=nearby_obstacles_ego_transforms[idx]. rotation)) obstacle_predictions_list.append( ObstaclePrediction(obstacle_trajectory, obstacle_trajectory.obstacle.transform, 1.0, predictions)) linear_prediction_stream.send( PredictionMessage(msg.timestamp, obstacle_predictions_list)) linear_prediction_stream.send(erdos.WatermarkMessage(msg.timestamp))
def create_segmented_camera_setup(camera_name_prefix, camera_location, width, height, fov=90): transform = Transform(camera_location, Rotation()) return SegmentedCameraSetup(camera_name_prefix + '_segmented', width, height, transform, fov=fov)
def _must_obey_american_traffic_light(self, ego_transform: Transform, tl_locations, tl_max_dist_thresh: float) -> bool: ego_waypoint = self._get_waypoint(ego_transform.location) # We're not on a road, or we're already in the intersection. Carry on. if ego_waypoint is None or self.__is_intersection(ego_waypoint): return (False, None) min_angle = 25.0 selected_tl_loc = None for tl_loc in tl_locations: if ego_transform.is_within_distance_ahead(tl_loc, tl_max_dist_thresh): angle, distance = ego_transform.get_angle_and_magnitude(tl_loc) if distance < 60.0 and angle < min(25.0, min_angle): min_angle = angle selected_tl_loc = tl_loc if selected_tl_loc is not None: return (True, selected_tl_loc) else: return (False, None)
def create_left_right_camera_setups(camera_name_prefix, location, width, height, camera_offset, fov=90): 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 test_camera_setup_failed_initialization(): """ Ensure that the CameraSetup constructor fails when wrong values or values of the wrong type are provided. """ # Set up the correct values for all the constructor values. name, camera_type = 'camera_setup', 'sensor.camera.rgb' width, height = 1920, 1080 transform = Transform(Location(), Rotation()) # Ensure that a wrong name throws an error. with pytest.raises(AssertionError): camera_setup = CameraSetup(name=1, camera_type=camera_type, width=width, height=height, transform=transform) # Ensure that a wrong camera_type throws an error. with pytest.raises(AssertionError): camera_setup = CameraSetup(name=name, camera_type=None, width=width, height=height, transform=transform) # Ensure that a wrong width, height throws an error. with pytest.raises(AssertionError): camera_setup = CameraSetup(name=name, camera_type=camera_type, width=float(width), height=float(height), transform=transform) # Ensure that a wrong transform throws an error. with pytest.raises(AssertionError): camera_setup = CameraSetup(name=name, camera_type=camera_type, width=width, height=height, transform=None) # Ensure that a bad fov raises an error. with pytest.raises(AssertionError): camera_setup = CameraSetup(name=name, camera_type=camera_type, width=width, height=height, transform=transform, fov=None)
def create_center_lidar_setup(location): rotation = Rotation() # Place the lidar in the same position as the camera. lidar_transform = Transform(location, rotation) return LidarSetup( name='front_center_lidar', lidar_type='sensor.lidar.ray_cast', transform=lidar_transform, range=5000, # in centimers rotation_frequency=20, channels=32, upper_fov=15, lower_fov=-30, points_per_second=500000)