Merge pull request #69 from parilo/adding_lidar

Adding 360 Lidars support (similar to Velodyne HDL-32E or VLP-16)

PythonClient/carla/client.py

@@ -175,16 +175,28 @@ class CarlaClient(object):
     def _iterate_sensor_data(raw_data):
         # At this point the only sensors available are images, the raw_data
         # consists of images only.
-        image_types = ['None', 'SceneFinal', 'Depth', 'SemanticSegmentation']
+        image_types = ['None', 'SceneFinal', 'Depth', 'SemanticSegmentation', 'Lidar']
         gettype = lambda id: image_types[id] if len(image_types) > id else 'Unknown'
         getval = lambda index: struct.unpack('<L', raw_data[index*4:index*4+4])[0]
+        getfloatval = lambda index: struct.unpack('<d', raw_data[index*4:index*4+8])[0]
         total_size = len(raw_data) / 4
         index = 0
         while index < total_size:
-            width = getval(index)
-            height = getval(index + 1)
-            image_type = gettype(getval(index + 2))
-            begin = index + 3
-            end = begin + width * height
-            index = end
-            yield sensor.Image(width, height, image_type, raw_data[begin*4:end*4])
+            sensor_type = gettype(getval(index + 2))
+            if sensor_type == 'Lidar':
+
+                horizontal_angle = getfloatval(index)
+                channels_count = getval(index + 3)
+                lm = sensor.LidarMeasurement(
+                    horizontal_angle, channels_count,
+                    sensor_type, raw_data[index*4:])
+                index += lm.size_in_bytes
+                yield lm
+
+            else:
+                width = getval(index)
+                height = getval(index + 1)
+                begin = index + 3
+                end = begin + width * height
+                index = end
+                yield sensor.Image(width, height, sensor_type, raw_data[begin*4:end*4])

PythonClient/carla/sensor.py

@@ -8,6 +8,8 @@
 
 
 import os
+import numpy as np
+import json
 
 
 # ==============================================================================
@@ -19,7 +21,12 @@ class Sensor(object):
     """
     Base class for sensor descriptions. Used to add sensors to CarlaSettings.
     """
-    pass
+
+    def set(self, **kwargs):
+        for key, value in kwargs.items():
+            if not hasattr(self, key):
+                raise ValueError('CarlaSettings.Camera: no key named %r' % key)
+            setattr(self, key, value)
 
 
 class Camera(Sensor):
@@ -42,12 +49,6 @@ class Camera(Sensor):
         self.CameraRotationYaw = 0
         self.set(**kwargs)
 
-    def set(self, **kwargs):
-        for key, value in kwargs.items():
-            if not hasattr(self, key):
-                raise ValueError('CarlaSettings.Camera: no key named %r' % key)
-            setattr(self, key, value)
-
     def set_image_size(self, pixels_x, pixels_y):
         self.ImageSizeX = pixels_x
         self.ImageSizeY = pixels_y
@@ -63,6 +64,51 @@ class Camera(Sensor):
         self.CameraRotationYaw = yaw
 
 
+class Lidar(Sensor):
+    """
+    Lidar description. This class can be added to a CarlaSettings object to add
+    a Lidar to the player vehicle.
+    """
+
+    def __init__(self, name, **kwargs):
+        self.LidarName = name
+        # Number of lasers
+        self.Channels = 32
+        # Measure distance
+        self.Range = 5000
+        # Points generated by all lasers per second
+        self.PointsPerSecond = 100000
+        # Lidar rotation frequency
+        self.RotationFrequency = 10
+        # Upper laser angle, counts from horizontal,
+        # positive values means above horizontal line
+        self.UpperFovLimit = 10
+        # Lower laser angle, counts from horizontal,
+        # negative values means under horizontal line
+        self.LowerFovLimit = -30
+        # wether to show debug points of laser hits in simulator
+        self.ShowDebugPoints = False
+        # Position relative to the player.
+        self.LidarPositionX = 0
+        self.LidarPositionY = 0
+        self.LidarPositionZ = 250
+        # Rotation relative to the player.
+        self.LidarRotationPitch = 0
+        self.LidarRotationRoll = 0
+        self.LidarRotationYaw = 0
+        self.set(**kwargs)
+
+    def set_position(self, x, y, z):
+        self.LidarPositionX = x
+        self.LidarPositionY = y
+        self.LidarPositionZ = z
+
+    def set_rotation(self, pitch, roll, yaw):
+        self.LidarRotationPitch = pitch
+        self.LidarRotationRoll = roll
+        self.LidarRotationYaw = yaw
+
+
 # ==============================================================================
 # -- SensorData ----------------------------------------------------------------
 # ==============================================================================
@@ -120,3 +166,62 @@ class Image(SensorData):
         if not os.path.isdir(folder):
             os.makedirs(folder)
         image.save(filename)
+
+
+class LidarMeasurement(SensorData):
+    """Data generated by a Lidar."""
+
+    def __init__(self, horizontal_angle, channels_count, lidar_type, raw_data):
+        self.horizontal_angle = horizontal_angle
+        self.channels_count = channels_count
+        self.type = lidar_type
+        self._converted_data = None
+
+        points_count_by_channel_size = channels_count * 4
+        points_count_by_channel_bytes = raw_data[4*4:4*4 + points_count_by_channel_size]
+        self.points_count_by_channel = np.frombuffer(points_count_by_channel_bytes, dtype=np.dtype('uint32'))
+
+        self.points_size = int(np.sum(self.points_count_by_channel) * 3 * 8)  # three floats X, Y, Z
+        begin = 4*4 + points_count_by_channel_size  # 4*4 is horizontal_angle, type, channels_count
+        end = begin + self.points_size
+        self.points_data = raw_data[begin:end]
+
+        self._data_size = 4*4 + points_count_by_channel_size + self.points_size
+
+    @property
+    def size_in_bytes(self):
+        return self._data_size
+
+    @property
+    def data(self):
+        """
+        Lazy initialization for data property, stores converted data in its
+        default format.
+        """
+        if self._converted_data is None:
+
+            points_in_one_channel = self.points_count_by_channel[0]
+            points = np.frombuffer(self.points_data[:self.points_size], dtype='float')
+            points = np.reshape(points, (self.channels_count, points_in_one_channel, 3))
+
+            self._converted_data = {
+                'horizontal_angle' : self.horizontal_angle,
+                'channels_count' : self.channels_count,
+                'points_count_by_channel' : self.points_count_by_channel,
+                'points' : points
+            }
+
+        return self._converted_data
+
+    def save_to_disk(self, filename):
+        """Save lidar measurements to disk"""
+        folder = os.path.dirname(filename)
+        if not os.path.isdir(folder):
+            os.makedirs(folder)
+        with open(filename, 'wt') as f:
+            f.write(json.dumps({
+                'horizontal_angle' : self.horizontal_angle,
+                'channels_count' : self.channels_count,
+                'points_count_by_channel' : self.points_count_by_channel.tolist(),
+                'points' : self._converted_data['points'].tolist()
+            }))

