lidar done, before merge master

PythonClient/CarlaSettings.ini

@@ -42,7 +42,7 @@ Channels = 32
 ; Measure distance
 Range = 5000
 ; Points generated by all lasers per second
-PointsPerSecond = 56000
+PointsPerSecond = 100000
 ; Lidar rotation frequency
 RotationFrequency = 10
 ;  Upper laser angle, counts from horizontal,
@@ -58,6 +58,6 @@ LidarPositionX = 0
 LidarPositionY = 0
 LidarPositionZ = 250
 ; Rotation relative to the player.
-LidarRotationPitch = -10
+LidarRotationPitch = 0
 LidarRotationRoll = 0
 LidarRotationYaw = 0

PythonClient/carla/datastream.py

@@ -98,20 +98,17 @@ class DataStream(object):
         pointer +=4
         channels_count = struct.unpack('<L',imagedata[pointer:(pointer+4)])[0]
         pointer +=4
-        print('--- pointcloud {} {}'.format(horizontal_angle, channels_count))
 
         points_count_by_channel_size = channels_count * 4
         points_count_by_channel_bytes = imagedata[pointer:(pointer + points_count_by_channel_size)]
         points_count_by_channel = np.frombuffer(points_count_by_channel_bytes, dtype=np.dtype('uint32'))
         pointer += points_count_by_channel_size
-        print('--- points counts {} {}'.format(points_count_by_channel_size, points_count_by_channel))
 
         points_in_one_channel = points_count_by_channel[0]
         points_size = channels_count * points_in_one_channel * 3 * 8
         points = np.frombuffer(imagedata[pointer:(pointer + points_size)], dtype='float')
         points = np.reshape(points, (channels_count, points_in_one_channel, 3))
         pointer += points_size
-        print('--- points {} {}'.format(points_size, points))
 
         lidar_measurement = {
             'horizontal_angle' : horizontal_angle,
@@ -120,26 +117,6 @@ class DataStream(object):
             'points' : points
         }
 
-        # points = []
-        # for points_count in points_count_by_channel.tolist():
-        #     points_size = points_count * 8
-        #     points.append(np.frombuffer(image_bytes[pointer: (pointer + points_size)], dtype='float'))
-
-        # image_size = width*height*4
-        #
-        # image_bytes = imagedata[pointer:(pointer+image_size)]
-        #
-        #
-        # dt = np.dtype("uint8")
-        #
-        # new_image =np.frombuffer(image_bytes,dtype=dt)
-        #
-        # new_image = np.reshape(new_image,(self._image_y,self._image_x,4)) # TODO: make this generic
-        #
-        # pointer += image_size
-
-
-
         return lidar_measurement, im_type, pointer
 
 
@@ -183,11 +160,12 @@ class DataStream(object):
 
         meas_dict.update({'Labels':[]})
 
+        meas_dict.update({'Lidars':[]})
+
 
         pointer = 0
         while pointer < len(imagedata):
             im_type = self._read_data_type(imagedata, pointer)
-            print('--- image type: {}'.format(im_type))
             if im_type == 0:
 
                 image,im_type,pointer = self._read_image(imagedata,pointer)
@@ -210,8 +188,8 @@ class DataStream(object):
                 logging.debug("RECEIVED scene_seg")
             if im_type == 10:
 
-                image,im_type,pointer = self._read_lidar_measurement(imagedata,pointer)
+                lidar_measurement,im_type,pointer = self._read_lidar_measurement(imagedata,pointer)
-                # meas_dict['Labels'].append(image)
+                meas_dict['Lidars'].append(lidar_measurement)
                 logging.debug("RECEIVED lidar_measurement")
 
         meas_dict.update({'WallTime':measurements.platform_timestamp})

PythonClient/carla_manual_control.py

@@ -183,6 +183,7 @@ class App(object):
         self.img_vec = measurements['BGRA']
         self.depth_vec = measurements['Depth']
         self.labels_vec = measurements['Labels']
+        self.lidars_vec = measurements['Lidars']
 
         if time.time() - self.prev_time > 1.:
             message = 'Step {step} ({fps:.1f} FPS): '
@@ -252,6 +253,26 @@ class App(object):
             self._display_surf.blit(surface, (x_pos, auxImgYPos))
             x_pos += f
 
