lidar sensor added

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(

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": {

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")

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),
+        )
+    )
+
+

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)

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")