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

Re enable radar and make it work with SensorSpawnerActor

This commit is contained in:
Aaron Samaniego 2024-03-15 15:40:31 +01:00 committed by Blyron
parent 36a29194bb
commit 40cd9f63e5
7 changed files with 113 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
LibCarla/source/carla/sensor/data/RadarData.h
View File

@ -70,6 +70,10 @@ namespace data {
_detections.push_back(detection);
}
const std::vector<RadarDetection>& GetDetections() const {
return _detections;
}
private:
std::vector<RadarDetection> _detections;

23
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp
View File

@ -9,6 +9,8 @@
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
#include "Kismet/KismetMathLibrary.h"
#include "Runtime/Core/Public/Async/ParallelFor.h"
#include "PhysicsEngine/PhysicsObjectExternalInterface.h"
#include <compiler/disable-ue4-macros.h>
#include <carla/geom/Math.h>
#include <carla/ros2/ROS2.h>
@ -59,6 +61,10 @@ void ARadar::SetPointsPerSecond(int NewPointsPerSecond)
RadarData.SetResolution(PointsPerSecond);
}
const carla::sensor::data::RadarData& ARadar::GetRadarData() const{
return RadarData;
}
void ARadar::BeginPlay()
{
Super::BeginPlay();
@ -81,7 +87,7 @@ void ARadar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
auto ROS2 = carla::ros2::ROS2::GetInstance();
if (ROS2->IsEnabled())
{
TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send");
TRACE_CPUPROFILER_EVENT_SCOPE_STR("ARadar::PostPhysTick ROS2 Send");
auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id();
AActor* ParentActor = GetAttachParentActor();
if (ParentActor)
@ -135,6 +141,7 @@ void ARadar::SendLineTraces(float DeltaTime)
Rays[i].Hitted = false;
}
auto LockedPhysObject = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
{
TRACE_CPUPROFILER_EVENT_SCOPE(ParallelFor);
ParallelFor(NumPoints, [&](int32 idx) {
@ -152,8 +159,7 @@ void ARadar::SendLineTraces(float DeltaTime)
MaxRy * Radius * Sin
});
// @CARLAUE5: WE CAN PROBABLY IMPROVE THE PERFORMANCE OF THIS:
const bool Hitted = GetWorld()->LineTraceSingleByChannel(
const bool Hitted = GetWorld()->ParallelLineTraceSingleByChannel(
OutHit,
RadarLocation,
EndLocation,
@ -179,7 +185,8 @@ void ARadar::SendLineTraces(float DeltaTime)
}
});
}
LockedPhysObject.Release();
// Write the detections in the output structure
for (auto& ray : Rays)
{
@ -187,10 +194,10 @@ void ARadar::SendLineTraces(float DeltaTime)
{
RadarData.WriteDetection(
{
(float)ray.RelativeVelocity,
(float)ray.AzimuthAndElevation.X,
(float)ray.AzimuthAndElevation.Y,
(float)ray.Distance
ray.RelativeVelocity,
UKismetMathLibrary::Conv_DoubleToFloat(ray.AzimuthAndElevation.X),
UKismetMathLibrary::Conv_DoubleToFloat(ray.AzimuthAndElevation.Y),
ray.Distance
});
}
}

7
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.h
View File

@ -44,6 +44,7 @@ public:
UFUNCTION(BlueprintCallable, Category = "Radar")
void SetPointsPerSecond(int NewPointsPerSecond);
const FRadarData& GetRadarData() const;
protected:
void BeginPlay() override;
@ -54,13 +55,13 @@ protected:
UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="Detection")
float Range;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "Detection")
float HorizontalFOV;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "Detection")
float VerticalFOV;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "Detection")
int PointsPerSecond;
private:

20
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Util/JsonFileManagerLibrary.cpp
View File

@ -243,3 +243,23 @@ bool UJsonFileManagerLibrary::SaveSensorJsonData(const FString& JsonFilePath,
return bWriteSuccess;
}
bool UJsonFileManagerLibrary::SaveRadarDataToJson(const FString& JsonFilePath,
const FRadarData& Rays, const FString& FrameNumber)
{
TArray<TSharedPtr<FJsonValue>> ArrayValue;
TSharedPtr<FJsonObject> JsonDataObject = GetSensorJsonData(JsonFilePath, ArrayValue);
bool bWriteSuccess = true;
for( auto& var : Rays.GetDetections() )
{
const TSharedPtr<FJsonObject> RadarDataObject = MakeShared<FJsonObject>();
RadarDataObject->SetStringField(TEXT("Frame"), FrameNumber);
RadarDataObject->SetStringField(TEXT("Velocity"), FString::SanitizeFloat(var.velocity));
RadarDataObject->SetStringField( TEXT("Azimuth"), FString::SanitizeFloat(var.azimuth));
RadarDataObject->SetStringField(TEXT("Altitude"), FString::SanitizeFloat(var.altitude));
RadarDataObject->SetStringField(TEXT("Depth"), FString::SanitizeFloat(var.depth));
bWriteSuccess &= SaveSensorJsonData(JsonFilePath, RadarDataObject, JsonDataObject, ArrayValue);
}
return bWriteSuccess;
}

6
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Util/JsonFileManagerLibrary.h
View File