PythonClient/carla/settings.py

@@ -46,6 +46,7 @@ class CarlaSettings(object):
         self.randomize_weather()
         self.set(**kwargs)
         self._cameras = []
+        self._lidars = []
 
     def set(self, **kwargs):
         for key, value in kwargs.items():
@@ -69,6 +70,8 @@ class CarlaSettings(object):
         """Add a sensor to the player vehicle (see sensor.py)."""
         if isinstance(sensor, carla_sensor.Camera):
             self._cameras.append(sensor)
+        elif isinstance(sensor, carla_sensor.Lidar):
+            self._lidars.append(sensor)
         else:
             raise ValueError('Sensor not supported')
 
@@ -99,6 +102,7 @@ class CarlaSettings(object):
 
         ini.add_section(S_CAPTURE)
         ini.set(S_CAPTURE, 'Cameras', ','.join(c.CameraName for c in self._cameras))
+        ini.set(S_CAPTURE, 'Lidars', ','.join(l.LidarName for l in self._lidars))
 
         for camera in self._cameras:
             add_section(S_CAPTURE + '/' + camera.CameraName, camera, [
@@ -113,6 +117,22 @@ class CarlaSettings(object):
                 'CameraRotationRoll',
                 'CameraRotationYaw'])
 
+        for lidar in self._lidars:
+            add_section(S_CAPTURE + '/' + lidar.LidarName, lidar, [
+                'Channels',
+                'Range',
+                'PointsPerSecond',
+                'RotationFrequency',
+                'UpperFovLimit',
+                'LowerFovLimit',
+                'ShowDebugPoints',
+                'LidarPositionX',
+                'LidarPositionY',
+                'LidarPositionZ',
+                'LidarRotationPitch',
+                'LidarRotationRoll',
+                'LidarRotationYaw'])
+
         if sys.version_info >= (3, 0):
             text = io.StringIO()
         else:
