From 39f5c4da4998df49df4e41c78317b269ffa94704 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Mon, 8 Jul 2019 17:05:33 +0200 Subject: [PATCH] Allow removing on tick callbacks --- CHANGELOG.md | 1 + Docs/python_api.md | 3 +- LibCarla/source/carla/AtomicList.h | 19 +++++++--- LibCarla/source/carla/client/GnssSensor.cpp | 16 ++++----- LibCarla/source/carla/client/GnssSensor.h | 12 ++++--- .../carla/client/LaneInvasionSensor.cpp | 15 ++++---- .../source/carla/client/LaneInvasionSensor.h | 8 +++-- LibCarla/source/carla/client/World.cpp | 6 +++- LibCarla/source/carla/client/World.h | 7 +++- .../source/carla/client/detail/CallbackList.h | 36 ++++++++++++++++--- LibCarla/source/carla/client/detail/Episode.h | 8 +++-- .../source/carla/client/detail/Simulator.h | 9 +++-- .../carla/client/detail/WalkerNavigation.h | 2 +- PythonAPI/carla/source/libcarla/World.cpp | 5 +-- 14 files changed, 105 insertions(+), 42 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4c2e2ec6d..c5ed4b6b5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,7 @@ * API extension: added `WorldSnapshot` that contains a list of `ActorSnapshot`, allows capturings a "still image" of the world at a single frame * API extension: `world.tick()` now synchronizes with the simulator and returns the id of the newly started frame * API extension: `world.apply_settings(settings)` now synchronizes with the simulator and returns the id of the frame when the settings took effect + * API extension: added `world.remove_on_tick(id)` to allow removing on tick callbacks * API change: Rename `frame_count` and `frame_number` as `frame`, old members are kept as deprecated * API change: `world.wait_for_tick()` now returns a `carla.WorldSnapshot` * API change: the callback of `world.on_tick(callback)` now receives a `carla.WorldSnapshot` diff --git a/Docs/python_api.md b/Docs/python_api.md index 218f2b7d8..d01f268b0 100644 --- a/Docs/python_api.md +++ b/Docs/python_api.md @@ -41,7 +41,8 @@ - `spawn_actor(blueprint, transform, attach_to=None)` - `try_spawn_actor(blueprint, transform, attach_to=None, attachment_type=carla.AttachmentType.Rigid)` - `wait_for_tick(seconds=1.0) -> carla.WorldSnapshot` -- `on_tick(callback)` +- `on_tick(callback) -> id of the callback` +- `remove_on_tick(id)` - `tick() -> int (id of the newly started frame)` ## `carla.WorldSettings` diff --git a/LibCarla/source/carla/AtomicList.h b/LibCarla/source/carla/AtomicList.h index 90cd44d57..31699d0ea 100644 --- a/LibCarla/source/carla/AtomicList.h +++ b/LibCarla/source/carla/AtomicList.h @@ -20,8 +20,9 @@ namespace detail { /// /// @warning Only Load method is atomic, modifications to the list are locked /// with a mutex. - template > + template class AtomicList : private NonCopyable { + using ListT = std::vector; public: AtomicList() : _list(std::make_shared()) {} @@ -30,17 +31,27 @@ namespace detail { void Push(ValueT &&value) { std::lock_guard lock(_mutex); auto new_list = std::make_shared(*Load()); - new_list->push_back(std::forward(value)); + new_list->emplace_back(std::forward(value)); _list = new_list; } - void Delete(unsigned int index) { + void DeleteByIndex(size_t index) { std::lock_guard lock(_mutex); auto new_list = std::make_shared(*Load()); - new_list->erase(new_list->begin() + index); + auto begin = new_list->begin(); + std::advance(begin, index); + new_list->erase(begin); _list = new_list; } + template + void DeleteByValue(const ValueT &value) { + std::lock_guard lock(_mutex); + auto list = std::make_shared(*Load()); + list->erase(std::remove(list->begin(), list->end(), value), list->end()); + _list = list; + } + void Clear() { std::lock_guard lock(_mutex); _list = std::make_shared(); diff --git a/LibCarla/source/carla/client/GnssSensor.cpp b/LibCarla/source/carla/client/GnssSensor.cpp index 3ae03f307..eecfd5dd7 100644 --- a/LibCarla/source/carla/client/GnssSensor.cpp +++ b/LibCarla/source/carla/client/GnssSensor.cpp @@ -18,7 +18,7 @@ namespace client { GnssSensor::~GnssSensor() = default; void GnssSensor::Listen(CallbackFunctionType callback) { - if (_is_listening) { + if (IsListening()) { log_error(GetDisplayId(), ": already listening"); return; } @@ -35,7 +35,7 @@ namespace client { auto self = boost::static_pointer_cast(shared_from_this()); log_debug(GetDisplayId(), ": subscribing to tick event"); - GetEpisode().Lock()->RegisterOnTickEvent([ + _callback_id = GetEpisode().Lock()->RegisterOnTickEvent([ cb=std::move(callback), weak_self=WeakPtr(self)](const auto &snapshot) { auto self = weak_self.lock(); @@ -46,8 +46,6 @@ namespace client { } } }); - - _is_listening = true; } SharedPtr GnssSensor::TickGnssSensor( @@ -59,15 +57,17 @@ namespace client { GetTransform(), _geo_reference.Transform(GetLocation())); } catch (const std::exception &e) { - /// @todo We need to unsubscribe the sensor. - // log_error("GnssSensor:", e.what()); + log_error("GnssSensor:", e.what()); + Stop(); return nullptr; } } void GnssSensor::Stop() { - /// @todo We need unsubscribe from the world on tick. - _is_listening = false; + if (_callback_id.has_value()) { + GetEpisode().Lock()->RemoveOnTickEvent(*_callback_id); + _callback_id = boost::none; + } } } // namespace client diff --git a/LibCarla/source/carla/client/GnssSensor.h b/LibCarla/source/carla/client/GnssSensor.h index 9cf6a60a3..6f39b30a5 100644 --- a/LibCarla/source/carla/client/GnssSensor.h +++ b/LibCarla/source/carla/client/GnssSensor.h @@ -5,16 +5,18 @@ #pragma once -#include "carla/client/Sensor.h" +#include "carla/client/ClientSideSensor.h" #include "carla/geom/GeoLocation.h" +#include + namespace carla { namespace client { - class GnssSensor final : public Sensor { + class GnssSensor final : public ClientSideSensor { public: - using Sensor::Sensor; + using ClientSideSensor::ClientSideSensor; ~GnssSensor(); @@ -33,7 +35,7 @@ namespace client { /// Return whether this Sensor instance is currently listening to the /// associated sensor in the simulator. bool IsListening() const override { - return _is_listening; + return _callback_id.has_value(); } private: @@ -42,7 +44,7 @@ namespace client { geom::GeoLocation _geo_reference; - bool _is_listening = false; + boost::optional _callback_id; }; } // namespace client diff --git a/LibCarla/source/carla/client/LaneInvasionSensor.cpp b/LibCarla/source/carla/client/LaneInvasionSensor.cpp index 878cde311..451635500 100644 --- a/LibCarla/source/carla/client/LaneInvasionSensor.cpp +++ b/LibCarla/source/carla/client/LaneInvasionSensor.cpp @@ -43,7 +43,7 @@ namespace client { LaneInvasionSensor::~LaneInvasionSensor() = default; void LaneInvasionSensor::Listen(CallbackFunctionType callback) { - if (_is_listening) { + if (IsListening()) { log_error(GetDisplayId(), ": already listening"); return; } @@ -60,7 +60,7 @@ namespace client { auto self = boost::static_pointer_cast(shared_from_this()); log_debug(GetDisplayId(), ": subscribing to tick event"); - GetEpisode().Lock()->RegisterOnTickEvent([ + _callback_id = GetEpisode().Lock()->RegisterOnTickEvent([ cb=std::move(callback), weak_self=WeakPtr(self)](const auto &snapshot) { auto self = weak_self.lock(); @@ -71,12 +71,13 @@ namespace client { } } }); - _is_listening = true; } void LaneInvasionSensor::Stop() { - /// @todo We need unsubscribe from the world on tick. - _is_listening = false; + if (_callback_id.has_value()) { + GetEpisode().Lock()->RemoveOnTickEvent(*_callback_id); + _callback_id = boost::none; + } } SharedPtr LaneInvasionSensor::TickLaneInvasionSensor( @@ -98,8 +99,8 @@ namespace client { _vehicle, crossed_lanes); } catch (const std::exception &e) { - /// @todo We need to unsubscribe the sensor. - // log_error("LaneInvasionSensor:", e.what()); + log_error("LaneInvasionSensor:", e.what()); + Stop(); return nullptr; } } diff --git a/LibCarla/source/carla/client/LaneInvasionSensor.h b/LibCarla/source/carla/client/LaneInvasionSensor.h index 71b1e5f44..d4f4d710f 100644 --- a/LibCarla/source/carla/client/LaneInvasionSensor.h +++ b/LibCarla/source/carla/client/LaneInvasionSensor.h @@ -9,6 +9,8 @@ #include "carla/client/ClientSideSensor.h" #include "carla/geom/Location.h" +#include + #include namespace carla { @@ -38,20 +40,20 @@ namespace client { /// Return whether this Sensor instance is currently listening to the /// associated sensor in the simulator. bool IsListening() const override { - return _is_listening; + return _callback_id.has_value(); } private: SharedPtr TickLaneInvasionSensor(const Timestamp ×tamp); - bool _is_listening = false; - SharedPtr _map; SharedPtr _vehicle; std::array _bounds; + + boost::optional _callback_id; }; } // namespace client diff --git a/LibCarla/source/carla/client/World.cpp b/LibCarla/source/carla/client/World.cpp index 4e692d8bf..3243da1f6 100644 --- a/LibCarla/source/carla/client/World.cpp +++ b/LibCarla/source/carla/client/World.cpp @@ -97,10 +97,14 @@ namespace client { return _episode.Lock()->WaitForTick(timeout); } - void World::OnTick(std::function callback) { + size_t World::OnTick(std::function callback) { return _episode.Lock()->RegisterOnTickEvent(std::move(callback)); } + void World::RemoveOnTick(size_t callback_id) { + _episode.Lock()->RemoveOnTickEvent(callback_id); + } + uint64_t World::Tick() { return _episode.Lock()->Tick(); } diff --git a/LibCarla/source/carla/client/World.h b/LibCarla/source/carla/client/World.h index 0963bfabd..2d8d1be18 100644 --- a/LibCarla/source/carla/client/World.h +++ b/LibCarla/source/carla/client/World.h @@ -104,7 +104,12 @@ namespace client { WorldSnapshot WaitForTick(time_duration timeout) const; /// Register a @a callback to be called every time a world tick is received. - void OnTick(std::function callback); + /// + /// @return ID of the callback, use it to remove the callback. + size_t OnTick(std::function callback); + + /// Remove a callback registered with OnTick. + void RemoveOnTick(size_t callback_id); /// Signal the simulator to continue to next tick (only has effect on /// synchronous mode). diff --git a/LibCarla/source/carla/client/detail/CallbackList.h b/LibCarla/source/carla/client/detail/CallbackList.h index 112a53be7..b62b7637b 100644 --- a/LibCarla/source/carla/client/detail/CallbackList.h +++ b/LibCarla/source/carla/client/detail/CallbackList.h @@ -9,6 +9,7 @@ #include "carla/AtomicList.h" #include "carla/NonCopyable.h" +#include #include namespace carla { @@ -23,13 +24,19 @@ namespace detail { void Call(InputsT... args) const { auto list = _list.Load(); - for (auto &callback : *list) { - callback(args...); + for (auto &item : *list) { + item.callback(args...); } } - void RegisterCallback(CallbackType &&callback) { - _list.Push(std::move(callback)); + size_t Push(CallbackType &&callback) { + auto id = ++_counter; + _list.Push(Item{id, std::move(callback)}); + return id; + } + + void Remove(size_t id) { + _list.DeleteByValue(id); } void Clear() { @@ -38,7 +45,26 @@ namespace detail { private: - AtomicList _list; + struct Item { + size_t id; + CallbackType callback; + + friend bool operator==(const Item &lhs, const Item &rhs) { + return lhs.id == rhs.id; + } + + friend bool operator==(const Item &lhs, size_t rhs) { + return lhs.id == rhs; + } + + friend bool operator==(size_t lhs, const Item &rhs) { + return lhs == rhs.id; + } + }; + + std::atomic_size_t _counter{0u}; + + AtomicList _list; }; } // namespace detail diff --git a/LibCarla/source/carla/client/detail/Episode.h b/LibCarla/source/carla/client/detail/Episode.h index 15f09ba24..aa630ef7e 100644 --- a/LibCarla/source/carla/client/detail/Episode.h +++ b/LibCarla/source/carla/client/detail/Episode.h @@ -71,8 +71,12 @@ namespace detail { return _snapshot.WaitFor(timeout); } - void RegisterOnTickEvent(std::function callback) { - _on_tick_callbacks.RegisterCallback(std::move(callback)); + size_t RegisterOnTickEvent(std::function callback) { + return _on_tick_callbacks.Push(std::move(callback)); + } + + void RemoveOnTickEvent(size_t id) { + _on_tick_callbacks.Remove(id); } private: diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index 2a64f632b..274801810 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -140,9 +140,14 @@ namespace detail { WorldSnapshot WaitForTick(time_duration timeout); - void RegisterOnTickEvent(std::function callback) { + size_t RegisterOnTickEvent(std::function callback) { DEBUG_ASSERT(_episode != nullptr); - _episode->RegisterOnTickEvent(std::move(callback)); + return _episode->RegisterOnTickEvent(std::move(callback)); + } + + void RemoveOnTickEvent(size_t id) { + DEBUG_ASSERT(_episode != nullptr); + _episode->RemoveOnTickEvent(id); } uint64_t Tick(); diff --git a/LibCarla/source/carla/client/detail/WalkerNavigation.h b/LibCarla/source/carla/client/detail/WalkerNavigation.h index 264ab97e1..4c8760703 100644 --- a/LibCarla/source/carla/client/detail/WalkerNavigation.h +++ b/LibCarla/source/carla/client/detail/WalkerNavigation.h @@ -41,7 +41,7 @@ namespace detail { while (i < list->size()) { if ((*list)[i].walker == walker_id && (*list)[i].controller == controller_id) { - _walkers.Delete(i); + _walkers.DeleteByIndex(i); break; } ++i; diff --git a/PythonAPI/carla/source/libcarla/World.cpp b/PythonAPI/carla/source/libcarla/World.cpp index 3b76a391f..236271c39 100644 --- a/PythonAPI/carla/source/libcarla/World.cpp +++ b/PythonAPI/carla/source/libcarla/World.cpp @@ -52,8 +52,8 @@ static auto WaitForTick(const carla::client::World &world, double seconds) { return world.WaitForTick(TimeDurationFromSeconds(seconds)); } -static void OnTick(carla::client::World &self, boost::python::object callback) { - self.OnTick(MakeCallback(std::move(callback))); +static size_t OnTick(carla::client::World &self, boost::python::object callback) { + return self.OnTick(MakeCallback(std::move(callback))); } static auto GetActorsById(carla::client::World &self, const boost::python::list &actor_ids) { @@ -145,6 +145,7 @@ void export_world() { .def("try_spawn_actor", SPAWN_ACTOR_WITHOUT_GIL(TrySpawnActor)) .def("wait_for_tick", &WaitForTick, (arg("seconds")=10.0)) .def("on_tick", &OnTick, (arg("callback"))) + .def("remove_on_tick", &cc::World::RemoveOnTick, (arg("callback_id"))) .def("tick", CALL_WITHOUT_GIL(cc::World, Tick)) .def(self_ns::str(self_ns::self)) ;