From 986a3764debcaf42339612ac95d7550e8fb50ea6 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Wed, 21 Feb 2018 17:01:12 +0100 Subject: [PATCH] Make Lidar send the measurements via SensorDataSink --- .../Carla/Source/Carla/Sensor/Lidar.cpp | 139 +++++++++++++----- .../Plugins/Carla/Source/Carla/Sensor/Lidar.h | 62 +++----- .../Carla/Source/Carla/Sensor/LidarLaser.cpp | 66 --------- .../Carla/Source/Carla/Sensor/LidarLaser.h | 26 ---- .../Source/Carla/Sensor/LidarMeasurement.h | 84 +++++++++++ 5 files changed, 200 insertions(+), 177 deletions(-) delete mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.cpp delete mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp index f37c2779c..3a20abbd1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.cpp @@ -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(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; iChannels; + check(NumberOfLasers > 0u); + const float DeltaAngle = + (Description->UpperFovLimit - Description->LowerFovLimit) / + static_cast(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(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; liRange; + 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; + } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h index 7725f0464..319e9d066 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Lidar.h @@ -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 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 LaserAngles; + FLidarMeasurement LidarMeasurement; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.cpp deleted file mode 100644 index 9710a2f4d..000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.cpp +++ /dev/null @@ -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; - } -} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.h deleted file mode 100644 index 6ddb842bd..000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarLaser.h +++ /dev/null @@ -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; - -}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h new file mode 100644 index 000000000..8f8584004 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarMeasurement.h @@ -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 . + +#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(Header[0]); + } + + void SetHorizontalAngle(float HorizontalAngle) + { + Header[0] = reinterpret_cast(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 Header; + + TArray Points; +};