diff --git a/Docs/python_api.md b/Docs/python_api.md index db9ae5be7..fe389f33e 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -72,6 +72,8 @@ - `get_world()` - `get_location()` - `get_transform()` +- `get_velocity()` +- `get_acceleration()` - `set_location(location)` - `set_transform(transform)` - `destroy()` diff --git a/LibCarla/source/carla/AtomicSharedPtr.h b/LibCarla/source/carla/AtomicSharedPtr.h index 5a0e017f6..f2fad1fde 100644 --- a/LibCarla/source/carla/AtomicSharedPtr.h +++ b/LibCarla/source/carla/AtomicSharedPtr.h @@ -17,6 +17,9 @@ namespace carla { class AtomicSharedPtr : private NonCopyable { public: + template + explicit AtomicSharedPtr(Args &&... args) : _ptr(std::forward(args)...) {} + void store(std::shared_ptr ptr) { std::atomic_store_explicit(&_ptr, ptr, std::memory_order_relaxed); } diff --git a/LibCarla/source/carla/client/Actor.cpp b/LibCarla/source/carla/client/Actor.cpp index ae1b2cba4..19eb07182 100644 --- a/LibCarla/source/carla/client/Actor.cpp +++ b/LibCarla/source/carla/client/Actor.cpp @@ -20,6 +20,14 @@ namespace client { return GetEpisode()->GetActorTransform(*this); } + geom::Vector3D Actor::GetVelocity() const { + return GetEpisode()->GetActorVelocity(*this); + } + + geom::Vector3D Actor::GetAcceleration() const { + return GetEpisode()->GetActorAcceleration(*this); + } + void Actor::SetLocation(const geom::Location &location) { GetEpisode()->SetActorLocation(*this, location); } diff --git a/LibCarla/source/carla/client/Actor.h b/LibCarla/source/carla/client/Actor.h index 90a41fc59..8f5d673bb 100644 --- a/LibCarla/source/carla/client/Actor.h +++ b/LibCarla/source/carla/client/Actor.h @@ -31,6 +31,10 @@ namespace client { geom::Transform GetTransform() const; + geom::Vector3D GetVelocity() const; + + geom::Vector3D GetAcceleration() const; + void SetLocation(const geom::Location &location); void SetTransform(const geom::Transform &transform); diff --git a/LibCarla/source/carla/client/detail/Episode.cpp b/LibCarla/source/carla/client/detail/Episode.cpp new file mode 100644 index 000000000..e311f4832 --- /dev/null +++ b/LibCarla/source/carla/client/detail/Episode.cpp @@ -0,0 +1,44 @@ +// 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 . + +#include "carla/client/detail/Episode.h" + +#include "carla/client/detail/Client.h" + +namespace carla { +namespace client { +namespace detail { + + static auto &CastData(const sensor::SensorData &data) { + using target_t = const sensor::data::RawEpisodeState; + DEBUG_ASSERT(dynamic_cast(&data) != nullptr); + return static_cast(data); + } + + Episode::Episode(Client &client) + : _client(client), + _description(client.GetEpisodeInfo()), + _state(std::make_shared()) {} + + Episode::~Episode() { + _client.UnSubscribeFromStream(_description.token); + } + + void Episode::Listen() { + std::weak_ptr weak = shared_from_this(); + _client.SubscribeToStream(_description.token, [weak](auto data) { + auto self = weak.lock(); + if (self != nullptr) { + /// @todo This is not atomic. + auto prev = self->_state.load(); + self->_state = prev->DeriveNextStep(CastData(*data)); + } + }); + } + +} // namespace detail +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/detail/Episode.h b/LibCarla/source/carla/client/detail/Episode.h new file mode 100644 index 000000000..80e684c63 --- /dev/null +++ b/LibCarla/source/carla/client/detail/Episode.h @@ -0,0 +1,57 @@ +// 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 "carla/AtomicSharedPtr.h" +#include "carla/NonCopyable.h" +#include "carla/client/detail/EpisodeState.h" +#include "carla/rpc/EpisodeInfo.h" + +namespace carla { +namespace client { +namespace detail { + + class Client; + + /// Represents the episode running on the Simulator. + class Episode + : public std::enable_shared_from_this, + private NonCopyable { + public: + + explicit Episode(Client &client); + + ~Episode(); + + void Listen(); + + auto GetId() const { + return _description.id; + } + + const std::string &GetMapName() const { + return _description.map_name; + } + + std::shared_ptr GetState() const { + auto state = _state.load(); + DEBUG_ASSERT(state != nullptr); + return state; + } + + private: + + Client &_client; + + const rpc::EpisodeInfo _description; + + AtomicSharedPtr _state; + }; + +} // namespace detail +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/detail/EpisodeState.cpp b/LibCarla/source/carla/client/detail/EpisodeState.cpp new file mode 100644 index 000000000..92da3185f --- /dev/null +++ b/LibCarla/source/carla/client/detail/EpisodeState.cpp @@ -0,0 +1,49 @@ +// 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 . + +#include "carla/client/detail/EpisodeState.h" + +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(); + next->_frame_number = state.GetFrameNumber(); + next->_game_timestamp = state.GetGameTimeStamp(); + next->_platform_timestamp = state.GetPlatformTimeStamp(); + const auto delta_time = next->_game_timestamp - _game_timestamp; + next->_actors.reserve(state.size()); + for (auto &&actor : state) { + auto acceleration = DeriveAcceleration( + delta_time, + GetActorState(actor.id).velocity, + actor.velocity); + DEBUG_ONLY(auto result = ) + next->_actors.emplace( + actor.id, + ActorState{actor.transform, actor.velocity, acceleration}); + DEBUG_ASSERT(result.second); + } + return next; + } + +} // namespace detail +} // namespace client +} // namespace carla diff --git a/LibCarla/source/carla/client/detail/EpisodeState.h b/LibCarla/source/carla/client/detail/EpisodeState.h index 0b14d0c51..37857c95e 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.h +++ b/LibCarla/source/carla/client/detail/EpisodeState.h @@ -7,30 +7,66 @@ #pragma once #include "carla/NonCopyable.h" -#include "carla/rpc/EpisodeInfo.h" +#include "carla/sensor/data/ActorDynamicState.h" +#include "carla/sensor/data/RawEpisodeState.h" + +#include +#include namespace carla { namespace client { namespace detail { - /// Represents an episode running on the Simulator. - class EpisodeState : private MovableNonCopyable { + /// Represents the state of all the actors of an episode at a given frame. + class EpisodeState + : std::enable_shared_from_this, + private NonCopyable { public: - EpisodeState(rpc::EpisodeInfo description) - : _description(std::move(description)) {} + struct ActorState { + geom::Transform transform; + geom::Vector3D velocity; + geom::Vector3D acceleration; + }; - auto GetId() const { - return _description.id; + /// @copydoc carla::sensor::SensorData::GetFrameNumber() + size_t GetFrameNumber() const { + return _frame_number; } - const std::string &GetMapName() const { - return _description.map_name; + /// @copydoc carla::sensor::data::RawEpisodeState::GetGameTimeStamp() + double GetGameTimeStamp() const { + return _game_timestamp; } + /// @copydoc carla::sensor::data::RawEpisodeState::GetPlatformTimeStamp() + double GetPlatformTimeStamp() const { + return _platform_timestamp; + } + + ActorState GetActorState(actor_id_type id) const { + ActorState state; + auto it = _actors.find(id); + if (it != _actors.end()) { + state = it->second; + } else { + log_debug("actor", id, "not found in episode"); + } + return state; + } + + std::shared_ptr DeriveNextStep( + const sensor::data::RawEpisodeState &state) const; + private: - rpc::EpisodeInfo _description; + size_t _frame_number = 0.0; + + double _game_timestamp = 0.0; + + double _platform_timestamp = 0.0; + + std::unordered_map _actors; }; } // namespace detail diff --git a/LibCarla/source/carla/client/detail/Simulator.cpp b/LibCarla/source/carla/client/detail/Simulator.cpp index 1c4bd5d29..4a38d2709 100644 --- a/LibCarla/source/carla/client/detail/Simulator.cpp +++ b/LibCarla/source/carla/client/detail/Simulator.cpp @@ -19,6 +19,10 @@ namespace carla { namespace client { namespace detail { + // =========================================================================== + // -- Static local methods --------------------------------------------------- + // =========================================================================== + static void ValidateVersions(Client &client) { const auto vc = client.GetClientVersion(); const auto vs = client.GetServerVersion(); @@ -52,7 +56,8 @@ namespace detail { EpisodeProxy Simulator::GetCurrentEpisode() { if (_episode == nullptr) { ValidateVersions(_client); - _episode = std::make_unique(_client.GetEpisodeInfo()); + _episode = std::make_shared(_client); + _episode->Listen(); } return EpisodeProxy{shared_from_this()}; } @@ -114,14 +119,6 @@ namespace detail { return success; } - geom::Location Simulator::GetActorLocation(const Actor &) { - throw std::runtime_error("GetActorLocation() not implemented!"); - } - - geom::Transform Simulator::GetActorTransform(const Actor &) { - throw std::runtime_error("GetActorTransform() not implemented!"); - } - // =========================================================================== // -- Operations with sensors ------------------------------------------------ // =========================================================================== diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 06565dc0d..43c15cfcf 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -14,8 +14,8 @@ #include "carla/client/GarbageCollectionPolicy.h" #include "carla/client/Vehicle.h" #include "carla/client/detail/Client.h" +#include "carla/client/detail/Episode.h" #include "carla/client/detail/EpisodeProxy.h" -#include "carla/client/detail/EpisodeState.h" #include "carla/profiler/LifetimeProfiled.h" namespace carla { @@ -126,9 +126,26 @@ namespace detail { bool DestroyActor(Actor &actor); - geom::Location GetActorLocation(const Actor &actor); + auto GetActorDynamicState(const Actor &actor) const { + DEBUG_ASSERT(_episode != nullptr); + return _episode->GetState()->GetActorState(actor.GetId()); + } - geom::Transform GetActorTransform(const Actor &actor); + geom::Location GetActorLocation(const Actor &actor) const { + return GetActorDynamicState(actor).transform.location; + } + + geom::Transform GetActorTransform(const Actor &actor) const { + return GetActorDynamicState(actor).transform; + } + + geom::Vector3D GetActorVelocity(const Actor &actor) const { + return GetActorDynamicState(actor).velocity; + } + + geom::Vector3D GetActorAcceleration(const Actor &actor) const { + return GetActorDynamicState(actor).acceleration; + } void SetActorLocation(Actor &actor, const geom::Location &location) { _client.SetActorLocation(actor.Serialize(), location); @@ -170,7 +187,7 @@ namespace detail { Client _client; - std::unique_ptr _episode; + std::shared_ptr _episode; GarbageCollectionPolicy _gc_policy; }; diff --git a/LibCarla/source/carla/sensor/data/ActorState.h b/LibCarla/source/carla/sensor/data/ActorDynamicState.h similarity index 69% rename from LibCarla/source/carla/sensor/data/ActorState.h rename to LibCarla/source/carla/sensor/data/ActorDynamicState.h index a32da676f..9aa4a58cf 100644 --- a/LibCarla/source/carla/sensor/data/ActorState.h +++ b/LibCarla/source/carla/sensor/data/ActorDynamicState.h @@ -8,6 +8,7 @@ #include "carla/geom/Transform.h" #include "carla/geom/Vector3D.h" +#include "carla/rpc/ActorId.h" #include @@ -17,10 +18,10 @@ namespace data { #pragma pack(push, 1) - /// Dynamic state of an actor. - struct ActorState { + /// Dynamic state of an actor at a certain frame. + struct ActorDynamicState { - uint32_t id; + actor_id_type id; geom::Transform transform; @@ -29,7 +30,7 @@ namespace data { #pragma pack(pop) - static_assert(sizeof(ActorState) == 10u * sizeof(uint32_t), "Invalid ActorState size!"); + static_assert(sizeof(ActorDynamicState) == 10u * sizeof(uint32_t), "Invalid ActorDynamicState size!"); } // namespace data } // namespace sensor diff --git a/LibCarla/source/carla/sensor/data/EpisodeState.h b/LibCarla/source/carla/sensor/data/RawEpisodeState.h similarity index 86% rename from LibCarla/source/carla/sensor/data/EpisodeState.h rename to LibCarla/source/carla/sensor/data/RawEpisodeState.h index 0da1f4929..a17ec3f90 100644 --- a/LibCarla/source/carla/sensor/data/EpisodeState.h +++ b/LibCarla/source/carla/sensor/data/RawEpisodeState.h @@ -7,7 +7,7 @@ #pragma once #include "carla/Debug.h" -#include "carla/sensor/data/ActorState.h" +#include "carla/sensor/data/ActorDynamicState.h" #include "carla/sensor/data/Array.h" #include "carla/sensor/s11n/EpisodeStateSerializer.h" @@ -16,15 +16,15 @@ namespace sensor { namespace data { /// State of the episode at a given frame. - class EpisodeState : public Array { - using Super = Array; + class RawEpisodeState : public Array { + using Super = Array; protected: using Serializer = s11n::EpisodeStateSerializer; friend Serializer; - explicit EpisodeState(RawData data) + explicit RawEpisodeState(RawData data) : Super(std::move(data)) { Super::SetOffset(Serializer::header_offset); } diff --git a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.cpp b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.cpp index fb31f5f85..a42240736 100644 --- a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.cpp +++ b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.cpp @@ -6,14 +6,14 @@ #include "carla/sensor/s11n/EpisodeStateSerializer.h" -#include "carla/sensor/data/EpisodeState.h" +#include "carla/sensor/data/RawEpisodeState.h" namespace carla { namespace sensor { namespace s11n { SharedPtr EpisodeStateSerializer::Deserialize(RawData data) { - return SharedPtr(new data::EpisodeState{std::move(data)}); + return SharedPtr(new data::RawEpisodeState{std::move(data)}); } } // namespace s11n diff --git a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h index cceddbc2b..9a482bb37 100644 --- a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h +++ b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h @@ -12,7 +12,7 @@ #include "carla/geom/Transform.h" #include "carla/geom/Vector3D.h" #include "carla/sensor/RawData.h" -#include "carla/sensor/data/ActorState.h" +#include "carla/sensor/data/ActorDynamicState.h" class FActorRegistry; @@ -60,9 +60,8 @@ namespace s11n { double game_timestamp, double platform_timestamp, const ActorRegistryT &actor_registry) { - uint64_t number_of_actors = actor_registry.Num(); // Set up buffer for writing. - buffer.reset(sizeof(Header) + sizeof(data::ActorState) * number_of_actors); + buffer.reset(sizeof(Header) + sizeof(data::ActorDynamicState) * actor_registry.Num()); auto begin = buffer.begin(); auto write_data = [&begin](const auto &data) { std::memcpy(begin, &data, sizeof(data)); @@ -77,7 +76,7 @@ namespace s11n { DEBUG_ASSERT(actor_view.GetActor() != nullptr); constexpr float TO_METERS = 1e-3; const auto velocity = TO_METERS * actor_view.GetActor()->GetVelocity(); - data::ActorState info = { + data::ActorDynamicState info = { actor_view.GetActorId(), actor_view.GetActor()->GetActorTransform(), geom::Vector3D{velocity.X, velocity.Y, velocity.Z} diff --git a/PythonAPI/source/libcarla/Actor.cpp b/PythonAPI/source/libcarla/Actor.cpp index b16c35008..b41dc45e9 100644 --- a/PythonAPI/source/libcarla/Actor.cpp +++ b/PythonAPI/source/libcarla/Actor.cpp @@ -33,8 +33,10 @@ void export_actor() { .add_property("bounding_box", CALL_RETURNING_COPY(cc::Actor, GetBoundingBox)) .add_property("is_alive", CALL_RETURNING_COPY(cc::Actor, IsAlive)) .def("get_world", CALL_RETURNING_COPY(cc::Actor, GetWorld)) - .def("get_location", CONST_CALL_WITHOUT_GIL(cc::Actor, GetLocation)) - .def("get_transform", CONST_CALL_WITHOUT_GIL(cc::Actor, GetTransform)) + .def("get_location", &cc::Actor::GetLocation) + .def("get_transform", &cc::Actor::GetTransform) + .def("get_velocity", &cc::Actor::GetVelocity) + .def("get_acceleration", &cc::Actor::GetAcceleration) .def("set_location", &cc::Actor::SetLocation, (arg("location"))) .def("set_transform", &cc::Actor::SetTransform, (arg("transform"))) .def("destroy", CALL_WITHOUT_GIL(cc::Actor, Destroy))