p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

working on adding lidar

This commit is contained in:
Anton Pechenko 2017-11-28 18:08:05 +03:00
parent eeac538116
commit 5f27d8b7be
11 changed files with 439 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CapturedLidarSegment.h Normal file
View File

@ -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;
};

4
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp
View File

@ -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()

17
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.cpp
View File

@ -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 -------------------------------------------------------------------
// =============================================================================

8
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaVehicleController.h
View File

@ -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;

96
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.cpp Normal file
View File

@ -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;
}

69
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Lidar.h Normal file
View File

@ -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;
};

54
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.cpp Normal file
View File

@ -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;
}
}

29
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/LidarLaser.h Normal file
View File

@ -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;
};

54
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.cpp
View File

@ -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);

7
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettings.h
View File

@ -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;
/// @}
};

70
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/LidarDescription.h Normal file
View File

@ -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