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"
15 #include "carla/client/Map.h"
16 #include "carla/client/Sensor.h"
22 
23 #include <exception>
24 #include <thread>
25 
26 using namespace std::string_literals;
27 
28 namespace carla {
29 namespace client {
30 namespace detail {
31 
32  // ===========================================================================
33  // -- Static local methods ---------------------------------------------------
34  // ===========================================================================
35 
36  static void ValidateVersions(Client &client) {
37  const auto vc = client.GetClientVersion();
38  const auto vs = client.GetServerVersion();
39  if (vc != vs) {
41  "Version mismatch detected: You are trying to connect to a simulator",
42  "that might be incompatible with this API");
43  log_warning("Client API version =", vc);
44  log_warning("Simulator API version =", vs);
45  }
46  }
47 
48  static bool SynchronizeFrame(uint64_t frame, const Episode &episode, time_duration timeout) {
49  bool result = true;
50  auto start = std::chrono::system_clock::now();
51  while (frame > episode.GetState()->GetTimestamp().frame) {
52  std::this_thread::yield();
53  auto end = std::chrono::system_clock::now();
54  auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(end-start);
55  if(timeout.to_chrono() < diff) {
56  result = false;
57  break;
58  }
59  }
60  if(result) {
62  }
63 
64  return result;
65  }
66 
67  // ===========================================================================
68  // -- Constructor ------------------------------------------------------------
69  // ===========================================================================
70 
71  Simulator::Simulator(
72  const std::string &host,
73  const uint16_t port,
74  const size_t worker_threads,
75  const bool enable_garbage_collection)
76  : LIBCARLA_INITIALIZE_LIFETIME_PROFILER("SimulatorClient("s + host + ":" + std::to_string(port) + ")"),
77  _client(host, port, worker_threads),
78  _light_manager(new LightManager()),
79  _gc_policy(enable_garbage_collection ?
81 
82  // ===========================================================================
83  // -- Load a new episode -----------------------------------------------------
84  // ===========================================================================
85 
86  EpisodeProxy Simulator::LoadEpisode(std::string map_name, bool reset_settings, rpc::MapLayer map_layers) {
87  const auto id = GetCurrentEpisode().GetId();
88  _client.LoadEpisode(std::move(map_name), reset_settings, map_layers);
89 
90  // We are waiting 50ms for the server to reload the episode.
91  // If in this time we have not detected a change of episode, we try again
92  // 'number_of_attempts' times.
93  // TODO This time is completly arbitrary so we need to improve
94  // this pipeline to not depend in this time because this timeout
95  // could result that the client resume the simulation in different
96  // initial ticks when loading a map in syncronous mode.
97  size_t number_of_attempts = _client.GetTimeout().milliseconds() / 50u;
98 
99  for (auto i = 0u; i < number_of_attempts; ++i) {
100  using namespace std::literals::chrono_literals;
103 
104  _episode->WaitForState(50ms);
105  auto episode = GetCurrentEpisode();
106 
107  if (episode.GetId() != id) {
108  return episode;
109  }
110  }
111  throw_exception(std::runtime_error("failed to connect to newly created map"));
112  }
113 
115  std::string opendrive,
116  const rpc::OpendriveGenerationParameters & params, bool reset_settings) {
117  // The "OpenDriveMap" is an ".umap" located in:
118  // "carla/Unreal/CarlaUE4/Content/Carla/Maps/"
119  // It will load the last sended OpenDRIVE by client's "LoadOpenDriveEpisode()"
120  constexpr auto custom_opendrive_map = "OpenDriveMap";
121  _client.CopyOpenDriveToServer(std::move(opendrive), params);
122  return LoadEpisode(custom_opendrive_map, reset_settings);
123  }
124 
125  // ===========================================================================
126  // -- Access to current episode ----------------------------------------------
127  // ===========================================================================
128 
130  if (_episode == nullptr) {
132  _episode = std::make_shared<Episode>(_client);
133  _episode->Listen();
134  if (!GetEpisodeSettings().synchronous_mode) {
136  }
137  _light_manager->SetEpisode(EpisodeProxy{shared_from_this()});
138  }
139  return EpisodeProxy{shared_from_this()};
140  }
141 
143  if (!_cached_map) {
144  return true;
145  }
146  if (map_info.name != _cached_map->GetName() ||
147  _open_drive_file.size() != _cached_map->GetOpenDrive().size()) {
148  return true;
149  }
150  return false;
151  }
152 
154  DEBUG_ASSERT(_episode != nullptr);
155  if (!_cached_map || _episode->HasMapChangedSinceLastCall()) {
156  rpc::MapInfo map_info = _client.GetMapInfo();
157  std::string map_name;
158  std::string map_base_path;
159  bool fill_base_string = false;
160  for (int i = map_info.name.size() - 1; i >= 0; --i) {
161  if (fill_base_string == false && map_info.name[i] != '/') {
162  map_name += map_info.name[i];
163  } else {
164  map_base_path += map_info.name[i];
165  fill_base_string = true;
166  }
167  }
168  std::reverse(map_name.begin(), map_name.end());
169  std::reverse(map_base_path.begin(), map_base_path.end());
170  std::string XODRFolder = map_base_path + "/OpenDrive/" + map_name + ".xodr";
171  if (FileTransfer::FileExists(XODRFolder) == false) _client.GetRequiredFiles();
173  _cached_map = MakeShared<Map>(map_info, _open_drive_file);
174  }
175 
176  return _cached_map;
177  }
178 
179  // ===========================================================================
180  // -- Required files ---------------------------------------------------------
181  // ===========================================================================
182 
183 
184  bool Simulator::SetFilesBaseFolder(const std::string &path) {
185  return _client.SetFilesBaseFolder(path);
186  }
187 
188  std::vector<std::string> Simulator::GetRequiredFiles(const std::string &folder, const bool download) const {
189  return _client.GetRequiredFiles(folder, download);
190  }
191 
192  void Simulator::RequestFile(const std::string &name) const {
193  _client.RequestFile(name);
194  }
195 
196  std::vector<uint8_t> Simulator::GetCacheFile(const std::string &name, const bool request_otherwise) const {
197  return _client.GetCacheFile(name, request_otherwise);
198  }
199 
200  // ===========================================================================
201  // -- Tick -------------------------------------------------------------------
202  // ===========================================================================
203 
205  DEBUG_ASSERT(_episode != nullptr);
206  auto result = _episode->WaitForState(timeout);
207  if (!result.has_value()) {
209  }
210  return *result;
211  }
212 
213  uint64_t Simulator::Tick(time_duration timeout) {
214  DEBUG_ASSERT(_episode != nullptr);
215  const auto frame = _client.SendTickCue();
216  bool result = SynchronizeFrame(frame, *_episode, timeout);
217  if (!result) {
219  }
220  return frame;
221  }
222 
223  // ===========================================================================
224  // -- Access to global objects in the episode --------------------------------
225  // ===========================================================================
226 
228  auto defs = _client.GetActorDefinitions();
229  return MakeShared<BlueprintLibrary>(std::move(defs));
230  }
231 
234  }
235 
237  return MakeActor(_client.GetSpectator());
238  }
239 
241  if (settings.synchronous_mode && !settings.fixed_delta_seconds) {
242  log_warning(
243  "synchronous mode enabled with variable delta seconds. It is highly "
244  "recommended to set 'fixed_delta_seconds' when running on synchronous mode.");
245  }
246  else if (settings.synchronous_mode && settings.substepping) {
247  if(settings.max_substeps < 1 || settings.max_substeps > 16) {
248  log_warning(
249  "synchronous mode and substepping are enabled but the number of substeps is not valid. "
250  "Please be aware that this value needs to be in the range [1-16].");
251  }
252  double n_substeps = settings.fixed_delta_seconds.get() / settings.max_substep_delta_time;
253 
254  if (n_substeps > static_cast<double>(settings.max_substeps)) {
255  log_warning(
256  "synchronous mode and substepping are enabled but the values for the simulation are not valid. "
257  "The values should fulfil fixed_delta_seconds <= max_substep_delta_time * max_substeps. "
258  "Be very careful about that, the time deltas are not guaranteed.");
259  }
260  }
261  const auto frame = _client.SetEpisodeSettings(settings);
262 
263  using namespace std::literals::chrono_literals;
264  SynchronizeFrame(frame, *_episode, 1s);
265 
266  return frame;
267  }
268 
269  // ===========================================================================
270  // -- AI ---------------------------------------------------------------------
271  // ===========================================================================
272 
274  auto walker = controller.GetParent();
275  if (walker == nullptr) {
276  throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
277  return;
278  }
279  DEBUG_ASSERT(_episode != nullptr);
280  auto navigation = _episode->CreateNavigationIfMissing();
281  DEBUG_ASSERT(navigation != nullptr);
282  navigation->RegisterWalker(walker->GetId(), controller.GetId());
283  }
284 
286  auto walker = controller.GetParent();
287  if (walker == nullptr) {
288  throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
289  return;
290  }
291  DEBUG_ASSERT(_episode != nullptr);
292  auto navigation = _episode->CreateNavigationIfMissing();
293  DEBUG_ASSERT(navigation != nullptr);
294  navigation->UnregisterWalker(walker->GetId(), controller.GetId());
295  }
296 
297  boost::optional<geom::Location> Simulator::GetRandomLocationFromNavigation() {
298  DEBUG_ASSERT(_episode != nullptr);
299  auto navigation = _episode->CreateNavigationIfMissing();
300  DEBUG_ASSERT(navigation != nullptr);
301  return navigation->GetRandomLocation();
302  }
303 
304  void Simulator::SetPedestriansCrossFactor(float percentage) {
305  DEBUG_ASSERT(_episode != nullptr);
306  auto navigation = _episode->CreateNavigationIfMissing();
307  DEBUG_ASSERT(navigation != nullptr);
308  navigation->SetPedestriansCrossFactor(percentage);
309  }
310 
311  // ===========================================================================
312  // -- General operations with actors -----------------------------------------
313  // ===========================================================================
314 
316  const ActorBlueprint &blueprint,
317  const geom::Transform &transform,
318  Actor *parent,
319  rpc::AttachmentType attachment_type,
321  rpc::Actor actor;
322  if (parent != nullptr) {
324  blueprint.MakeActorDescription(),
325  transform,
326  parent->GetId(),
327  attachment_type);
328  } else {
329  actor = _client.SpawnActor(
330  blueprint.MakeActorDescription(),
331  transform);
332  }
333  DEBUG_ASSERT(_episode != nullptr);
334  _episode->RegisterActor(actor);
335  const auto gca = (gc == GarbageCollectionPolicy::Inherit ? _gc_policy : gc);
336  auto result = ActorFactory::MakeActor(GetCurrentEpisode(), actor, gca);
337  log_debug(
338  result->GetDisplayId(),
339  "created",
340  gca == GarbageCollectionPolicy::Enabled ? "with" : "without",
341  "garbage collection");
342  return result;
343  }
344 
346  bool success = true;
347  success = _client.DestroyActor(actor.GetId());
348  if (success) {
349  // Remove it's persistent state so it cannot access the client anymore.
350  actor.GetEpisode().Clear();
351  log_debug(actor.GetDisplayId(), "destroyed.");
352  } else {
353  log_debug("failed to destroy", actor.GetDisplayId());
354  }
355  return success;
356  }
357 
358  // ===========================================================================
359  // -- Operations with sensors ------------------------------------------------
360  // ===========================================================================
361 
363  const Sensor &sensor,
364  std::function<void(SharedPtr<sensor::SensorData>)> callback) {
365  DEBUG_ASSERT(_episode != nullptr);
368  [cb=std::move(callback), ep=WeakEpisodeProxy{shared_from_this()}](auto buffer) {
369  auto data = sensor::Deserializer::Deserialize(std::move(buffer));
370  data->_episode = ep.TryLock();
371  cb(std::move(data));
372  });
373  }
374 
377  }
378 
381  }
382 
383 } // namespace detail
384 } // namespace client
385 } // 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:232
std::shared_ptr< Episode > _episode
Definition: Simulator.h:667
bool DestroyActor(Actor &actor)
Definition: Simulator.cpp:345
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
bool SetFilesBaseFolder(const std::string &path)
Holds the current episode, and the current episode state.
Definition: Episode.h:33
bool SetFilesBaseFolder(const std::string &path)
Definition: Simulator.cpp:184
static void ValidateVersions(Client &client)
Definition: Simulator.cpp:36
std::vector< rpc::ActorDefinition > GetActorDefinitions()
void throw_exception(const std::exception &e)
Definition: Carla.cpp:135
void reverse(I begin, I end)
Definition: pugixml.cpp:7358
void CopyOpenDriveToServer(std::string opendrive, const rpc::OpendriveGenerationParameters &params)
std::vector< uint8_t > GetCacheFile(const std::string &name, const bool request_otherwise) const
Definition: Simulator.cpp:196
std::vector< uint8_t > GetCacheFile(const std::string &name, const bool request_otherwise=true) const
static SharedPtr< Actor > MakeActor(EpisodeProxy episode, rpc::Actor actor_description, GarbageCollectionPolicy garbage_collection_policy)
Create an Actor based on the provided actor_description.
std::vector< std::string > GetRequiredFiles(const std::string &folder="", const bool download=true) const
Definition: Simulator.cpp:188
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:665
std::vector< std::pair< ActorId, VehicleLightState::flag_type > > VehicleLightStateList
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
const GarbageCollectionPolicy _gc_policy
Definition: Simulator.h:669
rpc::EpisodeSettings GetEpisodeSettings()
SharedPtr< Actor > GetSpectator()
Definition: Simulator.cpp:236
std::vector< std::string > GetRequiredFiles(const std::string &folder="", const bool download=true) const
boost::optional< geom::Location > GetRandomLocationFromNavigation()
Definition: Simulator.cpp:297
void RequestFile(const std::string &name) const
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:375
const std::string & GetDisplayId() const
SharedPtr< Map > _cached_map
Definition: Simulator.h:671
uint64_t SetEpisodeSettings(const rpc::EpisodeSettings &settings)
Definition: Simulator.cpp:240
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
#define LIBCARLA_INITIALIZE_LIFETIME_PROFILER(display_name)
rpc::EpisodeSettings GetEpisodeSettings()
Definition: Simulator.h:232
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:315
static bool SynchronizeFrame(uint64_t frame, const Episode &episode, time_duration timeout)
Definition: Simulator.cpp:48
void FreezeAllTrafficLights(bool frozen)
Definition: Simulator.cpp:379
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:86
void SubscribeToSensor(const Sensor &sensor, std::function< void(SharedPtr< sensor::SensorData >)> callback)
Definition: Simulator.cpp:362
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:324
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:285
SharedPtr< Actor > GetParent() const
Definition: ActorState.cpp:31
uint64_t Tick(time_duration timeout)
Definition: Simulator.cpp:213
rpc::Actor SpawnActor(const rpc::ActorDescription &description, const geom::Transform &transform)
SharedPtr< Map > GetCurrentMap()
Definition: Simulator.cpp:153
bool DestroyActor(rpc::ActorId actor)
EpisodeProxy LoadOpenDriveEpisode(std::string opendrive, const rpc::OpendriveGenerationParameters &params, bool reset_settings=true)
Definition: Simulator.cpp:114
rpc::ActorDescription MakeActorDescription() const
Represents an actor in the simulation.
Definition: client/Actor.h:18
void RequestFile(const std::string &name) const
Definition: Simulator.cpp:192
Positive time duration up to milliseconds resolution.
Definition: Time.h:19
std::string name
Definition: MapInfo.h:21
void UnSubscribeFromStream(const streaming::Token &token)
Contains all the necessary information for spawning an Actor.
void RegisterAIController(const WalkerAIController &controller)
Definition: Simulator.cpp:273
void SetPedestriansCrossFactor(float percentage)
Definition: Simulator.cpp:304
SharedPtr< BlueprintLibrary > GetBlueprintLibrary()
Definition: Simulator.cpp:227
constexpr auto to_chrono() const
Definition: Time.h:50
EpisodeProxyImpl< EpisodeProxyPointerType::Weak > WeakEpisodeProxy
Definition: EpisodeProxy.h:71
bool ShouldUpdateMap(rpc::MapInfo &map_info)
Definition: Simulator.cpp:142
const std::string GetEndpoint() const
static bool FileExists(std::string file)
WorldSnapshot WaitForTick(time_duration timeout)
Definition: Simulator.cpp:204
const rpc::Actor & GetActorDescription() const
uint64_t SetEpisodeSettings(const rpc::EpisodeSettings &settings)