+        if self.lidars_vec:
+
+            lidar_data = np.array(self.lidars_vec[0]['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
+            )
+            # surface = pygame.transform.scale(surface, (200, 200))
+            self._display_surf.blit(surface, (10, 10))
+
+
+
         pygame.display.flip()
 
     def on_cleanup(self):

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

@@ -92,30 +92,6 @@ static void Set(
   carla_lidar_measurement_data &cLidarMeasurementData)
 {
 
-  cLidarMeasurement.horizontal_angle = 0;
-  cLidarMeasurement.channels_count = 32;
-  cLidarMeasurementData.points_count_by_channel = MakeUnique<uint32_t[]>(cLidarMeasurement.channels_count);
-  for(int i=0; i<cLidarMeasurement.channels_count; i++)
-  {
-    cLidarMeasurementData.points_count_by_channel[i] = 2;
-  }
-  cLidarMeasurementData.points = MakeUnique<double[]>(3 * 32 * 2);
-  size_t points_filled = 0;
-  for(int i=0; i<cLidarMeasurement.channels_count; i++)
-  {
-    size_t points_count = cLidarMeasurementData.points_count_by_channel[i];
-    for(int pi=0; pi<points_count; pi++)
-    {
-      cLidarMeasurementData.points[3 * pi + 3 * points_filled] = 1 + 3 * pi;
-      cLidarMeasurementData.points[3 * pi + 1 + 3 * points_filled] = 2 + 3 * pi;
-      cLidarMeasurementData.points[3 * pi + 2 + 3 * points_filled] = 3 + 3 * pi;
-    }
-    points_filled += points_count;
-  }
-  cLidarMeasurement.points_count_by_channel = cLidarMeasurementData.points_count_by_channel.Get();
-  cLidarMeasurement.data = cLidarMeasurementData.points.Get();
-  return;
-
   if (uLidarSegment.LidarLasersSegments.Num() > 0) {
 
     cLidarMeasurement.horizontal_angle = uLidarSegment.HorizontalAngle;
@@ -130,16 +106,18 @@ static void Set(
       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[pi] = laser_points[pi].X;
+        cLidarMeasurementData.points[3 * (pi + points_filled)] = laser_points[pi].X;
-        cLidarMeasurementData.points[pi + 1] = laser_points[pi].Y;
+        cLidarMeasurementData.points[3 * (pi + points_filled) + 1] = laser_points[pi].Y;
-        cLidarMeasurementData.points[pi + 2] = laser_points[pi].Z;
+        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();

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

@@ -23,22 +23,6 @@ ALidar::ALidar()
 
 void ALidar::Set(const FLidarDescription &LidarDescription)
 {
-  // UE_LOG(LogCarla, Log, TEXT("--- Lidar settings --------------------------"));
-  // UE_LOG(LogCarla, Log, TEXT("pos x %f"), LidarDescription.Position.X);
-  // UE_LOG(LogCarla, Log, TEXT("pos y %f"), LidarDescription.Position.Y);
-  // UE_LOG(LogCarla, Log, TEXT("pos z %f"), LidarDescription.Position.Z);
-  // UE_LOG(LogCarla, Log, TEXT("rot p %f"), LidarDescription.Rotation.Pitch);
-  // UE_LOG(LogCarla, Log, TEXT("rot r %f"), LidarDescription.Rotation.Roll);
-  // UE_LOG(LogCarla, Log, TEXT("rot y %f"), LidarDescription.Rotation.Yaw);
-  //
-  // UE_LOG(LogCarla, Log, TEXT("ch %d"), LidarDescription.Channels);
-  // UE_LOG(LogCarla, Log, TEXT("r %f"), LidarDescription.Range);
-  // UE_LOG(LogCarla, Log, TEXT("pts %f"), LidarDescription.PointsPerSecond);
-  // UE_LOG(LogCarla, Log, TEXT("freq %f"), LidarDescription.RotationFrequency);
-  // UE_LOG(LogCarla, Log, TEXT("upper l %f"), LidarDescription.UpperFovLimit);
-  // UE_LOG(LogCarla, Log, TEXT("lower l %f"), LidarDescription.LowerFovLimit);
-  // UE_LOG(LogCarla, Log, TEXT("d %d"), LidarDescription.ShowDebugPoints?1:0);
-
   Channels = LidarDescription.Channels;
 	Range = LidarDescription.Range;
 	PointsPerSecond = LidarDescription.PointsPerSecond;
@@ -66,25 +50,11 @@ void ALidar::BeginPlay()
 	Super::BeginPlay();
 }
 
-// Called every frame
-// void ALidar::Tick(float DeltaTime)
-// {
-// 	Super::Tick(DeltaTime);
-// }
-
 void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData)
 {
-    // UE_LOG(LogCarla, Log, TEXT("--- Lidar tick %d"), ShowDebugPoints?1:0);
-    // FVector LidarBodyLoc = RootComponent->GetComponentLocation();
-    // UE_LOG(LogCarla, Log, TEXT("--- location: %f %f %f"), lidar_body_loc.X, lidar_body_loc.Y, lidar_body_loc.Z);
-    // UE_LOG(LogCarla, Log, TEXT("--- actor rotation: %s"), *GetActorRotation().ToString());
-    // UE_LOG(LogCarla, Log, TEXT("---  root rotation: %s"), *RootComponent->GetComponentRotation().ToString());
-
   	int PointsToScanWithOneLaser = int(FMath::RoundHalfFromZero(PointsPerSecond * DeltaTime / float(Channels)));
-  	// float HorizontalAngle = CurrentHorizontalAngle;
 
   	float AngleDistanceOfTick = RotationFrequency * 360 * DeltaTime;
-  	// PrintString(FString::Printf(TEXT("tick %f %f %d"), DeltaTime, angle_distance_of_tick, points_to_scan_with_one_laser));
   	float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
 
     LidarSegmentData.LidarLasersSegments.Empty();
@@ -107,7 +77,6 @@ void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData
   		}
   	}
 
-  	// lidar_body_->AddLocalRotation(FRotator(0, angle_distance_of_tick, 0), false);
+  	CurrentHorizontalAngle = fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f);
-  	CurrentHorizontalAngle += AngleDistanceOfTick;
     LidarSegmentData.HorizontalAngle = CurrentHorizontalAngle;
 }

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

