class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera( fov=145, transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) graph_post_modem_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen" ) target_speed: int = 80
class Camera(BaseModel): fov: int = Field(default=70, title="Field of View") transform: Transform = Field( default=Transform( location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0), ) ) image_size_x: int = Field(default=800, title="Image size width") image_size_y: int = Field(default=600, title="Image size width") distortion_coefficient: Optional[np.ndarray] = Field(default=np.array([0, 0, 0, 0, 0]), title="Distortion Coefficient from Intel Realsense") data: Optional[np.ndarray] = Field(default=None) intrinsics_matrix: Optional[np.ndarray] = Field(default=None) def calculate_default_intrinsics_matrix(self) -> np.ndarray: """ Calculate intrinsics matrix Will set the attribute intrinsic matrix so that re-calculation is not necessary. https://github.com/carla-simulator/carla/issues/56 [ ax, 0, cx, 0, ay, cy, 0 , 0, 1 ] Returns: Intrinsics_matrix """ intrinsics_matrix = np.identity(3) intrinsics_matrix[0, 2] = self.image_size_x / 2.0 intrinsics_matrix[1, 2] = self.image_size_y / 2.0 intrinsics_matrix[0, 0] = self.image_size_x / ( 2.0 * np.tan(self.fov * np.pi / 360.0) ) intrinsics_matrix[1, 1] = self.image_size_y / ( 2.0 * np.tan(self.fov * np.pi / 360.0) ) self.intrinsics_matrix = intrinsics_matrix return intrinsics_matrix class Config: arbitrary_types_allowed = True def visualize(self, title="CameraData", duration=1) -> None: """ Visualize camera data. Args: title: title of cv2 image duration: in milisecond Returns: None """ if self.data is not None: cv2.imshow(title, self.data.data) cv2.waitKey(duration)
def convert_rotation_from_source_to_agent( self, source: carla.Rotation) -> Rotation: """Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float).""" roll, pitch, yaw = source.roll, source.pitch, source.yaw if yaw <= 90: yaw = yaw + 90 else: yaw = yaw - 270 return Rotation(roll=roll, pitch=pitch, yaw=-yaw)
def convert_vehicle_from_source_to_agent(self, source: JetsonVehicle) -> Vehicle: return Vehicle(transform=Transform( location=Location(x=-source.location[0], y=source.location[1], z=-source.location[2]), rotation=Rotation(roll=-source.rotation[2], pitch=source.rotation[0], yaw=-source.rotation[1]), ), velocity=Vector3D(x=-source.velocity[0], y=source.velocity[1], z=-source.velocity[2]), wheel_base=0.26, control=self.convert_control_from_source_to_agent(source=source) )
def convert_rotation_from_source_to_agent(self, source: np.ndarray) -> Rotation: """ Convert a Jetson raw rotation to Rotation(pitch=float,yaw=float,roll=float). Args: source (): Returns: Rotation(pitch, yaw, roll) """ return Rotation(roll=source[0], pitch=source[1], yaw=source[2])
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \ Optional[ViveTrackerData]: if source is not None: vive_tracker_data = ViveTrackerData( location=Location(x=-source.x, y=source.y, z=-source.z), rotation=Rotation(roll=-source.roll, pitch=source.pitch + 60, yaw=-source.yaw), velocity=Vector3D(x=-source.vel_x, y=source.vel_y, z=-source.vel_z)) return vive_tracker_data return None
def _raw_coord_to_transform(self, raw: List[float]) -> Optional[Transform]: """ transform coordinate to Transform instance Args: raw: coordinate in form of [x, y, z, pitch, yaw, roll] Returns: Transform instance """ if len(raw) == 3: return Transform( location=Location(x=raw[0], y=raw[1], z=raw[2]), rotation=Rotation(pitch=0, yaw=0, roll=0), ) elif len(raw) == 6: return Transform( location=Location(x=raw[0], y=raw[1], z=raw[2]), rotation=Rotation(roll=raw[3], pitch=raw[4], yaw=raw[5]), ) else: self.logger.error(f"Point {raw} is invalid, skipping") return None
def convert_rotation_from_source_to_agent( self, source: carla.Rotation) -> Rotation: """Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float).""" roll, pitch, yaw = source.roll, source.pitch, source.yaw reflective_matrix = np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]) R_Carla = rotation_matrix_from_euler(roll=roll, pitch=pitch, yaw=yaw) # R_ROAR = P * R_carla * P R_ROAR = reflective_matrix @ R_Carla @ reflective_matrix r11, r21, r31, r32, r33 = R_ROAR[0][0], R_ROAR[1][0], R_ROAR[2][ 0], R_ROAR[2][1], R_ROAR[2][2] yaw = alpha = np.degrees(np.arctan2(r21, r11)) pitch = beta = np.degrees(np.arctan2(-r31, np.sqrt(r32**2 + r33**2))) roll = gamma = np.degrees(np.arctan2(r32, r33)) return Rotation(roll=roll - 90, pitch=pitch, yaw=yaw)
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \ Optional[ViveTrackerData]: """ Converts raw Vive Tracker data to ViveTrackerData(Location, Rotation, Velocity). Args: source (): Returns: ViveTrackerData(Location, Rotation, Velocity) """ if source is not None: vive_tracker_data = ViveTrackerData( location=Location(x=-source.x, y=source.y, z=-source.z), rotation=Rotation( roll=-source.roll, pitch=source.pitch - 90, # 不知道为什么有60度的误差 yaw=-source.yaw), velocity=Vector3D(x=source.vel_x, y=source.vel_y, z=source.vel_z)) return vive_tracker_data return None
def rotationMatrixToEulerAngles(self, R) -> Rotation: assert (self.isRotationMatrix(R)) sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) singular = sy < 1e-6 if not singular: x = math.atan2(R[2, 1], R[2, 2]) y = math.atan2(-R[2, 0], sy) z = math.atan2(R[1, 0], R[0, 0]) else: x = math.atan2(-R[1, 2], R[1, 1]) y = math.atan2(-R[2, 0], sy) z = 0 euler_angles_deg = np.rad2deg(np.array([x, y, z])) rotation = Rotation( roll=euler_angles_deg[2], pitch=euler_angles_deg[0], yaw=euler_angles_deg[1], ) return rotation
def run_in_series(self) -> VehicleControl: """ Run step for the local planner Procedure: 1. Sync data 2. get the correct look ahead for current speed 3. get the correct next waypoint 4. feed waypoint into controller 5. return result from controller Returns: next control that the local think the agent should execute. """ if (len(self.mission_planner.mission_plan) == 0 and len(self.way_points_queue) == 0): return VehicleControl() # get vehicle's location vehicle_transform: Union[Transform, None] = self.agent.vehicle.transform if vehicle_transform is None: raise AgentException( "I do not know where I am, I cannot proceed forward") # redefine closeness level based on speed self.set_closeness_threhold(self.closeness_threshold_config) # get current lane center lane_detector: LaneDetector = self.agent.lane_detector lane_center = lane_detector.lane_center if lane_center is not None: next_location = Location.from_array(lane_center[0] * 0.9 + lane_center[1] * 0.1) next_yaw, next_pitch = two_points_to_yaw_pitch( lane_center[0], lane_center[1]) next_rotation = Rotation(yaw=next_yaw, pitch=next_pitch, roll=0) target_waypoint = self.target_waypoint = Transform( location=next_location, rotation=next_rotation) else: target_waypoint = self.target_waypoint curr_closest_dist = float("inf") while True: if len(self.way_points_queue) == 0: self.logger.info("Destination reached") return VehicleControl() # waypoint: Transform = self.way_points_queue[0] waypoint: Transform = self.next_waypoint_smooth() curr_dist = vehicle_transform.location.distance(waypoint.location) if curr_dist < curr_closest_dist: # if i find a waypoint that is closer to me than before # note that i will always enter here to start the calculation for curr_closest_dist curr_closest_dist = curr_dist elif curr_dist < self.closeness_threshold: # i have moved onto a waypoint, remove that waypoint from the queue self.way_points_queue.popleft() else: break target_waypoint = target_waypoint * 0.4 + self.next_waypoint_smooth( ) * 0.6 control: VehicleControl = self.controller.run_in_series( next_waypoint=target_waypoint) self.logger.debug( f"\n" f"Curr Transform: {self.agent.vehicle.transform}\n" f"Target Location: {target_waypoint.location}\n" f"Control: {control} | Speed: {Vehicle.get_speed(self.agent.vehicle)}\n" ) return control
def convert_rotation_from_source_to_agent(self, source) -> Rotation: return Rotation(pitch=source.pitch, yaw=source.yaw, roll=source.roll)
def convert_rotation_from_source_to_agent(self, source: carla.Rotation) -> Rotation: """Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float).""" return Rotation(pitch=source.pitch, yaw=source.yaw, roll=source.roll)
class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera(fov=70, transform=Transform( location=Location(x=1.6, y=0, z=1.7 ), rotation=Rotation( pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera(fov=70, transform=Transform( location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0) ), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera(fov=145, transform=Transform( location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation( pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field(default=(Path( os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field(default=(Path( os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) json_qr_code_file_path: str = Field(default=(Path( os.getcwd()) / "ROAR_Jetson" / "data" / "track_1.json" ).as_posix()) output_data_folder_path: str = Field( default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen") enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") num_laps: int = Field(default=1, description="Number of laps to run for") output_data_folder_path: str = Field(default="./data/output", description="path to save newly generated waypoints txt file.") output_data_file_name: str = Field(default="map_waypoints", description="file name for a newly generated waypoints txt file.") max_speed: float = Field(default=200, description="maximum speed in which the vehicle can drive at") target_speed: int = Field(default=80, description="The tracking speed that the pid controller is trying to achieve") steering_boundary: tuple = Field(default=(-1,1), description="maximum and minimum boundary for steering") # ROAR Academy: throttle_boundary: tuple = Field(default=(0,1), description="maximum and minimum boundary for steering") # ROAR Academy: waypoints_look_ahead_values: dict = Field(default={"60": 5, "80": 10, "120": 20, "180": 50}) # ROAR Academy: simple_waypoint_local_planner_config_file_path: str = \ Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json") pid_values: dict = Field(default={ 'longitudinal_controller': { '40': { 'Kp': 0.8, 'Kd': 0.2, 'Ki': 0 }, '60': { 'Kp': 0.5, 'Kd': 0.2, 'Ki': 0 }, '150': { 'Kp': 0.2, 'Kd': 0.1, 'Ki': 0.1 } }, 'latitudinal_controller': { '60': { 'Kp': 0.8, 'Kd': 0.1, 'Ki': 0.1 }, '100': { 'Kp': 0.6, 'Kd': 0.2, 'Ki': 0.1 }, '150': { 'Kp': 0.5, 'Kd': 0.2, 'Ki': 0.1 } } } ) # ROAR Academy pid_config_file_path: str = Field(default="./ROAR_Sim/configurations/pid_config.json") lqr_config_file_path: str = Field(default="./ROAR_Sim/configurations/lqr_config.json")
def convert_rotation_from_source_to_agent(self, source: Rotation) -> Rotation: r = Rotation(roll=np.rad2deg(source.pitch), pitch=np.rad2deg(source.roll), yaw=np.rad2deg(source.yaw)) return r
class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera( fov=145, transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) json_qr_code_file_path: str = Field(default=(Path(os.getcwd()) / "ROAR_Jetson" / "data" / "track_1.json").as_posix()) output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen" ) target_speed: int = 80 pid_config_file_path: str = Field( default="./ROAR_Sim/configurations/pid_config.json") pid_config: dict = Field(default={60: { 1, 23, }}) depth_to_pcd_config_path: str = Field( default="./ROAR/configurations/carla/carla_depth_to_pcd_config.json") lqr_config_file_path: str = Field( default="./ROAR_Sim/configurations/lqr_config.json") occu_map_config_path: str = Field( default="./ROAR_Sim/configurations/occu_map_config.json") obstacle_from_depth_config_path: str = Field( default="./ROAR_Sim/configurations/obstacle_from_depth_config.json") simple_waypoint_local_planner_config_file_path: str = \ Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json") max_speed: float = Field( default=200, description="maximum speed in which the vehicle can drive at") num_laps: int = Field(default=1, description="Number of laps to run for")