working on adding lidar

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

@@ -0,0 +1,31 @@
+// 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 FCapturedLidarSegment
+{
+  GENERATED_USTRUCT_BODY()
+
+  UPROPERTY(VisibleAnywhere)
+  float horizontal = 0u;
+
+
+
+  UPROPERTY(VisibleAnywhere)
+  uint32 SizeX = 0u;
+
+  UPROPERTY(VisibleAnywhere)
+  uint32 SizeY = 0u;
+
+  UPROPERTY(VisibleAnywhere)
+  EPostProcessEffect PostProcessEffect = EPostProcessEffect::INVALID;
+
+  UPROPERTY(VisibleAnywhere)
+  TArray<FColor> BitMap;
+};

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/CarlaVehicleController.cpp

@@ -8,6 +8,7 @@
 #include "CarlaVehicleController.h"
 
 #include "SceneCaptureCamera.h"
+#include "Lidar.h"
 
 #include "Components/BoxComponent.h"
 #include "EngineUtils.h"
@@ -134,6 +135,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,96 @@
+// 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)
+{
+  // 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;
+	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();
+}
+
+// Called every frame
+void ALidar::Tick(float DeltaTime)
+{
+	Super::Tick(DeltaTime);
+
+  // 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);
+
+	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;
+
+	for (auto& Laser : Lasers)
+	{
+		for (int i=0; i<PointsToScanWithOneLaser; i++)
+		{
+			float Distance = Laser.Measure(this, CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * i, ShowDebugPoints);
+		}
+	}
+
+	// lidar_body_->AddLocalRotation(FRotator(0, angle_distance_of_tick, 0), false);
+	CurrentHorizontalAngle += AngleDistanceOfTick;
+
+}

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

@@ -0,0 +1,69 @@
+// 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 "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:
+	// Called every frame
+	virtual void Tick(float DeltaTime) override;
+
+  /** 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,54 @@
+// 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;
+}
+
+float LidarLaser::Measure(ALidar* Lidar, float HorizontalAngle, 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);
+	FVector EndTrace = Lidar->Range * UKismetMathLibrary::GetForwardVector(LidarBodyRot + LaserRot) + LidarBodyLoc;
+
+	Lidar->GetWorld()->LineTraceSingleByChannel(
+		HitInfo,
+		LidarBodyLoc,
+		EndTrace,
+		ECC_Visibility,
+		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
+  		);
+    }
+
+		return (LidarBodyLoc - HitInfo.ImpactPoint).Size();
+	} else {
+		return -1;
+	}
+}

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

@@ -0,0 +1,29 @@
+// 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();
+	float Measure(ALidar* Lidar, float HorizontalAngle, 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)
@@ -171,6 +211,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;
@@ -225,6 +266,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("================================================================================"));
 }
 
@@ -258,11 +306,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,70 @@
+// 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};
+};
+
+// 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