diff --git a/LibCarla/source/carla/client/detail/Episode.cpp b/LibCarla/source/carla/client/detail/Episode.cpp index e5584655b..c6dfa09bf 100644 --- a/LibCarla/source/carla/client/detail/Episode.cpp +++ b/LibCarla/source/carla/client/detail/Episode.cpp @@ -43,15 +43,14 @@ namespace detail { auto self = weak.lock(); if (self != nullptr) { auto data = sensor::Deserializer::Deserialize(std::move(buffer)); - const auto &raw_data = CastData(*data); - std::shared_ptr next; - std::shared_ptr prev = self->GetState(); + auto next = std::make_shared(CastData(*data)); + auto prev = self->GetState(); do { - if (prev->GetFrameCount() >= raw_data.GetFrameNumber()) { + if (prev->GetFrameCount() >= next->GetFrameCount()) { + self->_on_tick_callbacks.Call(next->GetTimestamp()); return; } - next = prev->DeriveNextStep(raw_data); } while (!self->_state.compare_exchange(&prev, next)); if (next->GetEpisodeId() != prev->GetEpisodeId()) { diff --git a/LibCarla/source/carla/client/detail/EpisodeState.cpp b/LibCarla/source/carla/client/detail/EpisodeState.cpp index b6eb9c426..f8a7a43e7 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.cpp +++ b/LibCarla/source/carla/client/detail/EpisodeState.cpp @@ -10,38 +10,26 @@ namespace carla { namespace client { namespace detail { - static auto DeriveAcceleration( - double delta_seconds, - const geom::Vector3D &v0, - const geom::Vector3D &v1) { - /// @todo add methods to Vector3D for scalar multiplication. - auto acc = v1 - v0; - acc.x /= delta_seconds; - acc.y /= delta_seconds; - acc.z /= delta_seconds; - return acc; - } - - std::shared_ptr EpisodeState::DeriveNextStep( - const sensor::data::RawEpisodeState &state) const { - auto next = std::make_shared(state.GetEpisodeId()); - next->_timestamp.frame_count = state.GetFrameNumber(); - next->_timestamp.elapsed_seconds = state.GetGameTimeStamp(); - next->_timestamp.platform_timestamp = state.GetPlatformTimeStamp(); - next->_timestamp.delta_seconds = next->_timestamp.elapsed_seconds - _timestamp.elapsed_seconds; - next->_actors.reserve(state.size()); + EpisodeState::EpisodeState(const sensor::data::RawEpisodeState &state) + : _episode_id(state.GetEpisodeId()), + _timestamp( + state.GetFrameNumber(), + state.GetGameTimeStamp(), + state.GetDeltaSeconds(), + state.GetPlatformTimeStamp()) { + _actors.reserve(state.size()); for (auto &&actor : state) { - auto acceleration = DeriveAcceleration( - next->_timestamp.delta_seconds, - GetActorState(actor.id).velocity, - actor.velocity); DEBUG_ONLY(auto result = ) - next->_actors.emplace( + _actors.emplace( actor.id, - ActorState{actor.transform, actor.velocity, actor.angular_velocity, acceleration, actor.state}); + ActorState{ + actor.transform, + actor.velocity, + actor.angular_velocity, + actor.acceleration, + actor.state}); DEBUG_ASSERT(result.second); } - return next; } } // namespace detail diff --git a/LibCarla/source/carla/client/detail/EpisodeState.h b/LibCarla/source/carla/client/detail/EpisodeState.h index abf7db792..9b092af46 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.h +++ b/LibCarla/source/carla/client/detail/EpisodeState.h @@ -36,6 +36,8 @@ namespace detail { explicit EpisodeState(uint64_t episode_id) : _episode_id(episode_id) {} + explicit EpisodeState(const sensor::data::RawEpisodeState &state); + auto GetEpisodeId() const { return _episode_id; } @@ -65,14 +67,11 @@ namespace detail { iterator::make_map_keys_iterator(_actors.end())); } - std::shared_ptr DeriveNextStep( - const sensor::data::RawEpisodeState &state) const; - private: const uint64_t _episode_id; - Timestamp _timestamp; + const Timestamp _timestamp; std::unordered_map _actors; }; diff --git a/LibCarla/source/carla/sensor/data/ActorDynamicState.h b/LibCarla/source/carla/sensor/data/ActorDynamicState.h index d02a8bf36..c5ebb4b81 100644 --- a/LibCarla/source/carla/sensor/data/ActorDynamicState.h +++ b/LibCarla/source/carla/sensor/data/ActorDynamicState.h @@ -117,6 +117,8 @@ namespace detail { geom::Vector3D angular_velocity; + geom::Vector3D acceleration; + union TypeDependentState { detail::TrafficLightData traffic_light_data; detail::VehicleData vehicle_data; @@ -127,7 +129,7 @@ namespace detail { #pragma pack(pop) static_assert( - sizeof(ActorDynamicState) == 13u * sizeof(uint32_t) + sizeof(detail::VehicleData), + sizeof(ActorDynamicState) == 16u * sizeof(uint32_t) + sizeof(detail::VehicleData), "Invalid ActorDynamicState size!"); } // namespace data diff --git a/LibCarla/source/carla/sensor/data/RawEpisodeState.h b/LibCarla/source/carla/sensor/data/RawEpisodeState.h index 07b322dca..caa938561 100644 --- a/LibCarla/source/carla/sensor/data/RawEpisodeState.h +++ b/LibCarla/source/carla/sensor/data/RawEpisodeState.h @@ -53,6 +53,11 @@ namespace data { double GetPlatformTimeStamp() const { return GetHeader().platform_timestamp; } + + /// Simulated seconds elapsed since previous frame. + double GetDeltaSeconds() const { + return GetHeader().delta_seconds; + } }; } // namespace data diff --git a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h index eea19c5aa..0078f401a 100644 --- a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h +++ b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h @@ -32,6 +32,7 @@ namespace s11n { uint64_t episode_id; double game_timestamp; double platform_timestamp; + float delta_seconds; }; #pragma pack(pop) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h index 8f19bec56..9f407ea59 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h @@ -25,4 +25,7 @@ public: FBoundingBox BoundingBox; carla::rpc::Actor SerializedData; + + /// @todo To be used solely by the FWorldObserver. + mutable FVector Velocity = {0.0f, 0.0f, 0.0f}; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp index f446ceb83..396ebce51 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp @@ -69,7 +69,7 @@ void FCarlaEngine::OnPreTick(ELevelTick TickType, float DeltaSeconds) if ((TickType == ELevelTick::LEVELTICK_All) && (CurrentEpisode != nullptr)) { CurrentEpisode->TickTimers(DeltaSeconds); - WorldObserver.BroadcastTick(*CurrentEpisode); + WorldObserver.BroadcastTick(*CurrentEpisode, DeltaSeconds); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp index 1c83161bf..31803e64c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp @@ -78,9 +78,31 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg return state; } +static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor) +{ + const auto RootComponent = Cast(Actor.GetRootComponent()); + const FVector AngularVelocity = + RootComponent != nullptr ? + RootComponent->GetPhysicsAngularVelocityInDegrees() : + FVector{0.0f, 0.0f, 0.0f}; + return {AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z}; +} + +static carla::geom::Vector3D FWorldObserver_GetAcceleration( + const FActorView &View, + const FVector &Velocity, + const float DeltaSeconds) +{ + FVector &PreviousVelocity = View.GetActorInfo()->Velocity; + const FVector Acceleration = (Velocity - PreviousVelocity) / DeltaSeconds; + PreviousVelocity = Velocity; + return {Acceleration.X, Acceleration.Y, Acceleration.Z}; +} + static carla::Buffer FWorldObserver_Serialize( carla::Buffer buffer, - const UCarlaEpisode &Episode) + const UCarlaEpisode &Episode, + float DeltaSeconds) { using Serializer = carla::sensor::s11n::EpisodeStateSerializer; using ActorDynamicState = carla::sensor::data::ActorDynamicState; @@ -101,6 +123,7 @@ static carla::Buffer FWorldObserver_Serialize( header.episode_id = Episode.GetId(); header.game_timestamp = Episode.GetElapsedGameTime(); header.platform_timestamp = FPlatformTime::Seconds(); + header.delta_seconds = DeltaSeconds; write_data(header); // Write every actor. @@ -108,19 +131,14 @@ static carla::Buffer FWorldObserver_Serialize( { check(View.IsValid()); constexpr float TO_METERS = 1e-2; - const auto velocity = TO_METERS * View.GetActor()->GetVelocity(); - // get the angular velocity - const auto RootComponent = Cast(View.GetActor()->GetRootComponent()); - FVector angularVelocity { 0.0f, 0.0f, 0.0f }; - if (RootComponent != nullptr) - { - angularVelocity = RootComponent->GetPhysicsAngularVelocityInDegrees(); - } + const auto Velocity = TO_METERS * View.GetActor()->GetVelocity(); + ActorDynamicState info = { View.GetActorId(), View.GetActor()->GetActorTransform(), - carla::geom::Vector3D{velocity.X, velocity.Y, velocity.Z}, - carla::geom::Vector3D{angularVelocity.X, angularVelocity.Y, angularVelocity.Z}, + carla::geom::Vector3D{Velocity.X, Velocity.Y, Velocity.Z}, + FWorldObserver_GetAngularVelocity(*View.GetActor()), + FWorldObserver_GetAcceleration(View, Velocity, DeltaSeconds), FWorldObserver_GetActorState(View, Registry) }; write_data(info); @@ -130,13 +148,14 @@ static carla::Buffer FWorldObserver_Serialize( return buffer; } -void FWorldObserver::BroadcastTick(const UCarlaEpisode &Episode) +void FWorldObserver::BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds) { auto AsyncStream = Stream.MakeAsyncDataStream(*this); auto buffer = FWorldObserver_Serialize( AsyncStream.PopBufferFromPool(), - Episode); + Episode, + DeltaSeconds); AsyncStream.Send(*this, std::move(buffer)); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.h index da1898f5c..ace91b45c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.h @@ -32,7 +32,7 @@ public: /// Send a message to every connected client with the info about the given @a /// Episode. - void BroadcastTick(const UCarlaEpisode &Episode); + void BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds); /// Dummy. Required for compatibility with other sensors only. FTransform GetActorTransform() const