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

Make Lidar send the measurements via SensorDataSink

This commit is contained in:
nsubiron 2018-02-21 17:01:12 +01:00
parent 4e27b134e1
commit 986a3764de
5 changed files with 200 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp
View File

@ -1,15 +1,15 @@
// Fill out your copyright notice in the Description page of Project Settings.
#include "Carla.h"
#include "Lidar.h"
#include "DrawDebugHelpers.h"
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
#include "StaticMeshResources.h"
// Sets default values
ALidar::ALidar(const FObjectInitializer& ObjectInitializer) :
Super(ObjectInitializer)
{
// 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"));
@ -18,66 +18,125 @@ ALidar::ALidar(const FObjectInitializer& ObjectInitializer) :
MeshComp->CastShadow = false;
MeshComp->PostPhysicsComponentTick.bCanEverTick = false;
RootComponent = MeshComp;
CreateLasers();
}
void ALidar::Set(const ULidarDescription &LidarDescription)
{
Channels = LidarDescription.Channels;
Range = LidarDescription.Range;
PointsPerSecond = LidarDescription.PointsPerSecond;
RotationFrequency = LidarDescription.RotationFrequency;
UpperFovLimit = LidarDescription.UpperFovLimit;
LowerFovLimit = LidarDescription.LowerFovLimit;
ShowDebugPoints = LidarDescription.ShowDebugPoints;
Description = &LidarDescription;
LidarMeasurement = FLidarMeasurement(Description->Channels);
CreateLasers();
}
void ALidar::CreateLasers()
{
float dAngle = (UpperFovLimit - LowerFovLimit) / (Channels - 1);
Lasers.Empty();
for(int i=0; i<Channels; i++)
check(Description != nullptr);
const auto NumberOfLasers = Description->Channels;
check(NumberOfLasers > 0u);
const float DeltaAngle =
(Description->UpperFovLimit - Description->LowerFovLimit) /
static_cast<float>(NumberOfLasers - 1);
LaserAngles.Empty(NumberOfLasers);
for(auto i = 0u; i < NumberOfLasers; ++i)
{
Lasers.Emplace(i, UpperFovLimit - i * dAngle);
const float VerticalAngle =
Description->UpperFovLimit - static_cast<float>(i) * DeltaAngle;
LaserAngles.Emplace(VerticalAngle);
}
}
// Called when the game starts or when spawned
void ALidar::BeginPlay()
void ALidar::Tick(const float DeltaTime)
{
Super::BeginPlay();
Super::Tick(DeltaTime);
ReadPoints(DeltaTime);
WriteSensorData(LidarMeasurement.GetView());
}
void ALidar::ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData)
void ALidar::ReadPoints(const float DeltaTime)
{
int PointsToScanWithOneLaser = int(FMath::RoundHalfFromZero(PointsPerSecond * DeltaTime / float(Channels)));
const uint32 ChannelCount = Description->Channels;
const uint32 PointsToScanWithOneLaser =
FMath::RoundHalfFromZero(
Description->PointsPerSecond * DeltaTime / float(ChannelCount));
check(PointsToScanWithOneLaser > 0);
check(ChannelCount == LaserAngles.Num());
check(Description != nullptr);
float AngleDistanceOfTick = RotationFrequency * 360 * DeltaTime;
float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
const float CurrentHorizontalAngle = LidarMeasurement.GetHorizontalAngle();
const float AngleDistanceOfTick = Description->RotationFrequency * 360.0f * DeltaTime;
const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
LidarSegmentData.LidarLasersSegments.Empty();
LidarMeasurement.Reset(ChannelCount * PointsToScanWithOneLaser);
auto NumOfLasers = Lasers.Num();
LidarSegmentData.LidarLasersSegments.AddDefaulted(NumOfLasers);
for (int li=0; li<NumOfLasers; li++)
for (auto Channel = 0u; Channel < ChannelCount; ++Channel)
{
auto& Laser = Lasers[li];
auto& LaserPoints = LidarSegmentData.LidarLasersSegments[li].Points;
LaserPoints.AddDefaulted(PointsToScanWithOneLaser);
for (int i=0; i<PointsToScanWithOneLaser; i++)
for (auto i = 0u; i < PointsToScanWithOneLaser; ++i)
{
Laser.Measure(
this,
CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * i,
LaserPoints[i],
ShowDebugPoints
);
FVector Point;
const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * i;
if (ShootLaser(Channel, Angle, Point))
{
LidarMeasurement.WritePoint(Channel, Point);
}
}
}
CurrentHorizontalAngle = fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f);
LidarSegmentData.HorizontalAngle = CurrentHorizontalAngle;
const float HorizontalAngle = std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f);
LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
}
bool ALidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ) const
{
const float VerticalAngle = LaserAngles[Channel];
FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
TraceParams.bTraceComplex = true;
TraceParams.bReturnPhysicalMaterial = false;
FHitResult HitInfo(ForceInit);
FVector LidarBodyLoc = GetActorLocation();
FRotator LidarBodyRot = GetActorRotation();
FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
LaserRot,
LidarBodyRot
);
const auto Range = Description->Range;
FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
GetWorld()->LineTraceSingleByChannel(
HitInfo,
LidarBodyLoc,
EndTrace,
ECC_MAX,
TraceParams,
FCollisionResponseParams::DefaultResponseParam
);
if (HitInfo.bBlockingHit)
{
if (Description->ShowDebugPoints)
{
DrawDebugPoint(
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 {
return false;
}
}

62
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h
View File

@ -2,70 +2,42 @@
#pragma once
#include "CoreMinimal.h"
#include "Sensor.h"
#include "LidarLaser.h"
#include "Sensor/LidarMeasurement.h"
#include "Sensor/Sensor.h"
#include "Settings/LidarDescription.h"
#include "Game/CapturedLidarSegment.h"
#include "Lidar.generated.h"
/// A ray-trace based Lidar sensor.
UCLASS()
class CARLA_API ALidar : public ASensor
{
GENERATED_BODY()
public:
// Sets default values for this actor's properties
ALidar(const FObjectInitializer& ObjectInitializer);
ALidar(const FObjectInitializer &ObjectInitializer);
void Set(const ULidarDescription &LidarDescription);
protected:
// Called when the game starts or when spawned
virtual void BeginPlay() override;
TArray<LidarLaser> Lasers;
float CurrentHorizontalAngle = 0;
virtual void Tick(float DeltaTime) override;
private:
/// Creates a Laser for each channel.
void CreateLasers();
public:
/// Updates LidarMeasurement with the points read in DeltaTime.
void ReadPoints(float DeltaTime);
/** Capture lidar segment points produced by DeltaTime */
void ReadPoints(float DeltaTime, FCapturedLidarSegment& LidarSegmentData);
/// Shoot a laser ray-trace, return whether the laser hit something.
bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point) const;
/** Number of lasers */
UPROPERTY(EditDefaultsOnly, Category = "Lidar Description")
int Channels = 32;
UPROPERTY(Category = "LiDAR", VisibleAnywhere)
const ULidarDescription *Description;
/** 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;
TArray<float> LaserAngles;
FLidarMeasurement LidarMeasurement;
};

66
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.cpp
View File

@ -1,66 +0,0 @@
// 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;
}
}

26
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.h
View File

@ -1,26 +0,0 @@
// 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;
};

84
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h Normal file
View File

@ -0,0 +1,84 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#include "Sensor/SensorDataView.h"
#include "Containers/Array.h"
/// Stores the data generated by ALidar. To be used by ALidar solely.
///
/// The header consists of an array of uint32's in the following layout
///
/// {
/// Horizontal angle (float),
/// Channel count,
/// Point count of channel 0,
/// ...
/// Point count of channel n,
/// }
///
/// The points are stored in an array of floats
///
/// {
/// X0, Y0, Z0,
/// ...
/// Xn, Yn, Zn,
/// }
///
class FLidarMeasurement {
static_assert(sizeof(float) == sizeof(uint32), "Invalid float size");
public:
explicit FLidarMeasurement(uint32 ChannelCount = 0u)
{
Header.AddDefaulted(2u + ChannelCount);
Header[1] = ChannelCount;
}
FLidarMeasurement &operator=(FLidarMeasurement &&Other)
{
Header = std::move(Other.Header);
Points = std::move(Other.Points);
return *this;
}
float GetHorizontalAngle() const
{
return reinterpret_cast<const float &>(Header[0]);
}
void SetHorizontalAngle(float HorizontalAngle)
{
Header[0] = reinterpret_cast<const uint32 &>(HorizontalAngle);
}
void Reset(uint32 TotalPointCount)
{
Points.Reset(3u * TotalPointCount);
}
void WritePoint(uint32 Channel, const FVector &Point)
{
check(Header[1] > Channel);
Header[2u + Channel] += 1u;
Points.Emplace(Point.X);
Points.Emplace(Point.Y);
Points.Emplace(Point.Z);
}
FSensorDataView GetView() const
{
return FSensorDataView(Header, Points);
}
private:
TArray<uint32> Header;
TArray<float> Points;
};