@@ -128,7 +148,8 @@ def _get_sensor_names(settings):
     The settings object can be a CarlaSettings or an INI formatted string.
     """
     if isinstance(settings, CarlaSettings):
-        return [camera.CameraName for camera in settings._cameras]
+        return [camera.CameraName for camera in settings._cameras] + \
+            [lidar.LidarName for lidar in settings._lidars]
     ini = ConfigParser()
     if sys.version_info >= (3, 0):
         ini.readfp(io.StringIO(settings))

PythonClient/client_example.py

@@ -17,13 +17,13 @@ import sys
 import time
 
 from carla.client import make_carla_client
-from carla.sensor import Camera
+from carla.sensor import Camera, Lidar, LidarMeasurement
 from carla.settings import CarlaSettings
 from carla.tcp import TCPConnectionError
 from carla.util import print_over_same_line
 
 
-def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filename_format, settings_filepath):
+def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filename_format, lidar_filename_format, settings_filepath):
     # Here we will run 3 episodes with 300 frames each.
     number_of_episodes = 3
     frames_per_episode = 300
@@ -71,6 +71,19 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena
                 camera1.set_position(30, 0, 130)
                 settings.add_sensor(camera1)
 
+                lidar0 = Lidar('Lidar32')
+                lidar0.set_position(0, 0, 250)
+                lidar0.set_rotation(0, 0, 0)
+                lidar0.set(
+                    Channels = 32,
+                    Range = 5000,
+                    PointsPerSecond = 100000,
+                    RotationFrequency = 10,
+                    UpperFovLimit = 10,
+                    LowerFovLimit = -30,
+                    ShowDebugPoints = False)
+                settings.add_sensor(lidar0)
+
             else:
 
                 # Alternatively, we can load these settings from a file.
@@ -104,8 +117,12 @@ def run_carla_client(host, port, autopilot_on, save_images_to_disk, image_filena
 
                 # Save the images to disk if requested.
                 if save_images_to_disk:
-                    for name, image in sensor_data.items():
-                        image.save_to_disk(image_filename_format.format(episode, name, frame))
+                    for name, measurement in sensor_data.items():
+                        if isinstance(measurement, LidarMeasurement):
+                            measurement.data
+                            measurement.save_to_disk(lidar_filename_format.format(episode, name, frame))
+                        else:
+                            measurement.save_to_disk(image_filename_format.format(episode, name, frame))
 
                 # We can access the encoded data of a given image as numpy
                 # array using its "data" property. For instance, to get the
@@ -210,6 +227,7 @@ def main():
                 autopilot_on=args.autopilot,
                 save_images_to_disk=args.images_to_disk,
                 image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png',
+                lidar_filename_format='_lidars/episode_{:0>3d}/{:s}/lidar_{:0>5d}.json',
                 settings_filepath=args.carla_settings)
 
             print('Done.')

PythonClient/manual_control.py

@@ -84,6 +84,19 @@ def make_carla_settings():
     camera2.set_position(200, 0, 140)
     camera2.set_rotation(0.0, 0.0, 0.0)
     settings.add_sensor(camera2)
+    lidar0 = sensor.Lidar('Lidar32')
+    lidar0.set_position(0, 0, 250)
+    lidar0.set_rotation(0, 0, 0)
+    lidar0.set(
+        Channels = 32,
+        Range = 5000,
+        PointsPerSecond = 100000,
+        RotationFrequency = 10,
+        UpperFovLimit = 10,
+        LowerFovLimit = -30,
+        ShowDebugPoints = False)
+    settings.add_sensor(lidar0)
+
     return settings
 
 
@@ -166,6 +179,7 @@ class CarlaGame(object):
         self._main_image = sensor_data['CameraRGB']
         self._mini_view_image1 = sensor_data['CameraDepth']
         self._mini_view_image2 = sensor_data['CameraSemSeg']
+        self._lidar_measurement = sensor_data['Lidar32']
 
         # Print measurements every second.
         if self._timer.elapsed_seconds_since_lap() > 1.0:
@@ -284,6 +298,23 @@ class CarlaGame(object):
             self._display.blit(
                 surface, (2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
 
+        if self._lidar_measurement is not None:
+
+            lidar_data = np.array(self._lidar_measurement.data['points'][:, :, :2])
+            lidar_data /= 50.0
+            lidar_data += 100.0
+            lidar_data = np.fabs(lidar_data)
+            lidar_data = lidar_data.astype(np.int32)
+            lidar_data = np.reshape(lidar_data, (-1, 2))
+            #draw lidar
+            lidar_img_size = (200, 200, 3)
+            lidar_img = np.zeros(lidar_img_size)
+            lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
+            surface = pygame.surfarray.make_surface(
+                lidar_img
+            )
+            self._display.blit(surface, (10, 10))
+
         if self._map_view is not None:
             array = self._map_view
             array = array[:, :, :3]
@@ -301,7 +332,7 @@ class CarlaGame(object):
                         agent.vehicle.transform.location.y,
                         agent.vehicle.transform.location.z])
                     w_pos = int(agent_position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0])))
-                    h_pos =int(agent_position[1] *(new_window_width/float(self._map_shape[1])))         
+                    h_pos =int(agent_position[1] *(new_window_width/float(self._map_shape[1])))
                     pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos,h_pos), 4, 0)
 
             self._display.blit(surface, (WINDOW_WIDTH, 0))

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CapturedLidarSegment.h

@@ -0,0 +1,29 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CapturedLidarSegment.generated.h"
+
+///
+/// Lidar segment captured by tick
+///
+USTRUCT()
+struct FCapturedLidarLaserSegment
+{
+  GENERATED_USTRUCT_BODY()
+
+  UPROPERTY(VisibleAnywhere)
+  TArray<FVector> Points;
+};
+
+USTRUCT()
+struct FCapturedLidarSegment
+{
+  GENERATED_USTRUCT_BODY()
+
+  UPROPERTY(VisibleAnywhere)
+  float HorizontalAngle = 0;
+
+  UPROPERTY(VisibleAnywhere)
+  TArray<FCapturedLidarLaserSegment> LidarLasersSegments;
+};

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp

@@ -224,6 +224,10 @@ void ACarlaGameModeBase::AttachCaptureCamerasToPlayer()
   for (const auto &Item : Settings.CameraDescriptions) {
     PlayerController->AddSceneCaptureCamera(Item.Value, OverridePostProcessParameters);
   }
+
+  for (const auto &Item : Settings.LidarDescriptions) {
+    PlayerController->AddSceneCaptureLidar(Item.Value);
+  }
 }
 
 void ACarlaGameModeBase::TagActorsForSemanticSegmentation()

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.cpp

@@ -16,6 +16,7 @@ void ACarlaPlayerState::Reset()
   CollisionIntensityPedestrians = 0.0f;
   CollisionIntensityOther = 0.0f;
   Images.Empty();
+  LidarSegments.Empty();
 }
 
 void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState)
@@ -45,6 +46,7 @@ void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState)
       OtherLaneIntersectionFactor = Other->OtherLaneIntersectionFactor;
       OffRoadIntersectionFactor = Other->OffRoadIntersectionFactor;
       Images = Other->Images;
+      LidarSegments = Other->LidarSegments;
       UE_LOG(LogCarla, Log, TEXT("Copied properties of ACarlaPlayerState"));
     }
   }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaPlayerState.h

@@ -9,6 +9,7 @@
 #include "GameFramework/PlayerState.h"
 #include "AI/TrafficLightState.h"
 #include "CapturedImage.h"
+#include "CapturedLidarSegment.h"
 #include "CarlaPlayerState.generated.h"
 
 /// Current state of the player, updated every frame by ACarlaVehicleController.
@@ -201,11 +202,28 @@ public:
     return GetNumberOfImages() > 0;
   }
 
+  UFUNCTION(BlueprintCallable)
+  int32 GetNumberOfLidarsMeasurements() const
+  {
+    return LidarSegments.Num();
+  }
+
+  UFUNCTION(BlueprintCallable)
+  bool HasLidarsMeasurements() const
+  {
+    return GetNumberOfLidarsMeasurements() > 0;
+  }
+
   const TArray<FCapturedImage> &GetImages() const
   {
     return Images;
   }
 
+  const TArray<FCapturedLidarSegment> &GetLidarSegments() const
+  {
+    return LidarSegments;
+  }
+
   /// @}
   // ===========================================================================
   // -- Modifiers --------------------------------------------------------------
@@ -286,4 +304,7 @@ private:
 
   UPROPERTY(VisibleAnywhere)
   TArray<FCapturedImage> Images;
+
+  UPROPERTY(VisibleAnywhere)
+  TArray<FCapturedLidarSegment> LidarSegments;
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaServer.cpp

@@ -81,6 +81,59 @@ static void Set(carla_image &cImage, const FCapturedImage &uImage)
   }
 }
 
+struct carla_lidar_measurement_data {
+  TUniquePtr<uint32_t[]> points_count_by_channel;
+  TUniquePtr<double[]> points;
+};
+
+static void Set(
+  carla_lidar_measurement &cLidarMeasurement,
+  const FCapturedLidarSegment &uLidarSegment,
+  carla_lidar_measurement_data &cLidarMeasurementData)
+{
+
+  if (uLidarSegment.LidarLasersSegments.Num() > 0) {
+
+    cLidarMeasurement.horizontal_angle = uLidarSegment.HorizontalAngle;
+    cLidarMeasurement.channels_count = uLidarSegment.LidarLasersSegments.Num();
+
+    cLidarMeasurementData.points_count_by_channel = MakeUnique<uint32_t[]>(cLidarMeasurement.channels_count);
+    size_t total_points = 0;
+    for(int i=0; i<cLidarMeasurement.channels_count; i++)
+    {
+      size_t points_count = uLidarSegment.LidarLasersSegments[0].Points.Num();
+      cLidarMeasurementData.points_count_by_channel[i] = points_count;
+      total_points += points_count;
+    }
+    cLidarMeasurementData.points = MakeUnique<double[]>(3 * total_points);
+    size_t points_filled = 0;
+    for(int i=0; i<cLidarMeasurement.channels_count; i++)
+    {
+      size_t points_count = cLidarMeasurementData.points_count_by_channel[i];
+      auto& laser_points = uLidarSegment.LidarLasersSegments[i].Points;
+      for(int pi=0; pi<points_count; pi++)
+      {
+        cLidarMeasurementData.points[3 * (pi + points_filled)] = laser_points[pi].X;
+        cLidarMeasurementData.points[3 * (pi + points_filled) + 1] = laser_points[pi].Y;
+        cLidarMeasurementData.points[3 * (pi + points_filled) + 2] = laser_points[pi].Z;
+      }
+      points_filled += points_count;
+    }
+
+    cLidarMeasurement.points_count_by_channel = cLidarMeasurementData.points_count_by_channel.Get();
+    cLidarMeasurement.data = cLidarMeasurementData.points.Get();
+
+#ifdef CARLA_SERVER_EXTRA_LOG
+    {
+      const auto Size = uImage.BitMap.Num();
+      UE_LOG(LogCarlaServer, Log, TEXT("Sending lidar measurement %d x %d"), uLidarSegment.LidarLasersSegments.Num(), uLidarSegment.LidarLasersSegments[0].Points.Num());
+    }
+  } else {
+    UE_LOG(LogCarlaServer, Warning, TEXT("Sending empty lidar measurement"));
+#endif // CARLA_SERVER_EXTRA_LOG
+  }
+}
+
 static void SetBoxSpeedAndType(carla_agent &values, const ACharacter *Walker)
 {
   values.type = CARLA_SERVER_AGENT_PEDESTRIAN;
@@ -311,11 +364,11 @@ CarlaServer::ErrorCode CarlaServer::SendMeasurements(
   Set(player.collision_other, PlayerState.GetCollisionIntensityOther());
   Set(player.intersection_otherlane, PlayerState.GetOtherLaneIntersectionFactor());
   Set(player.intersection_offroad, PlayerState.GetOffRoadIntersectionFactor());
-  Set(player.autopilot_control.steer, PlayerState.GetSteer());
-  Set(player.autopilot_control.throttle, PlayerState.GetThrottle());
-  Set(player.autopilot_control.brake, PlayerState.GetBrake());
-  Set(player.autopilot_control.hand_brake, PlayerState.GetHandBrake());
-  Set(player.autopilot_control.reverse, PlayerState.GetCurrentGear() < 0);
+  Set(player.ai_control.steer, PlayerState.GetSteer());
+  Set(player.ai_control.throttle, PlayerState.GetThrottle());
+  Set(player.ai_control.brake, PlayerState.GetBrake());
+  Set(player.ai_control.hand_brake, PlayerState.GetHandBrake());
+  Set(player.ai_control.reverse, PlayerState.GetCurrentGear() < 0);
 
   TArray<carla_agent> Agents;
   if (bSendNonPlayerAgentsInfo) {
@@ -338,5 +391,19 @@ CarlaServer::ErrorCode CarlaServer::SendMeasurements(
     }
   }
 
-  return ParseErrorCode(carla_write_measurements(Server, values, images.Get(), NumberOfImages));
+  // Lidars.
+  auto NumberOfLidarMeasurements = PlayerState.GetNumberOfLidarsMeasurements();
+  TUniquePtr<carla_lidar_measurement[]> lidar_measurements;
+  TUniquePtr<carla_lidar_measurement_data[]> lidar_measurements_data;
+  if (NumberOfLidarMeasurements > 0) {
+    lidar_measurements = MakeUnique<carla_lidar_measurement[]>(NumberOfLidarMeasurements);
+    lidar_measurements_data = MakeUnique<carla_lidar_measurement_data[]>(NumberOfLidarMeasurements);
+    for (auto i = 0; i < NumberOfLidarMeasurements; ++i) {
+      Set(lidar_measurements[i], PlayerState.GetLidarSegments()[i], lidar_measurements_data[i]);
+    }
+  }
+
+  return ParseErrorCode(carla_write_measurements(
+    Server, values, images.Get(), lidar_measurements.Get(),
+    NumberOfImages, NumberOfLidarMeasurements));
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.cpp

@@ -8,6 +8,7 @@
 #include "CarlaVehicleController.h"
 
 #include "SceneCaptureCamera.h"
+#include "Lidar.h"
 
 #include "Components/BoxComponent.h"
 #include "EngineUtils.h"
@@ -73,6 +74,11 @@ void ACarlaVehicleController::BeginPlay()
         Image.PostProcessEffect = Camera->GetPostProcessEffect();
       }
     }
+    // Lidars
+    const auto NumberOfLidars = SceneCaptureLidars.Num();
+    if (NumberOfLidars > 0) {
+      CarlaPlayerState->LidarSegments.AddDefaulted(NumberOfLidars);
+    }
   }
 }
 
@@ -105,6 +111,13 @@ void ACarlaVehicleController::Tick(float DeltaTime)
         Image.BitMap.Empty();
       }
     }
+    // Capture lidars
+    const auto NumberOfLidars = SceneCaptureLidars.Num();
+    check(NumberOfLidars == CarlaPlayerState->LidarSegments.Num());
+    for (auto i = 0; i < NumberOfLidars; ++i) {
+      auto &LidarSegment = CarlaPlayerState->LidarSegments[i];
+      SceneCaptureLidars[i]->ReadPoints(DeltaTime, LidarSegment);
+    }
   }
 }
 
@@ -134,6 +147,22 @@ void ACarlaVehicleController::AddSceneCaptureCamera(
       *PostProcessEffect::ToString(Camera->GetPostProcessEffect()));
 }
 
+void ACarlaVehicleController::AddSceneCaptureLidar(
+    const FLidarDescription &Description)
+{
+  auto Lidar = GetWorld()->SpawnActor<ALidar>(Description.Position, Description.Rotation);
+  Lidar->Set(Description);
+  Lidar->AttachToActor(GetPawn(), FAttachmentTransformRules::KeepRelativeTransform);
+  Lidar->SetOwner(GetPawn());
+  AddTickPrerequisiteActor(Lidar);
+  SceneCaptureLidars.Add(Lidar);
+  UE_LOG(
+      LogCarla,
+      Log,
+      TEXT("Created lidar %d"),
+      SceneCaptureLidars.Num() - 1);
+}
+
 // =============================================================================
 // -- Events -------------------------------------------------------------------
 // =============================================================================

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.h

@@ -12,7 +12,9 @@
 class ACarlaHUD;
 class ACarlaPlayerState;
 class ASceneCaptureCamera;
+class ALidar;
 struct FCameraDescription;
+struct FLidarDescription;
 
 /// The CARLA player controller.
 UCLASS()
@@ -85,6 +87,9 @@ public:
       const FCameraDescription &CameraDescription,
       const FCameraPostProcessParameters *OverridePostProcessParameters);
 
+  void AddSceneCaptureLidar(
+      const FLidarDescription &LidarDescription);
+
   /// @}
   // ===========================================================================
   /// @name Events
@@ -117,6 +122,9 @@ private:
   UPROPERTY()
   TArray<ASceneCaptureCamera *> SceneCaptureCameras;
 
+  UPROPERTY()
+  TArray<ALidar *> SceneCaptureLidars;
+
   // Cast for quick access to the custom player state.
   UPROPERTY()
   ACarlaPlayerState *CarlaPlayerState;

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.cpp

@@ -0,0 +1,82 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+
+#include "Carla.h"
+#include "Lidar.h"
+#include "StaticMeshResources.h"
+
+// Sets default values
+ALidar::ALidar()
+{
+  // Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
+  PrimaryActorTick.bCanEverTick = true;
+
+  auto MeshComp = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("CamMesh0"));
+  MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName);
+  MeshComp->bHiddenInGame = true;
+  MeshComp->CastShadow = false;
+  MeshComp->PostPhysicsComponentTick.bCanEverTick = false;
+  RootComponent = MeshComp;
+
+  CreateLasers();
+}
+
+void ALidar::Set(const FLidarDescription &LidarDescription)
+{
+  Channels = LidarDescription.Channels;
+  Range = LidarDescription.Range;
+  PointsPerSecond = LidarDescription.PointsPerSecond;
+  RotationFrequency = LidarDescription.RotationFrequency;
+  UpperFovLimit = LidarDescription.UpperFovLimit;
+  LowerFovLimit = LidarDescription.LowerFovLimit;
+  ShowDebugPoints = LidarDescription.ShowDebugPoints;
+  CreateLasers();
+}
+
+void ALidar::CreateLasers()
+{
+  float dAngle = (UpperFovLimit - LowerFovLimit) / (Channels - 1);
+
+  Lasers.Empty();
+  for(int i=0; i<Channels; i++)
+  {
+    Lasers.Emplace(i, UpperFovLimit - i * dAngle);
+  }
+}
+
+// Called when the game starts or when spawned
+void ALidar::BeginPlay()
+{
+  Super::BeginPlay();
+}
+
+void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData)
+{
+  int PointsToScanWithOneLaser = int(FMath::RoundHalfFromZero(PointsPerSecond * DeltaTime / float(Channels)));
+
+  float AngleDistanceOfTick = RotationFrequency * 360 * DeltaTime;
+  float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
+
+  LidarSegmentData.LidarLasersSegments.Empty();
+
+  auto NumOfLasers = Lasers.Num();
+  LidarSegmentData.LidarLasersSegments.AddDefaulted(NumOfLasers);
+  for (int li=0; li<NumOfLasers; li++)
+  {
+    auto& Laser = Lasers[li];
+    auto& LaserPoints = LidarSegmentData.LidarLasersSegments[li].Points;
+    LaserPoints.AddDefaulted(PointsToScanWithOneLaser);
+    for (int i=0; i<PointsToScanWithOneLaser; i++)
+    {
+      Laser.Measure(
+        this,
+        CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * i,
+        LaserPoints[i],
+        ShowDebugPoints
+      );
+    }
+  }
+
+  CurrentHorizontalAngle = fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f);
+  LidarSegmentData.HorizontalAngle = CurrentHorizontalAngle;
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h

@@ -0,0 +1,71 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+#include "GameFramework/Actor.h"
+#include "LidarLaser.h"
+#include "Settings/LidarDescription.h"
+#include "Game/CapturedLidarSegment.h"
+#include "Lidar.generated.h"
+
+UCLASS()
+class CARLA_API ALidar : public AActor
+{
+  GENERATED_BODY()
+
+public:
+  // Sets default values for this actor's properties
+  ALidar();
+
+  void Set(const FLidarDescription &LidarDescription);
+
+protected:
+  // Called when the game starts or when spawned
+  virtual void BeginPlay() override;
+
+  TArray<LidarLaser> Lasers;
+  float CurrentHorizontalAngle = 0;
+
+  void CreateLasers();
+
+public:
+
+  /** Capture lidar segment points produced by DeltaTime */
+  void ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData);
+
+  /** Number of lasers */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  int Channels = 32;
+
+  /** Measure distance */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float Range = 5000;
+
+  /** Points generated by all lasers per second */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float PointsPerSecond = 56000;
+
+  /** Lidar rotation frequency */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float RotationFrequency = 10;
+
+  /**
+    Upper laser angle, counts from horizontal,
+    positive values means above horizontal line
+  */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float UpperFovLimit = 10;
+
+  /**
+    Lower laser angle, counts from horizontal,
+    negative values means under horizontal line
+  */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float LowerFovLimit = -30;
+
+  /** wether to show debug points of laser hits in simulator */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  bool ShowDebugPoints = false;
+
+};

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp

@@ -0,0 +1,66 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#include "Carla.h"
+#include "LidarLaser.h"
+#include "Lidar.h"
+#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
+#include "DrawDebugHelpers.h"
+
+int LidarLaser::GetId()
+{
+  return Id;
+}
+
+bool LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, bool Debug)
+{
+  FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, Lidar);
+  TraceParams.bTraceComplex = true;
+  TraceParams.bReturnPhysicalMaterial = false;
+
+  FHitResult HitInfo(ForceInit);
+
+  FVector LidarBodyLoc = Lidar->GetActorLocation();
+  FRotator LidarBodyRot = Lidar->GetActorRotation();
+  FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);  // float InPitch, float InYaw, float InRoll
+  FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
+    LaserRot,
+    LidarBodyRot
+  );
+  FVector EndTrace = Lidar->Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
+
+  Lidar->GetWorld()->LineTraceSingleByChannel(
+    HitInfo,
+    LidarBodyLoc,
+    EndTrace,
+    ECC_MAX,
+    TraceParams,
+    FCollisionResponseParams::DefaultResponseParam
+  );
+
+  if (HitInfo.bBlockingHit)
+  {
+    if (Debug)
+    {
+      DrawDebugPoint(
+        Lidar->GetWorld(),
+        HitInfo.ImpactPoint,
+        10,  //size
+        FColor(255,0,255),
+        false,  //persistent (never goes away)
+        0.1  //point leaves a trail on moving object
+      );
+    }
+
+    XYZ = LidarBodyLoc - HitInfo.ImpactPoint;
+    XYZ = UKismetMathLibrary::RotateAngleAxis(
+      XYZ,
+      - LidarBodyRot.Yaw + 90,
+      FVector(0, 0, 1)
+    );
+
+    return true;
+  } else {
+    XYZ = FVector(0, 0, 0);
+    return false;
+  }
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h

@@ -0,0 +1,26 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "CoreMinimal.h"
+
+class ALidar;
+
+class CARLA_API LidarLaser
+{
+public:
+
+  LidarLaser(int Id, float VerticalAngle) :
+    Id(Id),
+    VerticalAngle(VerticalAngle)
+  {}
+
+  int GetId();
+  bool Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, bool Debug = false);
+
+private:
+
+  int Id;
+  float VerticalAngle;
+
+};

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp

@@ -82,6 +82,27 @@ static bool RequestedSemanticSegmentation(const FCameraDescription &Camera)
   return (Camera.PostProcessEffect == EPostProcessEffect::SemanticSegmentation);
 }
 
+static void GetLidarDescription(
+    const MyIniFile &ConfigFile,
+    const TCHAR* Section,
+    FLidarDescription &Lidar)
+{
+  ConfigFile.GetInt(Section, TEXT("LidarPositionX"), Lidar.Position.X);
+  ConfigFile.GetInt(Section, TEXT("LidarPositionY"), Lidar.Position.Y);
+  ConfigFile.GetInt(Section, TEXT("LidarPositionZ"), Lidar.Position.Z);
+  ConfigFile.GetInt(Section, TEXT("LidarRotationPitch"), Lidar.Rotation.Pitch);
+  ConfigFile.GetInt(Section, TEXT("LidarRotationRoll"), Lidar.Rotation.Roll);
+  ConfigFile.GetInt(Section, TEXT("LidarRotationYaw"), Lidar.Rotation.Yaw);
+
+  ConfigFile.GetInt(Section, TEXT("Channels"), Lidar.Channels);
+  ConfigFile.GetFloat(Section, TEXT("Range"), Lidar.Range);
+  ConfigFile.GetFloat(Section, TEXT("PointsPerSecond"), Lidar.PointsPerSecond);
+  ConfigFile.GetFloat(Section, TEXT("RotationFrequency"), Lidar.RotationFrequency);
+  ConfigFile.GetFloat(Section, TEXT("UpperFovLimit"), Lidar.UpperFovLimit);
+  ConfigFile.GetFloat(Section, TEXT("LowerFovLimit"), Lidar.LowerFovLimit);
+  ConfigFile.GetBool(Section, TEXT("ShowDebugPoints"), Lidar.ShowDebugPoints);
+}
+
 static void LoadSettingsFromConfig(
     const MyIniFile &ConfigFile,
     UCarlaSettings &Settings,
@@ -124,6 +145,25 @@ static void LoadSettingsFromConfig(
     ValidateCameraDescription(Camera);
     Settings.bSemanticSegmentationEnabled |= RequestedSemanticSegmentation(Camera);
   }
+  // Lidars.
+  FString Lidars;
+  ConfigFile.GetString(S_CARLA_SCENECAPTURE, TEXT("Lidars"), Lidars);
+  TArray<FString> LidarNames;
+  Lidars.ParseIntoArray(LidarNames, TEXT(","), true);
+  for (FString &Name : LidarNames) {
+    FLidarDescription &Lidar = Settings.LidarDescriptions.FindOrAdd(Name);
+    GetLidarDescription(ConfigFile, S_CARLA_SCENECAPTURE, Lidar);
+
+    TArray<FString> SubSections;
+    Name.ParseIntoArray(SubSections, TEXT("/"), true);
+    check(SubSections.Num() > 0);
+    FString Section = S_CARLA_SCENECAPTURE;
+    for (FString &SubSection : SubSections) {
+      Section += TEXT("/");
+      Section += SubSection;
+      GetLidarDescription(ConfigFile, *Section, Lidar);
+    }
+  }
 }
 
 static bool GetSettingsFilePathFromCommandLine(FString &Value)
@@ -174,6 +214,7 @@ void UCarlaSettings::LoadSettingsFromString(const FString &INIFileContents)
 {
   UE_LOG(LogCarla, Log, TEXT("Loading CARLA settings from string"));
   ResetCameraDescriptions();
+  ResetLidarDescriptions();
   MyIniFile ConfigFile;
   ConfigFile.ProcessInputFileContents(INIFileContents);
   constexpr bool bLoadCarlaServerSection = false;
@@ -228,6 +269,13 @@ void UCarlaSettings::LogSettings() const
     UE_LOG(LogCarla, Log, TEXT("Camera Rotation = (%s)"), *Item.Value.Rotation.ToString());
     UE_LOG(LogCarla, Log, TEXT("Post-Processing = %s"), *PostProcessEffect::ToString(Item.Value.PostProcessEffect));
   }
+  UE_LOG(LogCarla, Log, TEXT("Added %d lidars."), LidarDescriptions.Num());
+  for (auto &Item : LidarDescriptions) {
+    UE_LOG(LogCarla, Log, TEXT("[%s/%s]"), S_CARLA_SCENECAPTURE, *Item.Key);
+    UE_LOG(LogCarla, Log, TEXT("Lidar params = ch %f range %f pts %f"), Item.Value.Channels, Item.Value.Range, Item.Value.PointsPerSecond);
+    UE_LOG(LogCarla, Log, TEXT("Lidar Position = (%s)"), *Item.Value.Position.ToString());
+    UE_LOG(LogCarla, Log, TEXT("Lidar Rotation = (%s)"), *Item.Value.Rotation.ToString());
+  }
   UE_LOG(LogCarla, Log, TEXT("================================================================================"));
 }
 
@@ -261,11 +309,17 @@ void UCarlaSettings::ResetCameraDescriptions()
   bSemanticSegmentationEnabled = false;
 }
 
+void UCarlaSettings::ResetLidarDescriptions()
+{
+  LidarDescriptions.Empty();
+}
+
 void UCarlaSettings::LoadSettingsFromFile(const FString &FilePath, const bool bLogOnFailure)
 {
   if (FPaths::FileExists(FilePath)) {
     UE_LOG(LogCarla, Log, TEXT("Loading CARLA settings from \"%s\""), *FilePath);
     ResetCameraDescriptions();
+    ResetLidarDescriptions();
     const MyIniFile ConfigFile(FilePath);
     constexpr bool bLoadCarlaServerSection = true;
     LoadSettingsFromConfig(ConfigFile, *this, bLoadCarlaServerSection);

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h

@@ -8,6 +8,7 @@
 
 #include "CameraDescription.h"
 #include "WeatherDescription.h"
+#include "LidarDescription.h"
 
 #include "UObject/NoExportTypes.h"
 #include "CarlaSettings.generated.h"
@@ -58,6 +59,8 @@ private:
 
   void ResetCameraDescriptions();
 
+  void ResetLidarDescriptions();
+
   /** File name of the settings file used to load this settings. Empty if none used. */
   UPROPERTY(Category = "CARLA Settings|Debug", VisibleAnywhere)
   FString CurrentFileName;
@@ -143,5 +146,9 @@ public:
   UPROPERTY(Category = "Scene Capture", VisibleAnywhere)
   bool bSemanticSegmentationEnabled = false;
 
+  /** Descriptions of the lidars to be attached to the player. */
+  UPROPERTY(Category = "Scene Capture", VisibleAnywhere)
+  TMap<FString, FLidarDescription> LidarDescriptions;
+
   /// @}
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h

@@ -0,0 +1,53 @@
+// Fill out your copyright notice in the Description page of Project Settings.
+
+#pragma once
+
+#include "LidarDescription.generated.h"
+
+USTRUCT()
+struct FLidarDescription
+{
+  GENERATED_USTRUCT_BODY()
+
+  /** Number of lasers */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  int Channels = 32;
+
+  /** Measure distance */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float Range = 5000;
+
+  /** Points generated by all lasers per second */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float PointsPerSecond = 56000;
+
+  /** Lidar rotation frequency */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float RotationFrequency = 10;
+
+  /**
+    Upper laser angle, counts from horizontal,
+    positive values means above horizontal line
+  */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float UpperFovLimit = 10;
+
+  /**
+    Lower laser angle, counts from horizontal,
+    negative values means under horizontal line
+  */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  float LowerFovLimit = -30;
+
+  /** wether to show debug points of laser hits in simulator */
+  UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
+  bool ShowDebugPoints = false;
+
+  /** Position relative to the player. */
+  UPROPERTY(Category = "Lidar Description", EditDefaultsOnly)
+  FVector Position = {0.0f, 0.0f, 250.0f};
+
+  /** Rotation relative to the player. */
+  UPROPERTY(Category = "Lidar Description", EditDefaultsOnly)
+  FRotator Rotation = {0.0f, 0.0f, 0.0f};
+};

Util/CarlaServer/include/carla/carla_server.h

@@ -35,6 +35,13 @@ extern "C" {
     const uint32_t *data;
   };
 
+  struct carla_lidar_measurement {
+    float horizontal_angle;
+    int channels_count;
+    const uint32_t *points_count_by_channel;
+    const double *data;
+  };
+
   struct carla_transform {
     struct carla_vector3d location;
     struct carla_vector3d orientation;
@@ -139,7 +146,7 @@ extern "C" {
     /** Percentage of the car off-road. */
     float intersection_offroad;
     /** Vehicle's AI control that would apply this frame. */
-    struct carla_control autopilot_control;
+    struct carla_control ai_control;
   };
 
   /* ======================================================================== */
@@ -255,7 +262,9 @@ extern "C" {
       CarlaServerPtr self,
       const carla_measurements &values,
       const struct carla_image *images,
-      uint32_t number_of_images);
+      const struct carla_lidar_measurement *lidar_measurements,
+      const uint32_t number_of_images,
+      const uint32_t number_of_lidar_measurements);
 
 #ifdef __cplusplus
 }

Util/CarlaServer/source/carla/server/AgentServer.h

@@ -27,11 +27,12 @@ namespace server {
 
     error_code WriteMeasurements(
         const carla_measurements &measurements,
-        const_array_view<carla_image> images) {
+        const_array_view<carla_image> images,
+        const_array_view<carla_lidar_measurement> lidar_measurements) {
       error_code ec;
       if (!_control.TryGetResult(ec)) {
         auto writer = _measurements.buffer()->MakeWriter();
-        writer->Write(measurements, images);
+        writer->Write(measurements, images, lidar_measurements);
         ec = errc::success();
       }
       return ec;

Util/CarlaServer/source/carla/server/CarlaEncoder.cpp

@@ -126,7 +126,7 @@ namespace server {
     player->set_collision_other(values.player_measurements.collision_other);
     player->set_intersection_otherlane(values.player_measurements.intersection_otherlane);
     player->set_intersection_offroad(values.player_measurements.intersection_offroad);
-    Set(player->mutable_autopilot_control(), values.player_measurements.autopilot_control);
+    Set(player->mutable_autopilot_control(), values.player_measurements.ai_control);
     // Non-player agents.
     message->clear_non_player_agents(); // we need to clear as we cache the message.
     for (auto &agent : agents(values)) {

Util/CarlaServer/source/carla/server/CarlaServer.cpp

@@ -120,7 +120,9 @@ int32_t carla_write_measurements(
       CarlaServerPtr self,
       const carla_measurements &values,
       const struct carla_image *images,
-      const uint32_t number_of_images) {
+      const struct carla_lidar_measurement *lidar_measurements,
+      const uint32_t number_of_images,
+      const uint32_t number_of_lidar_measurements) {
   CARLA_PROFILE_SCOPE(C_API, WriteMeasurements);
   auto agent = Cast(self)->GetAgentServer();
   if (agent == nullptr) {
@@ -129,6 +131,8 @@ int32_t carla_write_measurements(
   } else {
     return agent->WriteMeasurements(
         values,
-        carla::const_array_view<carla_image>(images, number_of_images)).value();
+        carla::const_array_view<carla_image>(images, number_of_images),
+        carla::const_array_view<carla_lidar_measurement>(lidar_measurements, number_of_lidar_measurements)
+      ).value();
   }
 }

Util/CarlaServer/source/carla/server/EncoderServer.h

@@ -64,7 +64,7 @@ namespace server {
       const auto string = _encoder.Encode(values.measurements());
       auto ec = _server.Write(boost::asio::buffer(string), timeout);
       if (!ec) {
-        ec = _server.Write(values.images(), timeout);
+        ec = _server.Write(values.buffer(), timeout);
       }
       return ec;
     }

Util/CarlaServer/source/carla/server/ImagesMessage.cpp

@@ -35,28 +35,25 @@ namespace server {
     return size;
   }
 
-  void ImagesMessage::Write(const_array_view<carla_image> images) {
-    const size_t buffer_size = GetSizeOfBuffer(images);
-    Reset(sizeof(uint32_t) + buffer_size); // header + buffer.
+  size_t ImagesMessage::GetSize(const_array_view<carla_image> images) {
+    return GetSizeOfBuffer(images);
+  }
 
-    auto begin = _buffer.get();
-    begin += WriteSizeToBuffer(begin, buffer_size);
+  size_t ImagesMessage::Write(
+    const_array_view<carla_image> images,
+    unsigned char *buffer
+  ) {
+    long buffer_size = GetSizeOfBuffer(images);
+
+    auto begin = buffer;
     for (const auto &image : images) {
       begin += WriteSizeToBuffer(begin, image.width);
       begin += WriteSizeToBuffer(begin, image.height);
       begin += WriteSizeToBuffer(begin, image.type);
       begin += WriteImageToBuffer(begin, image);
     }
-    DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size);
-  }
-
-  void ImagesMessage::Reset(const uint32_t count) {
-    if (_capacity < count) {
-      log_info("allocating image buffer of", count, "bytes");
-      _buffer = std::make_unique<unsigned char[]>(count);
-      _capacity = count;
-    }
-    _size = count;
+    DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
+    return buffer_size;
   }
 
 } // namespace server

Util/CarlaServer/source/carla/server/ImagesMessage.h

@@ -31,27 +31,12 @@ namespace server {
   class ImagesMessage : private NonCopyable {
   public:
 
-    /// Allocates a new buffer if the capacity is not enough to hold the images,
-    /// but it does not allocate a smaller one if the capacity is greater than
-    /// the size of the images.
-    ///
-    /// @note The expected usage of this class is to mantain a constant size
-    /// buffer of images, so memory allocation occurs only once.
-    void Write(const_array_view<carla_image> images);
+    size_t Write(
+      const_array_view<carla_image> images,
+      unsigned char *buffer
+    );
 
-    const_buffer buffer() const {
-      return boost::asio::buffer(_buffer.get(), _size);
-    }
-
-  private:
-
-    void Reset(uint32_t count);
-
-    std::unique_ptr<unsigned char[]> _buffer = nullptr;
-
-    uint32_t _size = 0u;
-
-    uint32_t _capacity = 0u;
+    size_t GetSize(const_array_view<carla_image> images);
   };
 
 } // namespace server

Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.cpp

@@ -0,0 +1,78 @@
+
+#include "carla/server/LidarMeasurementsMessage.h"
+
+#include <cstring>
+
+#include "carla/Debug.h"
+#include "carla/Logging.h"
+
+namespace carla {
+namespace server {
+
+  static size_t GetSizeOfLidarPointsCounts(const carla_lidar_measurement& lidar_measurement) {
+    return sizeof(uint32_t) * lidar_measurement.channels_count;
+  }
+
+  static size_t GetSizeOfLidarPoints(const carla_lidar_measurement& lidar_measurement) {
+    size_t total_points = 0;
+    for(int i=0; i<lidar_measurement.channels_count; i++)
+    {
+      total_points += lidar_measurement.points_count_by_channel[i];
+    }
+    return total_points * 3 * sizeof(double);
+  }
+
+  static size_t GetSizeOfBuffer(const_array_view<carla_lidar_measurement> lidar_measurements) {
+    size_t total = 0u;
+    for (const auto &lidar_measurement : lidar_measurements) {
+      total += sizeof(double); // horizontal_angle
+      total += 2 * sizeof(uint32_t); // type_of_the_message, channels_count
+      total += GetSizeOfLidarPointsCounts(lidar_measurement);
+      total += GetSizeOfLidarPoints(lidar_measurement);
+    }
+    return total;
+  }
+
+  static size_t WriteIntToBuffer(unsigned char *buffer, uint32_t size) {
+    std::memcpy(buffer, &size, sizeof(uint32_t));
+    return sizeof(uint32_t);
+  }
+
+  static size_t WriteDoubleToBuffer(unsigned char *buffer, double val) {
+    std::memcpy(buffer, &val, sizeof(double));
+    return sizeof(double);
+  }
+
+  static size_t WriteLidarMeasurementToBuffer(unsigned char *buffer, const carla_lidar_measurement &lidar_measurement) {
+    const auto points_counts_size = GetSizeOfLidarPointsCounts(lidar_measurement);
+    const auto points_size = GetSizeOfLidarPoints(lidar_measurement);
+    DEBUG_ASSERT(lidar_measurement.points_count_by_channel != nullptr);
+    DEBUG_ASSERT(lidar_measurement.data != nullptr);
+    std::memcpy(buffer, lidar_measurement.points_count_by_channel, points_counts_size);
+    std::memcpy(buffer + points_counts_size, lidar_measurement.data, points_size);
+    return points_counts_size + points_size;
+  }
+
+  size_t LidarMeasurementsMessage::GetSize(const_array_view<carla_lidar_measurement> lidar_measurements) {
+    return GetSizeOfBuffer(lidar_measurements);
+  }
+
+  size_t LidarMeasurementsMessage::Write(
+    const_array_view<carla_lidar_measurement> lidar_measurements,
+    unsigned char *buffer
+  ) {
+    long buffer_size = GetSizeOfBuffer(lidar_measurements);
+
+    auto begin = buffer;
+    for (const auto &lidar_measurement : lidar_measurements) {
+      begin += WriteDoubleToBuffer(begin, lidar_measurement.horizontal_angle);
+      begin += WriteIntToBuffer(begin, 4); // type of lidar message
+      begin += WriteIntToBuffer(begin, lidar_measurement.channels_count);
+      begin += WriteLidarMeasurementToBuffer(begin, lidar_measurement);
+    }
+    DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
+    return buffer_size;
+  }
+
+} // namespace server
+} // namespace carla

Util/CarlaServer/source/carla/server/LidarMeasurementsMessage.h

@@ -0,0 +1,27 @@
+
+#pragma once
+
+#include <cstdint>
+#include <memory>
+
+#include "carla/ArrayView.h"
+#include "carla/NonCopyable.h"
+#include "carla/server/CarlaServerAPI.h"
+#include "carla/server/ServerTraits.h"
+
+namespace carla {
+namespace server {
+
+  class LidarMeasurementsMessage : private NonCopyable {
+  public:
+
+    size_t Write(
+      const_array_view<carla_lidar_measurement> lidar_measurements,
+      unsigned char *buffer
+    );
+
+    size_t GetSize(const_array_view<carla_lidar_measurement> lidar_measurements);
+  };
+
+} // namespace server
+} // namespace carla

Util/CarlaServer/source/carla/server/MeasurementsMessage.cpp

@@ -0,0 +1,22 @@
+
+#include "carla/server/MeasurementsMessage.h"
+
+#include <cstring>
+
+#include "carla/Debug.h"
+#include "carla/Logging.h"
+
+namespace carla {
+namespace server {
+
+void MeasurementsMessage::Reset(const uint32_t count) {
+  if (_capacity < count) {
+    log_info("allocating image buffer of", count, "bytes");
+    _buffer = std::make_unique<unsigned char[]>(count);
+    _capacity = count;
+  }
+  _size = count;
+}
+
+}
+}

Util/CarlaServer/source/carla/server/MeasurementsMessage.h

@@ -10,6 +10,7 @@
 #include "carla/server/CarlaMeasurements.h"
 #include "carla/server/CarlaServerAPI.h"
 #include "carla/server/ImagesMessage.h"
+#include "carla/server/LidarMeasurementsMessage.h"
 
 namespace carla {
 namespace server {
@@ -17,26 +18,54 @@ namespace server {
   class MeasurementsMessage : private NonCopyable {
   public:
 
+    /// Allocates a new buffer if the capacity is not enough to hold the images and
+    /// lidar measurements, but it does not allocate a smaller one if the capacity is
+    /// greater than the size of the images.
+    ///
+    /// @note The expected usage of this class is to mantain a constant size
+    /// buffer of images, so memory allocation occurs only once.
     void Write(
         const carla_measurements &measurements,
-        const_array_view<carla_image> images) {
+        const_array_view<carla_image> images,
+        const_array_view<carla_lidar_measurement> lidar_measurements) {
+
       _measurements.Write(measurements);
-      _images.Write(images);
+
+      uint32_t buffer_size = _images.GetSize(images) + _lidar_measurements.GetSize(lidar_measurements);
+      Reset(sizeof(uint32_t) + buffer_size); // header + buffer
+      auto begin = _buffer.get();
+      std::memcpy(begin, &buffer_size, sizeof(uint32_t));
+      begin += sizeof(uint32_t);
+
+      begin += _images.Write(images, begin);
+      _lidar_measurements.Write(lidar_measurements, begin);
     }
 
     const carla_measurements &measurements() const {
       return _measurements.measurements();
     }
 
-    const_buffer images() const {
-      return _images.buffer();
+    const_buffer buffer() const {
+      return boost::asio::buffer(_buffer.get(), _size);
     }
 
+  protected:
+
+    void Reset(uint32_t count);
+
   private:
 
     CarlaMeasurements _measurements;
 
     ImagesMessage _images;
+
+    LidarMeasurementsMessage _lidar_measurements;
+
+    std::unique_ptr<unsigned char[]> _buffer = nullptr;
+
+    uint32_t _size = 0u;
+
+    uint32_t _capacity = 0u;
   };
 
 } // namespace server

Util/CarlaServer/source/test/Test_CarlaServer.cpp

@@ -1,6 +1,5 @@
 #include <atomic>
 #include <future>
-#include <iostream>
 
 #include <gtest/gtest.h>
 
@@ -56,6 +55,12 @@ TEST(CarlaServerAPI, SimBlocking) {
     {ImageSizeX, ImageSizeY, 1u, image0}
   };
 
+  const uint32_t points_count_by_channel[32] = {1u};
+  const double lidar_data[32 * 3] = {1};
+  const carla_lidar_measurement lidar_measurements[] = {
+    {10, 32, points_count_by_channel, lidar_data}
+  };
+
   const carla_transform start_locations[] = {
     {carla_vector3d{0.0f, 0.0f, 0.0f}, carla_vector3d{0.0f, 0.0f, 0.0f}},
     {carla_vector3d{1.0f, 0.0f, 0.0f}, carla_vector3d{1.0f, 0.0f, 0.0f}},
@@ -106,7 +111,9 @@ TEST(CarlaServerAPI, SimBlocking) {
           carla_measurements measurements;
           measurements.non_player_agents = agents_data.data();
           measurements.number_of_non_player_agents = agents_data.size();
-          auto ec = carla_write_measurements(CarlaServer, measurements, images, SIZE_OF_ARRAY(images));
+          auto ec = carla_write_measurements(
+            CarlaServer, measurements, images, lidar_measurements,
+            SIZE_OF_ARRAY(images), SIZE_OF_ARRAY(lidar_measurements));
           if (ec != S)
             break;
         }

Util/CarlaServer/source/test/Test_TCPServer.cpp

@@ -1,5 +1,3 @@
-#include <iostream>
-
 #include <future>
 
 #include <gtest/gtest.h>