@@ -30,8 +30,6 @@ protected:
   void CreateLasers();
 
 public:
-	// Called every frame
-	// virtual void Tick(float DeltaTime) override;
 
   /** Capture lidar segment points produced by DeltaTime */
   void ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData);

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

@@ -21,22 +21,18 @@ bool LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, boo
 
 	FVector LidarBodyLoc = Lidar->GetActorLocation();
   FRotator LidarBodyRot = Lidar->GetActorRotation();
-	FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);
+	FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);  // float InPitch, float InYaw, float InRoll
-  // float InPitch, float InYaw, float InRoll
   FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
     LaserRot,
-    // UKismetMathLibrary::ComposeRotators(
+    LidarBodyRot
-    //   FRotator(25, 0, 0),
+  );
-      LidarBodyRot
-    // )
-  ); //up, no change, no change
 	FVector EndTrace = Lidar->Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
 
 	Lidar->GetWorld()->LineTraceSingleByChannel(
 		HitInfo,
 		LidarBodyLoc,
 		EndTrace,
-		ECC_Visibility,
+		ECC_MAX,
 		TraceParams,
 		FCollisionResponseParams::DefaultResponseParam
 	);
@@ -56,9 +52,15 @@ bool LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, FVector& XYZ, boo
     }
 
     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

@@ -6,9 +6,6 @@
 
 class ALidar;
 
-/**
- *
- */
 class CARLA_API LidarLaser
 {
 public:

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

@@ -51,20 +51,3 @@ struct FLidarDescription
   UPROPERTY(Category = "Lidar Description", EditDefaultsOnly)
   FRotator Rotation = {0.0f, 0.0f, 0.0f};
 };
-
-// Parameters of known lidars
-
-// Velodyne HDL-32E
-// +/- 2 cm accuracy
-// 32 Channels
-// 80m-100m Range
-// 700,000 Points per Second
-// 360° Horizontal FOV
-// +10° to -30° Vertical FOV
-
-// Velodyne VLP-16
-// 16 Channels
-// 100m Range
-// 300,000 Points per Second
-// 360° Horizontal FOV
-// +/- 15° Vertical FOV

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