@ -8,6 +8,7 @@
#include "CoreMinimal.h"
#include "Kismet/BlueprintFunctionLibrary.h"
#include "Sensor/Radar.h"
#include "JsonFileManagerLibrary.generated.h"
class FJsonValue;
@ -18,6 +19,8 @@ class CARLA_API UJsonFileManagerLibrary : public UBlueprintFunctionLibrary
{
GENERATED_BODY()
using FRadarData = carla::sensor::data::RadarData;
public:
static bool ReadStringFromFile(const FString& FilePath, FString& DataRead);
@ -37,6 +40,9 @@ public:
static bool SaveGnssDataToJson(const FString& JsonFilePath, double Altitude, double Latitude, double Longitude, const FString& FrameNumber);
// ToDo: Modify the parameter and add a reference to an FVector of a struct that contains the Velocity, Azimuth, Altitude and Depth.
static bool SaveRadarDataToJson(const FString& JsonFilePath, const FRadarData& Rays, const FString& FrameNumber);
static bool SaveLidarDataToPly(const FString& FilePath, const TArray<float>& PointArray, int ArrayElementSize);

49
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Util/SensorSpawnerActor.cpp
View File

@ -107,6 +107,11 @@ void ASensorSpawnerActor::StopRecordingSensorData()
}
}
void ASensorSpawnerActor::ToggleRadarVisibility()
{
bRadarVisibility = !bRadarVisibility;
}
const FActorDefinition* ASensorSpawnerActor::GetActorDefinitionByClass(const TSubclassOf<AActor> ActorClass) const
{
if(!ActorClass || !IsValid(CarlaEpisode))
@ -215,11 +220,15 @@ void ASensorSpawnerActor::Tick(float DeltaSeconds)
{
SaveSensorData(DeltaSeconds);
}
if(bRadarVisibility)
{
DrawRadarSensorPoints();
}
}
void ASensorSpawnerActor::SaveSensorData(float DeltaSeconds)
{
const FString FrameNumber = FString::Printf(TEXT("%lld"), UKismetSystemLibrary::GetFrameCount());
const FString FrameNumber = FString::Printf(TEXT("%lld"), FCarlaEngine::GetFrameCounter());
for(ASensor* CurrentSensor : SpawnedSensorsArray)
{
if(ASceneCaptureSensor* CaptureSensor = Cast<ASceneCaptureSensor>(CurrentSensor))
@ -253,6 +262,12 @@ void ASensorSpawnerActor::SaveSensorData(float DeltaSeconds)
const FString FilePath = FPaths::Combine(SaveImagePath, LidarSensor->GetName(), FString::Printf(TEXT("%lld"), FDateTime::Now().ToUnixTimestamp()) + "-Frame_" + FrameNumber + ".ply");
UJsonFileManagerLibrary::SaveLidarDataToPly(FilePath, LidarSensor->GetTestPointCloud(), 4);
}
if(const ARadar* RadarSensor = Cast<ARadar>(CurrentSensor))
{
const FString FilePath = FPaths::Combine(SaveImagePath, RadarSensor->GetName(), FString::Printf(TEXT("%lld"), FDateTime::Now().ToUnixTimestamp()) + "-Frame_" + FrameNumber + ".json");
UJsonFileManagerLibrary::SaveRadarDataToJson(FilePath, RadarSensor->GetRadarData(), FrameNumber);
}
}
}
@ -270,3 +285,35 @@ void ASensorSpawnerActor::AttachSensorToActor(AActor* SensorActor)
}
}
}
void ASensorSpawnerActor::DrawRadarSensorPoints() const
{
float velocity_range = VelocityRange; // m/s
for(const ASensor* CurrentSensor : SpawnedSensorsArray)
{
if(const ARadar* RadarSensor = Cast<ARadar>(CurrentSensor))
{
FRotator CurrentRot = RadarSensor->GetActorRotation();
for( auto& detect : RadarSensor->GetRadarData().GetDetections() )
{
float Azi = FMath::RadiansToDegrees(detect.azimuth);
float Alt = FMath::RadiansToDegrees(detect.altitude);
FVector FWVector( (detect.depth - 0.25f) * 100.0f, 0.f, 0.f);
FTransform SensorTransform(FRotator(CurrentRot.Pitch + Alt,
CurrentRot.Yaw + Azi,
CurrentRot.Roll));
FWVector = SensorTransform.GetRotation().RotateVector(FWVector);
float Norm_velocity = detect.velocity / velocity_range;
FColor ColorPoint;
ColorPoint.R = FMath::Clamp(Norm_velocity, 0.0f, 1.0f) * 255;
ColorPoint.G = FMath::Clamp( 1.0f - abs(Norm_velocity), 0.0f, 1.0f) * 255;
ColorPoint.B = FMath::Clamp(abs(- 1.0f - Norm_velocity), 0.0f, 1.0f) * 255;
DrawDebugPoint(GetWorld(), RadarSensor->GetActorLocation() + FWVector , PointSize, ColorPoint, false, 0.06);
}
}
}
}

18
Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Util/SensorSpawnerActor.h
View File

@ -48,6 +48,8 @@ public:
UFUNCTION(BlueprintCallable, CallInEditor, Category="Config")
void StopRecordingSensorData();
UFUNCTION(BlueprintCallable, CallInEditor, Category="Config")
void ToggleRadarVisibility();
protected:
// Called when the game starts or when spawned.
@ -95,10 +97,20 @@ protected:
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Config|SaveData")
FString SaveImagePath;
// cm/s
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Config|Radar")
float VelocityRange = 0.075;
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Config|Radar")
float PointSize = 15.0f;
private:
bool bRecordingData = false;
bool bRadarVisibility = false;
UFUNCTION()
void OnEpisodeInitialised(UCarlaEpisode* InitialisedEpisode);
@ -120,6 +132,8 @@ private:
void AttachSensorToActor(AActor* SensorActor);
void DrawRadarSensorPoints() const;
UPROPERTY()
UCarlaEpisode* CarlaEpisode;