diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/BaseLidar.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/BaseLidar.cpp index bf53d224..3886d1b6 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/BaseLidar.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/BaseLidar.cpp @@ -44,6 +44,16 @@ void ABaseLidar::Scan() checkNoEntry(); } +void ABaseLidar::ScanMultiframe() +{ + checkNoEntry(); +} + +void ABaseLidar::DrawLidar() +{ + checkNoEntry(); +} + void ABaseLidar::LidarMessageUpdate(UROS2GenericMsg* TopicMessage) { checkNoEntry(); @@ -76,10 +86,16 @@ void ABaseLidar::GetData(TArray& OutHits, float& OutTime) OutTime = TimeOfLastScan; } -FLinearColor ABaseLidar::GetColorFromIntensity(const float Intensity) +FLinearColor ABaseLidar::GetColorFromIntensity(const float Intensity, const float alpha) { + // alpha doesn't seem to work with DrawPoints, so using alpha to change value (as in HSV) float NormalizedIntensity = (Intensity - IntensityMin) / (IntensityMax - IntensityMin); - return InterpolateColor(NormalizedIntensity); + FLinearColor RetColor = InterpolateColor(NormalizedIntensity); + RetColor.R *= alpha; + RetColor.G *= alpha; + RetColor.B *= alpha; + //RetColor.A = alpha; + return RetColor; } FLinearColor ABaseLidar::InterpolateColor(float x) diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/SensorLidar3D.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/SensorLidar3D.cpp index 57ef677b..90307dca 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/SensorLidar3D.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/SensorLidar3D.cpp @@ -61,11 +61,75 @@ void ASensorLidar3D::Run() TraceHandles.Init(FTraceHandle{}, NSamplesPerScan * NChannelsPerScan); #endif - GetWorld()->GetGameInstance()->GetTimerManager().SetTimer( - TimerHandle, this, &ASensorLidar3D::Scan, 1.f / static_cast(ScanFrequency), true); + if (SingleFrameScan) + { + GetWorld()->GetGameInstance()->GetTimerManager().SetTimer( + TimerHandle, this, &ASensorLidar3D::Scan, 1.f / static_cast(ScanFrequency), true); + } + else + { + // assume fixed framerate of 100FPS - need to find a way to get this information not using DeltaTime + NSteps = 1.f / (.01f * ScanFrequency); + NSamplesPerStep = NSamplesPerScan * NChannelsPerScan / NSteps; + GetWorld()->GetGameInstance()->GetTimerManager().SetTimer( + TimerHandle, this, &ASensorLidar3D::ScanMultiframe, .01f, true); + } + IsInitialized = true; } +void ASensorLidar3D::ScanMultiframe() +{ + DHAngle = FOVHorizontal / static_cast(NSamplesPerScan); + DVAngle = FOVVertical / static_cast(NChannelsPerScan); + + // complex collisions: true + FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this); + TraceParams.bReturnPhysicalMaterial = true; + + // TraceParams.bIgnoreTouches = true; + TraceParams.bTraceComplex = true; + TraceParams.bReturnFaceIndex = true; + + FVector lidarPos = GetActorLocation(); + FRotator lidarRot = GetActorRotation(); + + ParallelFor( + NSamplesPerStep, + [this, &TraceParams, &lidarPos, &lidarRot](int32 Index) + { + const int32 GlobalIndex = Index + CurrentBatch * NSamplesPerStep; + const int IdxX = GlobalIndex % NSamplesPerScan; + const int IdxY = GlobalIndex / NSamplesPerScan; + const float HAngle = StartAngle + DHAngle * IdxX; + const float VAngle = StartVerticalAngle + DVAngle * IdxY; + + FRotator laserRot(VAngle, HAngle, 0); + FRotator rot = UKismetMathLibrary::ComposeRotators(laserRot, lidarRot); + + FVector startPos = lidarPos + MinRange * UKismetMathLibrary::GetForwardVector(rot); + FVector endPos = lidarPos + MaxRange * UKismetMathLibrary::GetForwardVector(rot); + // + WithNoise * FVector(GaussianRNGPosition(Gen),GaussianRNGPosition(Gen),GaussianRNGPosition(Gen)); + + GetWorld()->LineTraceSingleByChannel( + RecordedHits[Index], startPos, endPos, ECC_Visibility, TraceParams, FCollisionResponseParams::DefaultResponseParam); + }, + false); + + if (CurrentBatch == 0) + { + TimeOfLastScan = UGameplayStatics::GetTimeSeconds(GetWorld()); + dt = 1.f / static_cast(ScanFrequency); + } + + if (ShowLidarRays) + { + DrawLidar(); + } + + CurrentBatch = (CurrentBatch + 1) % NSteps; +} + void ASensorLidar3D::Scan() { DHAngle = FOVHorizontal / static_cast(NSamplesPerScan); @@ -157,23 +221,38 @@ void ASensorLidar3D::Scan() // need to store on a structure associating hits with time? // GetROS2Data needs to get all data since the last Get? or the last within the last time interval? + if (ShowLidarRays) + { + DrawLidar(); + } +} + +void ASensorLidar3D::DrawLidar() +{ ULineBatchComponent* const LineBatcher = GetWorld()->PersistentLineBatcher; - if (LineBatcher != nullptr && ShowLidarRays) + if (LineBatcher != nullptr) { - for (auto& h : RecordedHits) + // this can be maybe parallelized? a first trial with ParallelFor crashes - need to verify that LineBatchComponent is thread-safe + for (int i=0; i 0) + h.Distance) * .01f; + + // coloring this way does not seem to work - looks as if CurrentBatch is constant over time + float alpha = 1;//(NSteps > 1) ? static_cast((i/NSamplesPerStep + NSteps-1 - CurrentBatch) % NSteps) / static_cast(NSteps) : 1; + if (h.PhysMaterial != nullptr) { + // retroreflective material if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1) { // UE_LOG(LogTemp, Warning, TEXT("retroreflective surface type hit")); // LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorReflected, 10, .5, dt); LineBatcher->DrawPoint( - h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityReflective, Distance)), 5, 10, dt); + h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityReflective, Distance), alpha), 5, 10, dt); } // non reflective material else if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default) @@ -181,7 +260,7 @@ void ASensorLidar3D::Scan() // UE_LOG(LogTemp, Warning, TEXT("default surface type hit")); // LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorHit, 10, .5, dt); LineBatcher->DrawPoint( - h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance)), 5, 10, dt); + h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance), alpha), 5, 10, dt); } // reflective material else if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2) @@ -204,7 +283,7 @@ void ASensorLidar3D::Scan() h.ImpactPoint, GetColorFromIntensity(IntensityFromDist( NormalAlignment * (IntensityReflective - IntensityNonReflective) + IntensityNonReflective, - Distance)), + Distance), alpha), 5, 10, dt); @@ -215,7 +294,7 @@ void ASensorLidar3D::Scan() // UE_LOG(LogTemp, Warning, TEXT("no physics material")); // LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorHit, 10, .5, dt); LineBatcher->DrawPoint( - h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance)), 5, 10, dt); + h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance), alpha), 5, 10, dt); } } else if (ShowLidarRayMisses) @@ -342,53 +421,104 @@ FROSPointCloud2 ASensorLidar3D::GetROS2Data() retValue.row_step = sizeof(float) * 5 * NSamplesPerScan; retValue.data.Init(0, RecordedHits.Num() * sizeof(float) * 5); - for (auto i = 0; i < RecordedHits.Num(); i++) - { - float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f; - const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen); - float Intensity = 0; - if (RecordedHits.Last(i).PhysMaterial != nullptr) + ParallelFor( + RecordedHits.Num(), + [this,&retValue](int i) { - // retroreflective material - if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1) + float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f; + const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen); + float Intensity = 0; + if (RecordedHits.Last(i).PhysMaterial != nullptr) { - Intensity = IntensityScale * IntensityReflective; - } - // non-reflective material - else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default) - { - Intensity = IntensityScale * IntensityNonReflective; + // retroreflective material + if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1) + { + Intensity = IntensityScale * IntensityReflective; + } + // non-reflective material + else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default) + { + Intensity = IntensityScale * IntensityNonReflective; + } + // reflective material + else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2) + { + FVector HitSurfaceNormal = RecordedHits.Last(i).Normal; + FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart; + RayDirection.Normalize(); + + // the dot product for this should always be between 0 and 1 + const float UnnormalizedIntensity = + FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) * + FVector::DotProduct(HitSurfaceNormal, -RayDirection), + IntensityNonReflective, + IntensityReflective); + check(UnnormalizedIntensity >= IntensityNonReflective); + check(UnnormalizedIntensity <= IntensityReflective); + Intensity = IntensityScale * UnnormalizedIntensity; + } } - // reflective material - else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2) + else { - FVector HitSurfaceNormal = RecordedHits.Last(i).Normal; - FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart; - RayDirection.Normalize(); - - // the dot product for this should always be between 0 and 1 - const float UnnormalizedIntensity = - FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) * - FVector::DotProduct(HitSurfaceNormal, -RayDirection), - IntensityNonReflective, - IntensityReflective); - check(UnnormalizedIntensity >= IntensityNonReflective); - check(UnnormalizedIntensity <= IntensityReflective); - Intensity = IntensityScale * UnnormalizedIntensity; + Intensity = 0;//std::numeric_limits::quiet_NaN(); } - } - else - { - Intensity = 0;//std::numeric_limits::quiet_NaN(); - } - FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f; - memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4); - memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4); - memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4); - memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4); - memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4); - } + FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f; + memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4); + memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4); + memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4); + memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4); + memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4); + }, + false + ); + // for (auto i = 0; i < RecordedHits.Num(); i++) + // { + // float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f; + // const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen); + // float Intensity = 0; + // if (RecordedHits.Last(i).PhysMaterial != nullptr) + // { + // // retroreflective material + // if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1) + // { + // Intensity = IntensityScale * IntensityReflective; + // } + // // non-reflective material + // else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default) + // { + // Intensity = IntensityScale * IntensityNonReflective; + // } + // // reflective material + // else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2) + // { + // FVector HitSurfaceNormal = RecordedHits.Last(i).Normal; + // FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart; + // RayDirection.Normalize(); + + // // the dot product for this should always be between 0 and 1 + // const float UnnormalizedIntensity = + // FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) * + // FVector::DotProduct(HitSurfaceNormal, -RayDirection), + // IntensityNonReflective, + // IntensityReflective); + // check(UnnormalizedIntensity >= IntensityNonReflective); + // check(UnnormalizedIntensity <= IntensityReflective); + // Intensity = IntensityScale * UnnormalizedIntensity; + // } + // } + // else + // { + // Intensity = 0;//std::numeric_limits::quiet_NaN(); + // } + + // FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f; + // memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4); + // memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4); + // memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4); + // memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4); + // memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4); + // } retValue.is_dense = true; diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/BaseLidar.h b/Source/RapyutaSimulationPlugins/Public/Sensors/BaseLidar.h index 2d77468f..a9e76c4c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/BaseLidar.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/BaseLidar.h @@ -40,6 +40,12 @@ class RAPYUTASIMULATIONPLUGINS_API ABaseLidar : public AActor UFUNCTION(BlueprintCallable) virtual void Scan(); + UFUNCTION(BlueprintCallable) + virtual void ScanMultiframe(); + + UFUNCTION() + virtual void DrawLidar(); + UFUNCTION() virtual void LidarMessageUpdate(UROS2GenericMsg* TopicMessage); @@ -131,7 +137,22 @@ class RAPYUTASIMULATIONPLUGINS_API ABaseLidar : public AActor UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Intensity") float IntensityMax = 10000.f; - FLinearColor GetColorFromIntensity(const float Intensity); + FLinearColor GetColorFromIntensity(const float Intensity, const float alpha=1); + + + + // this variable indicates the number of frames used to perform a full scan + UPROPERTY(VisibleAnywhere) + int32 NSteps = 1; + + UPROPERTY(VisibleAnywhere) + int32 NSamplesPerStep = 0; + + UPROPERTY(VisibleAnywhere) + int32 CurrentBatch = 0; + + UPROPERTY(EditAnywhere) + bool SingleFrameScan = true; protected: float dt = 0.f; diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/SensorLidar3D.h b/Source/RapyutaSimulationPlugins/Public/Sensors/SensorLidar3D.h index bdfcc8a2..d3a7553c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/SensorLidar3D.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/SensorLidar3D.h @@ -27,6 +27,10 @@ class RAPYUTASIMULATIONPLUGINS_API ASensorLidar3D : public ABaseLidar void Scan() override; + void ScanMultiframe() override; + + void DrawLidar() override; + void LidarMessageUpdate(UROS2GenericMsg* TopicMessage) override; bool Visible(AActor* TargetActor) override;