diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.cpp index 965d41c8f..5443785bf 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.cpp @@ -246,6 +246,7 @@ void CarlaReplayer::CheckPlayAfterMapLoaded(void) void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) { double Per = 0.0f; + double DeltaTime = 0.0f; double NewTime = CurrentTime + Time; bool bFrameFound = false; bool bExitAtNextFrame = false; @@ -255,6 +256,7 @@ void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) if (NewTime >= Frame.Elapsed && NewTime < Frame.Elapsed + Frame.DurationThis) { Per = (NewTime - Frame.Elapsed) / Frame.DurationThis; + DeltaTime = Frame.DurationThis * Per; bFrameFound = true; bExitLoop = true; // UE_LOG(LogCarla, Log, TEXT("Frame %f (%f) now %f per %f"), Frame.Elapsed, Frame.Elapsed + Frame.DurationThis, NewTime, Per); @@ -277,6 +279,7 @@ void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) if (NewTime < Frame.Elapsed + Frame.DurationThis) { Per = (NewTime - Frame.Elapsed) / Frame.DurationThis; + DeltaTime = Frame.DurationThis * Per; bFrameFound = true; // UE_LOG(LogCarla, Log, TEXT("Frame %f (%f) now %f per %f"), Frame.Elapsed, Frame.Elapsed + Frame.DurationThis, NewTime, Per); } @@ -336,7 +339,7 @@ void CarlaReplayer::ProcessToTime(double Time, bool IsFirstTime) // update all positions if (Enabled && bFrameFound) { - UpdatePositions(Per); + UpdatePositions(Per, DeltaTime); } // save current time @@ -518,7 +521,7 @@ void CarlaReplayer::ProcessPositions(bool IsFirstTime) } } -void CarlaReplayer::UpdatePositions(double Per) +void CarlaReplayer::UpdatePositions(double Per, double DeltaTime) { unsigned int i; uint32_t NewFollowId = 0; @@ -551,15 +554,15 @@ void CarlaReplayer::UpdatePositions(double Per) // check if time factor is high if (TimeFactor >= 2.0) // assign first position - InterpolatePosition(PrevPos[Result->second], CurrPos[i], 0.0); + InterpolatePosition(PrevPos[Result->second], CurrPos[i], 0.0, DeltaTime); else // interpolate - InterpolatePosition(PrevPos[Result->second], CurrPos[i], Per); + InterpolatePosition(PrevPos[Result->second], CurrPos[i], Per, DeltaTime); } else { // assign last position (we don't have previous one) - InterpolatePosition(CurrPos[i], CurrPos[i], 0.0); + InterpolatePosition(CurrPos[i], CurrPos[i], 0.0, DeltaTime); } // move the camera to follow this actor if required @@ -576,10 +579,11 @@ void CarlaReplayer::UpdatePositions(double Per) void CarlaReplayer::InterpolatePosition( const CarlaRecorderPosition &Pos1, const CarlaRecorderPosition &Pos2, - double Per) + double Per, + double DeltaTime) { // call the callback - Helper.ProcessReplayerPosition(Pos1, Pos2, Per); + Helper.ProcessReplayerPosition(Pos1, Pos2, Per, DeltaTime); } // tick for the replayer diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.h index dba92a803..577949e4e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayer.h @@ -130,8 +130,8 @@ private: void ProcessStates(void); // positions - void UpdatePositions(double Per); + void UpdatePositions(double Per, double DeltaTime); - void InterpolatePosition(const CarlaRecorderPosition &Start, const CarlaRecorderPosition &End, double Per); + void InterpolatePosition(const CarlaRecorderPosition &Start, const CarlaRecorderPosition &End, double Per, double DeltaTime); }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp index 757b0babc..2c2d0b432 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp @@ -7,6 +7,8 @@ #include "CarlaReplayerHelper.h" #include "Carla/Actor/ActorView.h" #include "Carla/Actor/ActorDescription.h" +#include "Carla/Walker/WalkerController.h" +#include "Carla/Walker/WalkerControl.h" // create or reuse an actor for replaying std::pairCarlaReplayerHelper::TryToCreateReplayerActor( @@ -179,7 +181,7 @@ std::pair CarlaReplayerHelper::ProcessReplayerEventAdd( if (result.first != 0) { // disable physics - // SetActorSimulatePhysics(result.second, false); + SetActorSimulatePhysics(result.second, false); // disable autopilot SetActorAutopilot(result.second, false); } @@ -222,29 +224,40 @@ bool CarlaReplayerHelper::ProcessReplayerEventParent(uint32_t ChildId, uint32_t } // reposition actors -bool CarlaReplayerHelper::ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per) +bool CarlaReplayerHelper::ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per, double DeltaTime) { check(Episode != nullptr); AActor *Actor = Episode->GetActorRegistry().Find(Pos1.DatabaseId).GetActor(); + FVector Location; + FRotator Rotation; if (Actor && !Actor->IsPendingKill()) { // check to assign first position or interpolate between both if (Per == 0.0) { // assign position 1 - FTransform Trans(FRotator::MakeFromEuler(Pos1.Rotation), FVector(Pos1.Location), FVector(1, 1, 1)); - Actor->SetActorTransform(Trans, false, nullptr, ETeleportType::TeleportPhysics); + Location = FVector(Pos1.Location); + Rotation = FRotator::MakeFromEuler(Pos1.Rotation); + // reset velocities + // ResetVelocities(Actor); } else { // interpolate positions - FVector Location = FMath::Lerp(FVector(Pos1.Location), FVector(Pos2.Location), Per); - FRotator Rotation = FMath::Lerp(FRotator::MakeFromEuler(Pos1.Rotation), FRotator::MakeFromEuler(Pos2.Rotation), Per); - FTransform Trans(Rotation, Location, FVector(1, 1, 1)); - Actor->SetActorTransform(Trans, false, nullptr, ETeleportType::TeleportPhysics); + Location = FMath::Lerp(FVector(Pos1.Location), FVector(Pos2.Location), Per); + Rotation = FMath::Lerp(FRotator::MakeFromEuler(Pos1.Rotation), FRotator::MakeFromEuler(Pos2.Rotation), Per); + // apply new velocities + // FVector Vel((Location - FVector(Pos1.Location)) / DeltaTime); + // FVector Rot((Rotation - FRotator::MakeFromEuler(Pos1.Rotation)).Euler() / DeltaTime); + // SetVelocities(Actor, Vel, Rot); + // UE_LOG(LogCarla, Log, TEXT("Set velocities for %d at [%f,%f,%f] [%f,%f,%f]"), Pos1.DatabaseId, Vel.X, Vel.Y, Vel.Z, Rot.X, Rot.Y, Rot.Z); } - // reset velocities - ResetVelocities(Actor); + // set new transform + FTransform Trans(Rotation, Location, FVector(1, 1, 1)); + Actor->SetActorTransform(Trans, false, nullptr, ETeleportType::None); + // play walker animation + FVector Vel((Location - FVector(Pos1.Location)) / DeltaTime); + SetWalkerSpeedForAnimation(Actor, Vel); return true; } return false; @@ -253,7 +266,7 @@ bool CarlaReplayerHelper::ProcessReplayerPosition(CarlaRecorderPosition Pos1, Ca // reset velocity vectors on actor void CarlaReplayerHelper::ResetVelocities(AActor *Actor) { - if (Actor && !Actor->IsPendingKill()) + if (Actor && !Actor->IsPendingKill()) { auto RootComponent = Cast(Actor->GetRootComponent()); if (RootComponent != nullptr) @@ -266,6 +279,42 @@ void CarlaReplayerHelper::ResetVelocities(AActor *Actor) } } +// apply new velocities +void CarlaReplayerHelper::SetVelocities(AActor *Actor, FVector Linear, FVector Angular) +{ + if (Actor && !Actor->IsPendingKill()) + { + auto RootComponent = Cast(Actor->GetRootComponent()); + if (RootComponent != nullptr) + { + // velocities + RootComponent->SetPhysicsLinearVelocity(Linear, false, "None"); + RootComponent->SetPhysicsAngularVelocityInDegrees(Angular, false, "None"); + } + } +} + +// set speed of walker to force animation to play +void CarlaReplayerHelper::SetWalkerSpeedForAnimation(AActor *Actor, FVector Linear) +{ + if (Actor && !Actor->IsPendingKill()) + { + // check to set speed in walkers + auto Walker = Cast(Actor); + if (Walker) + { + auto Controller = Cast(Walker->GetController()); + if (Controller != nullptr) + { + FWalkerControl Control; + Control.Speed = Linear.Size(); + Controller->ApplyWalkerControl(Control); + // UE_LOG(LogCarla, Log, TEXT("Set Speed for %f"), Control.Speed); + } + } + } +} + // reposition the camera bool CarlaReplayerHelper::SetCameraPosition(uint32_t Id, FVector Offset, FQuat Rotation) { @@ -311,15 +360,15 @@ bool CarlaReplayerHelper::ProcessReplayerStateTrafficLight(CarlaRecorderStateTra // replay finish bool CarlaReplayerHelper::ProcessReplayerFinish(bool bApplyAutopilot) { - if (!bApplyAutopilot) - { - return false; - } - // set autopilot to all AI vehicles + // set autopilot and physics to all AI vehicles auto registry = Episode->GetActorRegistry(); for (auto ActorView : registry) { - SetActorAutopilot(ActorView, true); + // enable physics + SetActorSimulatePhysics(ActorView, true); + // autopilot + if (bApplyAutopilot) + SetActorAutopilot(ActorView, true); } return true; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.h index c2ec2a86e..9b8c48ddf 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.h @@ -35,7 +35,7 @@ public: bool ProcessReplayerEventParent(uint32_t ChildId, uint32_t ParentId); // reposition actors - bool ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per); + bool ProcessReplayerPosition(CarlaRecorderPosition Pos1, CarlaRecorderPosition Pos2, double Per, double DeltaTime); // replay event for traffic light state bool ProcessReplayerStateTrafficLight(CarlaRecorderStateTrafficLight State); @@ -64,5 +64,9 @@ private: bool SetActorAutopilot(FActorView &ActorView, bool bEnabled); // reset velocity vectors on actor void ResetVelocities(AActor *Actor); + // apply new velocities + void SetVelocities(AActor *Actor, FVector Linear, FVector Angular); + // set speed of walker to force animation to play + void SetWalkerSpeedForAnimation(AActor *Actor, FVector Linear); -}; \ No newline at end of file +};