def test_negative_rotation_from_carla(): """ Test that Rotation throws a ValueError if incorrect carla_rot argument is passed. """ DummyType = namedtuple("DummyType", "pitch, yaw, roll") dummy_instance = DummyType(10, 20, 30) with pytest.raises(ValueError): Rotation.from_carla_rotation(dummy_instance)
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_rotation(): """ Test the as_carla_rotation instance method of Rotation """ rotation = Rotation(pitch=1, yaw=2, roll=3) carla_rotation = rotation.as_carla_rotation() assert isinstance(carla_rotation, carla.Rotation), "Returned instance is " "not of the type carla.Rotation" assert np.isclose(carla_rotation.pitch, rotation.pitch), "Returned " "instance pitch value is not the same as the one in rotation." assert np.isclose(carla_rotation.yaw, rotation.yaw), "Returned instance " "yaw value is not the same as the one in rotation." assert np.isclose(carla_rotation.roll, rotation.roll), "Returned instance " "roll value is not the same as the one in location."
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 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 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 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 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 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 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 _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_rotation_from_carla(pitch, yaw, roll): """ Test that the Rotation is initialized correctly from a carla.Rotation instance """ carla_rotation = carla.Rotation(pitch, yaw, roll) rotation = Rotation.from_carla_rotation(carla_rotation) assert np.isclose(rotation.pitch, pitch), "pitch values are not the same." assert np.isclose(rotation.yaw, yaw), "yaw values are not the same." assert np.isclose(rotation.roll, roll), "roll values are not the same."
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 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 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 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 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 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 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)
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_lane_waypoint(self, location): if self.is_on_lane(location): return Transform(location, Rotation()) closest_transform = None min_dist = np.infty for transform in self.left_markings: dist = transform.location.distance(location) if dist < min_dist: min_dist = dist closest_transform = transform for transform in self.right_markings: dist = transform.location.distance(location) if dist < min_dist: min_dist = dist closest_transform = transform return closest_transform
def generate_predicted_trajectories(self, msg, linear_prediction_stream): self._logger.debug('@{}: received trajectories message'.format( msg.timestamp)) obstacle_predictions_list = [] for obstacle in msg.obstacle_trajectories: # Time step matrices used in regression. num_steps = min(self._flags.prediction_num_past_steps, len(obstacle.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[-(t + 1)] xy[t][0] = transform.location.x xy[t][1] = transform.location.y linear_model_params = np.linalg.lstsq(ts, xy)[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): predictions.append( Transform(location=Location(x=predict_array[t][0], y=predict_array[t][1]), rotation=Rotation())) # Get the current transform of the obstacle, which is the last # trajectory value. cur_transform = obstacle.trajectory[-1] obstacle_predictions_list.append( ObstaclePrediction( obstacle.label, obstacle.id, cur_transform, obstacle.bounding_box, 1.0, # probability predictions)) linear_prediction_stream.send( PredictionMessage(msg.timestamp, obstacle_predictions_list))
def test_get_pixel_locations(depth_frame, pixels, expected): height, width = depth_frame.shape camera_setup = CameraSetup('test_setup', 'test_type', width, height, Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)), fov=90) depth_frame = DepthFrame(depth_frame, camera_setup) locations = depth_frame.get_pixel_locations(pixels) for i in range(len(pixels)): assert np.isclose(locations[i].x, expected[i].x), 'Returned x ' 'value is not the same as expected' assert np.isclose(locations[i].y, expected[i].y), 'Returned y ' 'value is not the same as expected' assert np.isclose(locations[i].z, expected[i].z), 'Returned z ' 'value is not the same as expected'
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_depth_to_point_cloud(depth_frame, expected): height, width = depth_frame.shape camera_setup = CameraSetup('test_setup', 'test_type', width, height, Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)), fov=90) depth_frame = DepthFrame(depth_frame, camera_setup) # Resulting unreal coordinates. point_cloud = depth_frame.as_point_cloud() for i in range(width * height): assert np.isclose(point_cloud[i].x, expected[i].x), 'Returned x ' 'value is not the same as expected' assert np.isclose(point_cloud[i].y, expected[i].y), 'Returned y ' 'value is not the same as expected' assert np.isclose(point_cloud[i].z, expected[i].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 on_pose_update(self, data): self._counter += 1 if self._counter % self._modulo_to_send != 0: return loc = Location(data.pose.position.x, data.pose.position.y, data.pose.position.z) quaternion = [ data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w ] roll, pitch, yaw = euler_from_quaternion(quaternion) rotation = Rotation(np.degrees(pitch), np.degrees(yaw), np.degrees(roll)) timestamp = erdos.Timestamp(coordinates=[self._msg_cnt]) pose = Pose(Transform(loc, rotation), self._forward_speed) self._logger.debug('NDT localization {}'.format(pose)) self._pose_stream.send(erdos.Message(timestamp, pose)) self._pose_stream.send(erdos.WatermarkMessage(timestamp)) self._msg_cnt += 1