CARLA
Simulator.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
8 
9 #include "carla/Debug.h"
10 #include "carla/Exception.h"
11 #include "carla/Logging.h"
14 #include "carla/client/Map.h"
15 #include "carla/client/Sensor.h"
21 
22 #include <exception>
23 #include <thread>
24 
25 using namespace std::string_literals;
26 
27 namespace carla {
28 namespace client {
29 namespace detail {
30 
31  // ===========================================================================
32  // -- Static local methods ---------------------------------------------------
33  // ===========================================================================
34 
35  static void ValidateVersions(Client &client) {
36  const auto vc = client.GetClientVersion();
37  const auto vs = client.GetServerVersion();
38  if (vc != vs) {
40  "Version mismatch detected: You are trying to connect to a simulator",
41  "that might be incompatible with this API");
42  log_warning("Client API version =", vc);
43  log_warning("Simulator API version =", vs);
44  }
45  }
46 
47  static bool SynchronizeFrame(uint64_t frame, const Episode &episode, time_duration timeout) {
48  bool result = true;
49  auto start = std::chrono::system_clock::now();
50  while (frame > episode.GetState()->GetTimestamp().frame) {
51  std::this_thread::yield();
52  auto end = std::chrono::system_clock::now();
53  auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(end-start);
54  if(timeout.to_chrono() < diff) {
55  result = false;
56  break;
57  }
58  }
59  if(result) {
61  }
62 
63  return result;
64  }
65 
66  // ===========================================================================
67  // -- Constructor ------------------------------------------------------------
68  // ===========================================================================
69 
70  Simulator::Simulator(
71  const std::string &host,
72  const uint16_t port,
73  const size_t worker_threads,
74  const bool enable_garbage_collection)
75  : LIBCARLA_INITIALIZE_LIFETIME_PROFILER("SimulatorClient("s + host + ":" + std::to_string(port) + ")"),
76  _client(host, port, worker_threads),
77  _light_manager(new LightManager()),
78  _gc_policy(enable_garbage_collection ?
80 
81  // ===========================================================================
82  // -- Load a new episode -----------------------------------------------------
83  // ===========================================================================
84 
85  EpisodeProxy Simulator::LoadEpisode(std::string map_name, bool reset_settings, rpc::MapLayer map_layers) {
86  const auto id = GetCurrentEpisode().GetId();
87  _client.LoadEpisode(std::move(map_name), reset_settings, map_layers);
88 
89  // We are waiting 50ms for the server to reload the episode.
90  // If in this time we have not detected a change of episode, we try again
91  // 'number_of_attempts' times.
92  // TODO This time is completly arbitrary so we need to improve
93  // this pipeline to not depend in this time because this timeout
94  // could result that the client resume the simulation in different
95  // initial ticks when loading a map in syncronous mode.
96  size_t number_of_attempts = _client.GetTimeout().milliseconds() / 50u;
97 
98  for (auto i = 0u; i < number_of_attempts; ++i) {
99  using namespace std::literals::chrono_literals;
102 
103  _episode->WaitForState(50ms);
104  auto episode = GetCurrentEpisode();
105 
106  if (episode.GetId() != id) {
107  return episode;
108  }
109  }
110  throw_exception(std::runtime_error("failed to connect to newly created map"));
111  }
112 
114  std::string opendrive,
115  const rpc::OpendriveGenerationParameters & params, bool reset_settings) {
116  // The "OpenDriveMap" is an ".umap" located in:
117  // "carla/Unreal/CarlaUE4/Content/Carla/Maps/"
118  // It will load the last sended OpenDRIVE by client's "LoadOpenDriveEpisode()"
119  constexpr auto custom_opendrive_map = "OpenDriveMap";
120  _client.CopyOpenDriveToServer(std::move(opendrive), params);
121  return LoadEpisode(custom_opendrive_map, reset_settings);
122  }
123 
124  // ===========================================================================
125  // -- Access to current episode ----------------------------------------------
126  // ===========================================================================
127 
129  if (_episode == nullptr) {
131  _episode = std::make_shared<Episode>(_client);
132  _episode->Listen();
133  if (!GetEpisodeSettings().synchronous_mode) {
135  }
136  _light_manager->SetEpisode(EpisodeProxy{shared_from_this()});
137  }
138  return EpisodeProxy{shared_from_this()};
139  }
140 
142  return MakeShared<Map>(_client.GetMapInfo());
143  }
144 
145  // ===========================================================================
146  // -- Tick -------------------------------------------------------------------
147  // ===========================================================================
148 
150  DEBUG_ASSERT(_episode != nullptr);
151  auto result = _episode->WaitForState(timeout);
152  if (!result.has_value()) {
154  }
155  return *result;
156  }
157 
158  uint64_t Simulator::Tick(time_duration timeout) {
159  DEBUG_ASSERT(_episode != nullptr);
160  const auto frame = _client.SendTickCue();
161  bool result = SynchronizeFrame(frame, *_episode, timeout);
162  if (!result) {
164  }
165  return frame;
166  }
167 
168  // ===========================================================================
169  // -- Access to global objects in the episode --------------------------------
170  // ===========================================================================
171 
173  auto defs = _client.GetActorDefinitions();
174  return MakeShared<BlueprintLibrary>(std::move(defs));
175  }
176 
179  }
180 
182  return MakeActor(_client.GetSpectator());
183  }
184 
186  if (settings.synchronous_mode && !settings.fixed_delta_seconds) {
187  log_warning(
188  "synchronous mode enabled with variable delta seconds. It is highly "
189  "recommended to set 'fixed_delta_seconds' when running on synchronous mode.");
190  }
191  else if (settings.synchronous_mode && settings.substepping) {
192  if(settings.max_substeps < 1 || settings.max_substeps > 16) {
193  log_warning(
194  "synchronous mode and substepping are enabled but the number of substeps is not valid. "
195  "Please be aware that this value needs to be in the range [1-16].");
196  }
197  double n_substeps = settings.fixed_delta_seconds.get() / settings.max_substep_delta_time;
198 
199  if (n_substeps > static_cast<double>(settings.max_substeps)) {
200  log_warning(
201  "synchronous mode and substepping are enabled but the values for the simulation are not valid. "
202  "The values should fulfil fixed_delta_seconds <= max_substep_delta_time * max_substeps. "
203  "Be very careful about that, the time deltas are not guaranteed.");
204  }
205  }
206  const auto frame = _client.SetEpisodeSettings(settings);
207 
208  using namespace std::literals::chrono_literals;
209  SynchronizeFrame(frame, *_episode, 1s);
210 
211  return frame;
212  }
213 
214  // ===========================================================================
215  // -- AI ---------------------------------------------------------------------
216  // ===========================================================================
217 
219  auto walker = controller.GetParent();
220  if (walker == nullptr) {
221  throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
222  return;
223  }
224  DEBUG_ASSERT(_episode != nullptr);
225  auto navigation = _episode->CreateNavigationIfMissing();
226  DEBUG_ASSERT(navigation != nullptr);
227  navigation->RegisterWalker(walker->GetId(), controller.GetId());
228  }
229 
231  auto walker = controller.GetParent();
232  if (walker == nullptr) {
233  throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
234  return;
235  }
236  DEBUG_ASSERT(_episode != nullptr);
237  auto navigation = _episode->CreateNavigationIfMissing();
238  DEBUG_ASSERT(navigation != nullptr);
239  navigation->UnregisterWalker(walker->GetId(), controller.GetId());
240  }
241 
242  boost::optional<geom::Location> Simulator::GetRandomLocationFromNavigation() {
243  DEBUG_ASSERT(_episode != nullptr);
244  auto navigation = _episode->CreateNavigationIfMissing();
245  DEBUG_ASSERT(navigation != nullptr);
246  return navigation->GetRandomLocation();
247  }
248 
249  void Simulator::SetPedestriansCrossFactor(float percentage) {
250  DEBUG_ASSERT(_episode != nullptr);
251  auto navigation = _episode->CreateNavigationIfMissing();
252  DEBUG_ASSERT(navigation != nullptr);
253  navigation->SetPedestriansCrossFactor(percentage);
254  }
255 
256  // ===========================================================================
257  // -- General operations with actors -----------------------------------------
258  // ===========================================================================
259 
261  const ActorBlueprint &blueprint,
262  const geom::Transform &transform,
263  Actor *parent,
264  rpc::AttachmentType attachment_type,
266  rpc::Actor actor;
267  if (parent != nullptr) {
269  blueprint.MakeActorDescription(),
270  transform,
271  parent->GetId(),
272  attachment_type);
273  } else {
274  actor = _client.SpawnActor(
275  blueprint.MakeActorDescription(),
276  transform);
277  }
278  DEBUG_ASSERT(_episode != nullptr);
279  _episode->RegisterActor(actor);
280  const auto gca = (gc == GarbageCollectionPolicy::Inherit ? _gc_policy : gc);
281  auto result = ActorFactory::MakeActor(GetCurrentEpisode(), actor, gca);
282  log_debug(
283  result->GetDisplayId(),
284  "created",
285  gca == GarbageCollectionPolicy::Enabled ? "with" : "without",
286  "garbage collection");
287  return result;
288  }
289 
291  bool success = true;
292  success = _client.DestroyActor(actor.GetId());
293  if (success) {
294  // Remove it's persistent state so it cannot access the client anymore.
295  actor.GetEpisode().Clear();
296  log_debug(actor.GetDisplayId(), "destroyed.");
297  } else {
298  log_debug("failed to destroy", actor.GetDisplayId());
299  }
300  return success;
301  }
302 
303  // ===========================================================================
304  // -- Operations with sensors ------------------------------------------------
305  // ===========================================================================
306 
308  const Sensor &sensor,
309  std::function<void(SharedPtr<sensor::SensorData>)> callback) {
310  DEBUG_ASSERT(_episode != nullptr);
313  [cb=std::move(callback), ep=WeakEpisodeProxy{shared_from_this()}](auto buffer) {
314  auto data = sensor::Deserializer::Deserialize(std::move(buffer));
315  data->_episode = ep.TryLock();
316  cb(std::move(data));
317  });
318  }
319 
322  }
323 
326  }
327 
328 } // namespace detail
329 } // namespace client
330 } // namespace carla
rpc::VehicleLightStateList GetVehiclesLightStates()
Returns a list of pairs where the firts element is the vehicle ID and the second one is the light sta...
Definition: Simulator.cpp:177
std::shared_ptr< Episode > _episode
Definition: Simulator.h:613
bool DestroyActor(Actor &actor)
Definition: Simulator.cpp:290
streaming::Token GetStreamToken() const
Definition: rpc/Actor.h:45
Seting for map generation from opendrive without additional geometry.
static SharedPtr< SensorData > Deserialize(Buffer &&buffer)
static time_duration milliseconds(size_t timeout)
Definition: Time.h:26
Holds the current episode, and the current episode state.
Definition: Episode.h:33
static void ValidateVersions(Client &client)
Definition: Simulator.cpp:35
std::vector< rpc::ActorDefinition > GetActorDefinitions()
void throw_exception(const std::exception &e)
Definition: Carla.cpp:101
void CopyOpenDriveToServer(std::string opendrive, const rpc::OpendriveGenerationParameters &params)
static SharedPtr< Actor > MakeActor(EpisodeProxy episode, rpc::Actor actor_description, GarbageCollectionPolicy garbage_collection_policy)
Create an Actor based on the provided actor_description.
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
SharedPtr< LightManager > _light_manager
Definition: Simulator.h:611
std::vector< std::pair< ActorId, VehicleLightState::flag_type > > VehicleLightStateList
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
const GarbageCollectionPolicy _gc_policy
Definition: Simulator.h:615
rpc::EpisodeSettings GetEpisodeSettings()
SharedPtr< Actor > GetSpectator()
Definition: Simulator.cpp:181
boost::optional< geom::Location > GetRandomLocationFromNavigation()
Definition: Simulator.cpp:242
void SubscribeToStream(const streaming::Token &token, std::function< void(Buffer)> callback)
rpc::VehicleLightStateList GetVehiclesLightStates()
Returns a list of pairs where the firts element is the vehicle ID and the second one is the light sta...
static void log_debug(Args &&...)
Definition: Logging.h:75
void UnSubscribeFromSensor(const Sensor &sensor)
Definition: Simulator.cpp:320
const std::string & GetDisplayId() const
Definition: ActorState.h:33
uint64_t SetEpisodeSettings(const rpc::EpisodeSettings &settings)
Definition: Simulator.cpp:185
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
#define LIBCARLA_INITIALIZE_LIFETIME_PROFILER(display_name)
rpc::EpisodeSettings GetEpisodeSettings()
Definition: Simulator.h:217
SharedPtr< Actor > SpawnActor(const ActorBlueprint &blueprint, const geom::Transform &transform, Actor *parent=nullptr, rpc::AttachmentType attachment_type=rpc::AttachmentType::Rigid, GarbageCollectionPolicy gc=GarbageCollectionPolicy::Inherit)
Spawns an actor into the simulation.
Definition: Simulator.cpp:260
static bool SynchronizeFrame(uint64_t frame, const Episode &episode, time_duration timeout)
Definition: Simulator.cpp:47
void FreezeAllTrafficLights(bool frozen)
Definition: Simulator.cpp:324
void LoadEpisode(std::string map_name, bool reset_settings=true, rpc::MapLayer map_layer=rpc::MapLayer::All)
EpisodeProxy LoadEpisode(std::string map_name, bool reset_settings=true, rpc::MapLayer map_layers=rpc::MapLayer::All)
Definition: Simulator.cpp:85
void SubscribeToSensor(const Sensor &sensor, std::function< void(SharedPtr< sensor::SensorData >)> callback)
Definition: Simulator.cpp:307
static void log_warning(Args &&... args)
Definition: Logging.h:96
SharedPtr< Actor > MakeActor(rpc::Actor actor_description, GarbageCollectionPolicy gc=GarbageCollectionPolicy::Disabled)
Creates an actor instance out of a description of an existing actor.
Definition: Simulator.h:309
std::shared_ptr< const EpisodeState > GetState() const
Definition: Episode.h:48
Provides communication with the rpc and streaming servers of a CARLA simulator.
rpc::Actor SpawnActorWithParent(const rpc::ActorDescription &description, const geom::Transform &transform, rpc::ActorId parent, rpc::AttachmentType attachment_type)
void UnregisterAIController(const WalkerAIController &controller)
Definition: Simulator.cpp:230
SharedPtr< Actor > GetParent() const
Definition: ActorState.cpp:31
uint64_t Tick(time_duration timeout)
Definition: Simulator.cpp:158
rpc::Actor SpawnActor(const rpc::ActorDescription &description, const geom::Transform &transform)
SharedPtr< Map > GetCurrentMap()
Definition: Simulator.cpp:141
bool DestroyActor(rpc::ActorId actor)
EpisodeProxy LoadOpenDriveEpisode(std::string opendrive, const rpc::OpendriveGenerationParameters &params, bool reset_settings=true)
Definition: Simulator.cpp:113
rpc::ActorDescription MakeActorDescription() const
Represents an actor in the simulation.
Definition: client/Actor.h:18
Positive time duration up to milliseconds resolution.
Definition: Time.h:19
void UnSubscribeFromStream(const streaming::Token &token)
Contains all the necessary information for spawning an Actor.
void RegisterAIController(const WalkerAIController &controller)
Definition: Simulator.cpp:218
void SetPedestriansCrossFactor(float percentage)
Definition: Simulator.cpp:249
SharedPtr< BlueprintLibrary > GetBlueprintLibrary()
Definition: Simulator.cpp:172
constexpr auto to_chrono() const
Definition: Time.h:50
EpisodeProxyImpl< EpisodeProxyPointerType::Weak > WeakEpisodeProxy
Definition: EpisodeProxy.h:71
const std::string GetEndpoint() const
WorldSnapshot WaitForTick(time_duration timeout)
Definition: Simulator.cpp:149
const rpc::Actor & GetActorDescription() const
Definition: ActorState.h:64
uint64_t SetEpisodeSettings(const rpc::EpisodeSettings &settings)