diff --git a/Bridges/carla_bridge.py b/Bridges/carla_bridge.py index de48e942..22fabdd0 100644 --- a/Bridges/carla_bridge.py +++ b/Bridges/carla_bridge.py @@ -16,6 +16,7 @@ IMUData, Vector3D, Transform, + LidarData ) from ROAR.utilities_module.utilities import png_to_depth @@ -42,7 +43,7 @@ def convert_rotation_from_source_to_agent(self, source: carla.Rotation) -> Rotat """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 + yaw = yaw + 90 else: yaw = yaw - 270 return Rotation(roll=roll, pitch=pitch, yaw=-yaw) @@ -124,8 +125,26 @@ def convert_sensor_data_from_source_to_agent(self, source: dict) -> SensorsData: imu_data=self.convert_imu_from_source_to_agent( source=source.get("imu", None) ), + lidar=self.convert_lidar_from_source_to_agent( + source=source.get("lidar", None) + ) ) + def convert_lidar_from_source_to_agent(self, source: carla.LidarMeasurement): + # Get the lidar data and convert it to a numpy array. + try: + p_cloud_size = len(source) + p_cloud = np.copy(np.frombuffer(source.raw_data, dtype=np.dtype('f4'))) + p_cloud = np.reshape(p_cloud, (p_cloud_size, 4)) + return LidarData( + num_channel=source.channels, + horizontal_angle=source.horizontal_angle, + raw_data=p_cloud + ) + except Exception as e: + self.logger.error(e) + return None + def convert_vehicle_from_source_to_agent(self, source: carla.Vehicle) -> Vehicle: """Converts Velocity, Transform, and Control of carla.Vehicle""" control: VehicleControl = self.convert_control_from_source_to_agent( diff --git a/ROAR/configurations/carla/carla_agent_configuration.json b/ROAR/configurations/carla/carla_agent_configuration.json index b36e8561..14e82396 100644 --- a/ROAR/configurations/carla/carla_agent_configuration.json +++ b/ROAR/configurations/carla/carla_agent_configuration.json @@ -1,4 +1,31 @@ { + "lidar_config": { + "channels": 32, + "transform": { + "location": { + "x": 1, + "y": 1.8, + "z": 0 + }, + "rotation": { + "pitch": 0.0, + "yaw": 0.0, + "roll": 0.0 + } + }, + "range": 100, + "points_per_second": 56000, + "rotation_frequency": 10, + "upper_fov":10, + "lower_fov":-30, + "horizontal_fov":360, + "atmosphere_attenuation_rate":0.004, + "dropoff_general_rate":0.45, + "dropoff_intensity_limit": 0.8, + "dropoff_zero_intensity": 0.4, + "sensor_tick": 0, + "noise_stddev":0 + }, "front_depth_cam": { "fov": 70, "transform": { diff --git a/ROAR/configurations/configuration.py b/ROAR/configurations/configuration.py index b42abb7b..9c373394 100644 --- a/ROAR/configurations/configuration.py +++ b/ROAR/configurations/configuration.py @@ -1,6 +1,6 @@ from pydantic import Field, BaseModel from pathlib import Path -from ROAR.utilities_module.camera_models import Camera +from ROAR.utilities_module.camera_models import Camera, LidarConfigModel from ROAR.utilities_module.data_structures_models import Transform, Location, Rotation import os @@ -9,6 +9,7 @@ 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") + lidar_config: LidarConfigModel = Field(default=LidarConfigModel()), front_depth_cam: Camera = Field(default=Camera(fov=70, transform=Transform( location=Location(x=1.6, @@ -75,8 +76,8 @@ class Configuration(BaseModel): 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,} + default={ + 60: {1, 23, } } ) lqr_config_file_path: str = Field(default="./ROAR_Sim/configurations/lqr_config.json") diff --git a/ROAR/utilities_module/camera_models.py b/ROAR/utilities_module/camera_models.py index 54ed1f4f..c378b10e 100644 --- a/ROAR/utilities_module/camera_models.py +++ b/ROAR/utilities_module/camera_models.py @@ -42,10 +42,10 @@ def calculate_default_intrinsics_matrix(self) -> np.ndarray: 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) + 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) + 2.0 * np.tan(self.fov * np.pi / 360.0) ) self.intrinsics_matrix = intrinsics_matrix return intrinsics_matrix @@ -65,3 +65,28 @@ def visualize(self, title="CameraData", duration=1) -> None: if self.data is not None: cv2.imshow(title, self.data.data) cv2.waitKey(duration) + + +class LidarConfigModel(BaseModel): + channels: int = Field(32) + range: int = Field(100), + points_per_second: int = Field(56000), + rotation_frequency: float = Field(10), + upper_fov: float = Field(10), + lower_fov: float = Field(-30), + horizontal_fov: float = Field(360), + atmosphere_attenuation_rate: float = Field(0.004), + dropoff_general_rate: float = Field(0.45), + dropoff_intensity_limit: float = Field(0.8), + dropoff_zero_intensity: float = Field(0.4), + sensor_tick: float = Field(0), + noise_stddev: float = Field(0) + + transform: Transform = Field( + default=Transform( + location=Location(x=1.6, y=0, z=1.7), + rotation=Rotation(pitch=0, yaw=0, roll=0), + ) + ) + + diff --git a/ROAR/utilities_module/data_structures_models.py b/ROAR/utilities_module/data_structures_models.py index f5a22fbe..a0a63d68 100644 --- a/ROAR/utilities_module/data_structures_models.py +++ b/ROAR/utilities_module/data_structures_models.py @@ -35,7 +35,7 @@ def __add__(self, other): return Location(x=self.x + other.x, y=self.y + other.y, z=self.z + other.z) def __str__(self): - return f"x: {round(self.x, 3)}, y: {round(self.y, 3)}, z: {round(self.z,3)}" + return f"x: {round(self.x, 3)}, y: {round(self.y, 3)}, z: {round(self.z, 3)}" def __truediv__(self, scalar): return Location(x=self.x / scalar, y=self.y / scalar, z=self.z / scalar) @@ -195,11 +195,21 @@ class TrackingData(BaseModel): velocity: Vector3D = Field() +class LidarData(BaseModel): + num_channel: int = Field(...) + horizontal_angle: float = Field(...) + raw_data: np.ndarray = Field(...) + + class Config: + arbitrary_types_allowed = True + + class SensorsData(BaseModel): front_rgb: Optional[RGBData] = Field(default=None) rear_rgb: Optional[RGBData] = Field(default=None) front_depth: Optional[DepthData] = Field(default=None) imu_data: Optional[IMUData] = Field(default=None) + lidar_data: Optional[LidarData] = Field(default=None) location: Optional[Location] = Field(default=None) rotation: Optional[Rotation] = Field(default=None) velocity: Optional[Vector3D] = Field(default=None) diff --git a/runner_sim.py b/runner_sim.py index 5ccd4ea8..676d9997 100644 --- a/runner_sim.py +++ b/runner_sim.py @@ -7,6 +7,7 @@ import argparse from misc.utils import str2bool from ROAR.agent_module.michael_pid_agent import PIDAgent +from ROAR.agent_module.forward_only_agent import ForwardOnlyAgent def main(args): @@ -19,7 +20,7 @@ def main(args): npc_agent_class=PurePursuitAgent) try: my_vehicle = carla_runner.set_carla_world() - agent = PIDAgent(vehicle=my_vehicle, + agent = ForwardOnlyAgent(vehicle=my_vehicle, agent_settings=agent_config) carla_runner.start_game_loop(agent=agent, use_manual_control=not args.auto) @@ -35,6 +36,7 @@ def main(args): '- %(message)s', datefmt="%H:%M:%S", level=logging.DEBUG) + logging.getLogger(" streaming client").setLevel(logging.WARNING) import warnings warnings.filterwarnings("ignore", module="carla")