@@ -132,6 +132,7 @@ int32_t carla_write_measurements(
     return agent->WriteMeasurements(
         values,
         carla::const_array_view<carla_image>(images, number_of_images),
-        carla::const_array_view<carla_lidar_measurement>(lidar_measurements, number_of_lidar_measurements)).value();
+        carla::const_array_view<carla_lidar_measurement>(lidar_measurements, number_of_lidar_measurements)
+      ).value();
   }
 }

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

@@ -44,11 +44,8 @@ namespace server {
     unsigned char *buffer
   ) {
     long buffer_size = GetSizeOfBuffer(images);
-    // Reset(sizeof(uint32_t) + buffer_size); // header + buffer.
 
     auto begin = buffer;
-    // auto begin = _buffer.get();
-    // begin += WriteSizeToBuffer(begin, buffer_size);
     for (const auto &image : images) {
       begin += WriteSizeToBuffer(begin, image.width);
       begin += WriteSizeToBuffer(begin, image.height);
@@ -56,18 +53,8 @@ namespace server {
       begin += WriteImageToBuffer(begin, image);
     }
     DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
-    // DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size);
     return buffer_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;
-  // }
-
 } // namespace server
 } // namespace carla

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

@@ -31,32 +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.
     size_t Write(
       const_array_view<carla_image> images,
       unsigned char *buffer
     );
 
     size_t GetSize(const_array_view<carla_image> images);
-
-    // 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;
   };
 
 } // namespace server

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

@@ -2,7 +2,6 @@
 #include "carla/server/LidarMeasurementsMessage.h"
 
 #include <cstring>
-#include <iostream>
 
 #include "carla/Debug.h"
 #include "carla/Logging.h"
@@ -46,9 +45,7 @@ namespace server {
 
   static size_t WriteLidarMeasurementToBuffer(unsigned char *buffer, const carla_lidar_measurement &lidar_measurement) {
     const auto points_counts_size = GetSizeOfLidarPointsCounts(lidar_measurement);
-    std::cout << "--- points_counts_size: " << points_counts_size << std::endl;
     const auto points_size = GetSizeOfLidarPoints(lidar_measurement);
-    std::cout << "--- points_size: " << points_size << std::endl;
     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);
@@ -65,12 +62,8 @@ namespace server {
     unsigned char *buffer
   ) {
     long buffer_size = GetSizeOfBuffer(lidar_measurements);
-    // Reset(sizeof(uint32_t) + buffer_size); // header + buffer
-    std::cout << "--- buffer size: " << buffer_size << std::endl;
 
     auto begin = buffer;
-    // auto begin = _buffer.get();
-    // begin += WriteIntToBuffer(begin, buffer_size);
     for (const auto &lidar_measurement : lidar_measurements) {
       begin += WriteDoubleToBuffer(begin, lidar_measurement.horizontal_angle);
       begin += WriteIntToBuffer(begin, 10); // type of lidar message
@@ -78,18 +71,8 @@ namespace server {
       begin += WriteLidarMeasurementToBuffer(begin, lidar_measurement);
     }
     DEBUG_ASSERT(std::distance(buffer, begin) == buffer_size);
-    // DEBUG_ASSERT(std::distance(_buffer.get(), begin) == _size);
     return buffer_size;
   }
 
-  // void LidarMeasurementsMessage::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;
-  // }
-
 } // namespace server
 } // namespace carla

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

@@ -12,46 +12,15 @@
 namespace carla {
 namespace server {
 
-  /// Encodes the given images as binary array to be sent to the client.
-  ///
-  /// The message consists of an array of uint32's in the following layout
-  ///
-  ///    {
-  ///      total size,
-  ///      width, height, type, color[0], color[1],...,     <- first image
-  ///      width, height, type, color[0], color[1],...,     <- second image
-  ///      ...
-  ///    }
-  ///
   class LidarMeasurementsMessage : 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.
     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);
-
-    // 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;
   };
 
 } // namespace server

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

@@ -18,6 +18,12 @@ 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,
@@ -43,14 +49,6 @@ namespace server {
       return boost::asio::buffer(_buffer.get(), _size);
     }
 
-    // const_buffer images() const {
-    //   return _images.buffer();
-    // }
-
-    // const_buffer lidar_measurements() const {
-    //   return _lidar_measurements.buffer();
-    // }
-
   protected:
 
     void Reset(uint32_t count);

Util/CarlaServer/source/test/Test_CarlaServer.cpp

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

Util/CarlaServer/source/test/Test_TCPServer.cpp

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