CARLA
CarlaServer.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 
7 #include "Carla.h"
10 #include "EngineUtils.h"
11 
15 #include "Carla/Util/RayTracer.h"
19 #include "GameFramework/CharacterMovementComponent.h"
20 #include "Carla/Game/Tagger.h"
24 #include "Carla/Actor/ActorData.h"
25 #include "CarlaServerResponse.h"
27 #include "Misc/FileHelper.h"
28 
30 #include <carla/Functional.h>
31 #include <carla/Version.h>
32 #include <carla/rpc/Actor.h>
36 #include <carla/rpc/Command.h>
38 #include <carla/rpc/DebugShape.h>
40 #include <carla/rpc/EpisodeInfo.h>
43 #include <carla/rpc/LightState.h>
44 #include <carla/rpc/MapInfo.h>
45 #include <carla/rpc/MapLayer.h>
46 #include <carla/rpc/Response.h>
47 #include <carla/rpc/Server.h>
48 #include <carla/rpc/String.h>
49 #include <carla/rpc/Transform.h>
50 #include <carla/rpc/Vector2D.h>
51 #include <carla/rpc/Vector3D.h>
52 #include <carla/rpc/VehicleDoor.h>
62 #include <carla/streaming/Server.h>
63 #include <carla/rpc/Texture.h>
66 
67 #include <vector>
68 #include <map>
69 #include <tuple>
70 
71 template <typename T>
73 
74 // =============================================================================
75 // -- Static local functions ---------------------------------------------------
76 // =============================================================================
77 
78 template <typename T, typename Other>
79 static std::vector<T> MakeVectorFromTArray(const TArray<Other> &Array)
80 {
81  return {Array.GetData(), Array.GetData() + Array.Num()};
82 }
83 
84 // =============================================================================
85 // -- FCarlaServer::FPimpl -----------------------------------------------
86 // =============================================================================
87 
89 {
90 public:
91 
92  FPimpl(uint16_t RPCPort, uint16_t StreamingPort)
93  : Server(RPCPort),
94  StreamingServer(StreamingPort),
95  BroadcastStream(StreamingServer.MakeStream())
96  {
97  BindActions();
98  }
99 
100  /// Map of pairs < port , ip > with all the Traffic Managers active in the simulation
101  std::map<uint16_t, std::string> TrafficManagerInfo;
102 
104 
106 
108 
109  UCarlaEpisode *Episode = nullptr;
110 
111  size_t TickCuesReceived = 0u;
112 
113 private:
114 
115  void BindActions();
116 };
117 
118 // =============================================================================
119 // -- Define helper macros -----------------------------------------------------
120 // =============================================================================
121 
122 #if WITH_EDITOR
123 # define CARLA_ENSURE_GAME_THREAD() check(IsInGameThread());
124 #else
125 # define CARLA_ENSURE_GAME_THREAD()
126 #endif // WITH_EDITOR
127 
128 #define RESPOND_ERROR(str) { \
129  UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), TEXT(str)); \
130  return carla::rpc::ResponseError(str); }
131 
132 #define RESPOND_ERROR_FSTRING(fstr) { \
133  UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), *fstr); \
134  return carla::rpc::ResponseError(carla::rpc::FromFString(fstr)); }
135 
136 #define REQUIRE_CARLA_EPISODE() \
137  CARLA_ENSURE_GAME_THREAD(); \
138  if (Episode == nullptr) { RESPOND_ERROR("episode not ready"); }
139 
141  const FString& FuncName,
142  const FString& ErrorMessage,
143  const FString& ExtraInfo = "")
144 {
145  FString TotalMessage = "Responding error from function " + FuncName + ": " +
146  ErrorMessage + ". " + ExtraInfo;
147  UE_LOG(LogCarlaServer, Log, TEXT("%s"), *TotalMessage);
148  return carla::rpc::ResponseError(carla::rpc::FromFString(TotalMessage));
149 }
150 
152  const FString& FuncName,
153  const ECarlaServerResponse& Error,
154  const FString& ExtraInfo = "")
155 {
156  return RespondError(FuncName, GetStringError(Error), ExtraInfo);
157 }
158 
160 {
161 public:
162 
163  constexpr ServerBinder(const char *name, carla::rpc::Server &srv, bool sync)
164  : _name(name),
165  _server(srv),
166  _sync(sync) {}
167 
168  template <typename FuncT>
169  auto operator<<(FuncT func)
170  {
171  if (_sync)
172  {
173  _server.BindSync(_name, func);
174  }
175  else
176  {
177  _server.BindAsync(_name, func);
178  }
179  return func;
180  }
181 
182 private:
183 
184  const char *_name;
185 
187 
188  bool _sync;
189 };
190 
191 #define BIND_SYNC(name) auto name = ServerBinder(# name, Server, true)
192 #define BIND_ASYNC(name) auto name = ServerBinder(# name, Server, false)
193 
194 // =============================================================================
195 // -- Bind Actions -------------------------------------------------------------
196 // =============================================================================
197 
199 {
200  namespace cr = carla::rpc;
201  namespace cg = carla::geom;
202 
203  /// Looks for a Traffic Manager running on port
204  BIND_SYNC(is_traffic_manager_running) << [this] (uint16_t port) ->R<bool>
205  {
206  return (TrafficManagerInfo.find(port) != TrafficManagerInfo.end());
207  };
208 
209  /// Gets a pair filled with the <IP, port> of the Trafic Manager running on port.
210  /// If there is no Traffic Manager running the pair will be ("", 0)
211  BIND_SYNC(get_traffic_manager_running) << [this] (uint16_t port) ->R<std::pair<std::string, uint16_t>>
212  {
213  auto it = TrafficManagerInfo.find(port);
214  if(it != TrafficManagerInfo.end()) {
215  return std::pair<std::string, uint16_t>(it->second, it->first);
216  }
217  return std::pair<std::string, uint16_t>("",0);
218  };
219 
220  /// Add a new Traffic Manager running on <IP, port>
221  BIND_SYNC(add_traffic_manager_running) << [this] (std::pair<std::string, uint16_t> trafficManagerInfo) ->R<bool>
222  {
223  uint16_t port = trafficManagerInfo.second;
224  auto it = TrafficManagerInfo.find(port);
225  if(it == TrafficManagerInfo.end()) {
226  TrafficManagerInfo.insert(
227  std::pair<uint16_t, std::string>(port, trafficManagerInfo.first));
228  return true;
229  }
230  return false;
231 
232  };
233 
234  BIND_SYNC(destroy_traffic_manager) << [this] (uint16_t port) ->R<bool>
235  {
236  auto it = TrafficManagerInfo.find(port);
237  if(it != TrafficManagerInfo.end()) {
238  TrafficManagerInfo.erase(it);
239  return true;
240  }
241  return false;
242  };
243 
244  BIND_ASYNC(version) << [] () -> R<std::string>
245  {
246  return carla::version();
247  };
248 
249  // ~~ Tick ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
250 
251  BIND_SYNC(tick_cue) << [this]() -> R<uint64_t>
252  {
253  TRACE_CPUPROFILER_EVENT_SCOPE(TickCueReceived);
256  };
257 
258  // ~~ Load new episode ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
259 
260  BIND_ASYNC(get_available_maps) << [this]() -> R<std::vector<std::string>>
261  {
262  const auto MapNames = UCarlaStatics::GetAllMapNames();
263  std::vector<std::string> result;
264  result.reserve(MapNames.Num());
265  for (const auto &MapName : MapNames)
266  {
267  if (MapName.Contains("/Sublevels/"))
268  continue;
269  if (MapName.Contains("/BaseMap/"))
270  continue;
271  if (MapName.Contains("/BaseLargeMap/"))
272  continue;
273  if (MapName.Contains("_Tile_"))
274  continue;
275 
276  result.emplace_back(cr::FromFString(MapName));
277  }
278  return result;
279  };
280 
281  BIND_SYNC(load_new_episode) << [this](const std::string &map_name, const bool reset_settings, cr::MapLayer MapLayers) -> R<void>
282  {
284 
285  UCarlaGameInstance* GameInstance = UCarlaStatics::GetGameInstance(Episode->GetWorld());
286  if (!GameInstance)
287  {
288  RESPOND_ERROR("unable to find CARLA game instance");
289  }
290  GameInstance->SetMapLayer(static_cast<int32>(MapLayers));
291 
292  if(!Episode->LoadNewEpisode(cr::ToFString(map_name), reset_settings))
293  {
294  RESPOND_ERROR("map not found");
295  }
296 
297  return R<void>::Success();
298  };
299 
300  BIND_SYNC(load_map_layer) << [this](cr::MapLayer MapLayers) -> R<void>
301  {
303 
304  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
305  if (!GameMode)
306  {
307  RESPOND_ERROR("unable to find CARLA game mode");
308  }
309  GameMode->LoadMapLayer(static_cast<int32>(MapLayers));
310 
311  return R<void>::Success();
312  };
313 
314  BIND_SYNC(unload_map_layer) << [this](cr::MapLayer MapLayers) -> R<void>
315  {
317 
318  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
319  if (!GameMode)
320  {
321  RESPOND_ERROR("unable to find CARLA game mode");
322  }
323  GameMode->UnLoadMapLayer(static_cast<int32>(MapLayers));
324 
325  return R<void>::Success();
326  };
327 
328  BIND_SYNC(copy_opendrive_to_file) << [this](const std::string &opendrive, cr::OpendriveGenerationParameters Params) -> R<void>
329  {
331  if (!Episode->LoadNewOpendriveEpisode(cr::ToLongFString(opendrive), Params))
332  {
333  RESPOND_ERROR("opendrive could not be correctly parsed");
334  }
335  return R<void>::Success();
336  };
337 
338  BIND_SYNC(apply_color_texture_to_objects) << [this](
339  const std::vector<std::string> &actors_name,
340  const cr::MaterialParameter& parameter,
341  const cr::TextureColor& Texture) -> R<void>
342  {
344  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
345  if (!GameMode)
346  {
347  RESPOND_ERROR("unable to find CARLA game mode");
348  }
349  TArray<AActor*> ActorsToPaint;
350  for(const std::string& actor_name : actors_name)
351  {
352  AActor* ActorToPaint = GameMode->FindActorByName(cr::ToFString(actor_name));
353  if (ActorToPaint)
354  {
355  ActorsToPaint.Add(ActorToPaint);
356  }
357  }
358 
359  if(!ActorsToPaint.Num())
360  {
361  RESPOND_ERROR("unable to find Actor to apply the texture");
362  }
363 
364  UTexture2D* UETexture = GameMode->CreateUETexture(Texture);
365 
366  for(AActor* ActorToPaint : ActorsToPaint)
367  {
368  GameMode->ApplyTextureToActor(
369  ActorToPaint,
370  UETexture,
371  parameter);
372  }
373  return R<void>::Success();
374  };
375 
376  BIND_SYNC(apply_float_color_texture_to_objects) << [this](
377  const std::vector<std::string> &actors_name,
378  const cr::MaterialParameter& parameter,
379  const cr::TextureFloatColor& Texture) -> R<void>
380  {
382  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
383  if (!GameMode)
384  {
385  RESPOND_ERROR("unable to find CARLA game mode");
386  }
387  TArray<AActor*> ActorsToPaint;
388  for(const std::string& actor_name : actors_name)
389  {
390  AActor* ActorToPaint = GameMode->FindActorByName(cr::ToFString(actor_name));
391  if (ActorToPaint)
392  {
393  ActorsToPaint.Add(ActorToPaint);
394  }
395  }
396 
397  if(!ActorsToPaint.Num())
398  {
399  RESPOND_ERROR("unable to find Actor to apply the texture");
400  }
401 
402  UTexture2D* UETexture = GameMode->CreateUETexture(Texture);
403 
404  for(AActor* ActorToPaint : ActorsToPaint)
405  {
406  GameMode->ApplyTextureToActor(
407  ActorToPaint,
408  UETexture,
409  parameter);
410  }
411  return R<void>::Success();
412  };
413 
414  BIND_SYNC(get_names_of_all_objects) << [this]() -> R<std::vector<std::string>>
415  {
417  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
418  if (!GameMode)
419  {
420  RESPOND_ERROR("unable to find CARLA game mode");
421  }
422  TArray<FString> NamesFString = GameMode->GetNamesOfAllActors();
423  std::vector<std::string> NamesStd;
424  for (const FString &Name : NamesFString)
425  {
426  NamesStd.emplace_back(cr::FromFString(Name));
427  }
428  return NamesStd;
429  };
430 
431  // ~~ Episode settings and info ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
432 
433  BIND_SYNC(get_episode_info) << [this]() -> R<cr::EpisodeInfo>
434  {
436  return cr::EpisodeInfo{Episode->GetId(), BroadcastStream.token()};
437  };
438 
439  BIND_SYNC(get_map_info) << [this]() -> R<cr::MapInfo>
440  {
442  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
443  const auto &SpawnPoints = Episode->GetRecommendedSpawnPoints();
444  FString FullMapPath = GameMode->GetFullMapPath();
445  FString MapDir = FullMapPath.RightChop(FullMapPath.Find("Content/", ESearchCase::CaseSensitive) + 8);
446  MapDir += "/" + Episode->GetMapName();
447  return cr::MapInfo{
448  cr::FromFString(MapDir),
449  MakeVectorFromTArray<cg::Transform>(SpawnPoints)};
450  };
451 
452  BIND_SYNC(get_map_data) << [this]() -> R<std::string>
453  {
455  return cr::FromLongFString(UOpenDrive::GetXODR(Episode->GetWorld()));
456  };
457 
458  BIND_SYNC(get_navigation_mesh) << [this]() -> R<std::vector<uint8_t>>
459  {
461  auto FileContents = FNavigationMesh::Load(Episode->GetMapName());
462  // make a mem copy (from TArray to std::vector)
463  std::vector<uint8_t> Result(FileContents.Num());
464  memcpy(&Result[0], FileContents.GetData(), FileContents.Num());
465  return Result;
466  };
467 
468  BIND_SYNC(get_required_files) << [this](std::string folder = "") -> R<std::vector<std::string>>
469  {
471 
472  // Check that the path ends in a slash, add it otherwise
473  if (folder[folder.size() - 1] != '/' && folder[folder.size() - 1] != '\\') {
474  folder += "/";
475  }
476 
477  // Get the map's folder absolute path and check if it's in its own folder
478  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
479  const auto mapDir = GameMode->GetFullMapPath();
480  const auto folderDir = mapDir + "/" + folder.c_str();
481  const auto fileName = mapDir.EndsWith(Episode->GetMapName()) ? "*" : Episode->GetMapName();
482 
483  // Find all the xodr and bin files from the map
484  TArray<FString> Files;
485  IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName + ".xodr"), true, false, false);
486  IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName + ".bin"), true, false, false);
487 
488  // Remove the start of the path until the content folder and put each file in the result
489  std::vector<std::string> result;
490  for (auto File : Files) {
491  File.RemoveFromStart(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
492  result.emplace_back(TCHAR_TO_UTF8(*File));
493  }
494 
495  return result;
496  };
497  BIND_SYNC(request_file) << [this](std::string name) -> R<std::vector<uint8_t>>
498  {
500 
501  // Get the absolute path of the file
502  FString path(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
503  path.Append(name.c_str());
504 
505  // Copy the binary data of the file into the result and return it
506  TArray<uint8_t> Content;
507  FFileHelper::LoadFileToArray(Content, *path, 0);
508  std::vector<uint8_t> Result(Content.Num());
509  memcpy(&Result[0], Content.GetData(), Content.Num());
510 
511  return Result;
512  };
513 
514  BIND_SYNC(get_episode_settings) << [this]() -> R<cr::EpisodeSettings>
515  {
517  return cr::EpisodeSettings{Episode->GetSettings()};
518  };
519 
520  BIND_SYNC(set_episode_settings) << [this](
521  const cr::EpisodeSettings &settings) -> R<uint64_t>
522  {
524  Episode->ApplySettings(settings);
525  StreamingServer.SetSynchronousMode(settings.synchronous_mode);
527  };
528 
529  BIND_SYNC(get_actor_definitions) << [this]() -> R<std::vector<cr::ActorDefinition>>
530  {
532  return MakeVectorFromTArray<cr::ActorDefinition>(Episode->GetActorDefinitions());
533  };
534 
535  BIND_SYNC(get_spectator) << [this]() -> R<cr::Actor>
536  {
539  if (!CarlaActor)
540  {
541  RESPOND_ERROR("internal error: unable to find spectator");
542  }
543  return Episode->SerializeActor(CarlaActor);
544  };
545 
546  BIND_SYNC(get_all_level_BBs) << [this](uint8 QueriedTag) -> R<std::vector<cg::BoundingBox>>
547  {
549  TArray<FBoundingBox> Result;
550  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
551  if (!GameMode)
552  {
553  RESPOND_ERROR("unable to find CARLA game mode");
554  }
555  Result = GameMode->GetAllBBsOfLevel(QueriedTag);
556  ALargeMapManager* LargeMap = GameMode->GetLMManager();
557  if (LargeMap)
558  {
559  for(auto& Box : Result)
560  {
561  Box.Origin = LargeMap->LocalToGlobalLocation(Box.Origin);
562  }
563  }
564  return MakeVectorFromTArray<cg::BoundingBox>(Result);
565  };
566 
567  BIND_SYNC(get_environment_objects) << [this](uint8 QueriedTag) -> R<std::vector<cr::EnvironmentObject>>
568  {
570  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
571  if (!GameMode)
572  {
573  RESPOND_ERROR("unable to find CARLA game mode");
574  }
575  TArray<FEnvironmentObject> Result = GameMode->GetEnvironmentObjects(QueriedTag);
576  ALargeMapManager* LargeMap = GameMode->GetLMManager();
577  if (LargeMap)
578  {
579  for(auto& Object : Result)
580  {
581  Object.Transform = LargeMap->LocalToGlobalTransform(Object.Transform);
582  }
583  }
584  return MakeVectorFromTArray<cr::EnvironmentObject>(Result);
585  };
586 
587  BIND_SYNC(enable_environment_objects) << [this](std::vector<uint64_t> EnvObjectIds, bool Enable) -> R<void>
588  {
590  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
591  if (!GameMode)
592  {
593  RESPOND_ERROR("unable to find CARLA game mode");
594  }
595 
596  TSet<uint64> EnvObjectIdsSet;
597  for(uint64 Id : EnvObjectIds)
598  {
599  EnvObjectIdsSet.Emplace(Id);
600  }
601 
602  GameMode->EnableEnvironmentObjects(EnvObjectIdsSet, Enable);
603  return R<void>::Success();
604  };
605 
606  // ~~ Weather ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
607 
608  BIND_SYNC(get_weather_parameters) << [this]() -> R<cr::WeatherParameters>
609  {
611  auto *Weather = Episode->GetWeather();
612  if (Weather == nullptr)
613  {
614  RESPOND_ERROR("internal error: unable to find weather");
615  }
616  return Weather->GetCurrentWeather();
617  };
618 
619  BIND_SYNC(set_weather_parameters) << [this](
620  const cr::WeatherParameters &weather) -> R<void>
621  {
623  auto *Weather = Episode->GetWeather();
624  if (Weather == nullptr)
625  {
626  RESPOND_ERROR("internal error: unable to find weather");
627  }
628  Weather->ApplyWeather(weather);
629  return R<void>::Success();
630  };
631 
632  // ~~ Actor operations ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
633 
634  BIND_SYNC(get_actors_by_id) << [this](
635  const std::vector<FCarlaActor::IdType> &ids) -> R<std::vector<cr::Actor>>
636  {
638  std::vector<cr::Actor> Result;
639  Result.reserve(ids.size());
640  for (auto &&Id : ids)
641  {
642  FCarlaActor* View = Episode->FindCarlaActor(Id);
643  if (View)
644  {
645  Result.emplace_back(Episode->SerializeActor(View));
646  }
647  }
648  return Result;
649  };
650 
651  BIND_SYNC(spawn_actor) << [this](
652  cr::ActorDescription Description,
654  {
656 
657  auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description));
658 
659  if (Result.Key != EActorSpawnResultStatus::Success)
660  {
661  UE_LOG(LogCarla, Error, TEXT("Actor not Spawned"));
663  }
664 
666  if(LargeMap)
667  {
668  LargeMap->OnActorSpawned(*Result.Value);
669  }
670 
671  return Episode->SerializeActor(Result.Value);
672  };
673 
674  BIND_SYNC(spawn_actor_with_parent) << [this](
675  cr::ActorDescription Description,
676  const cr::Transform &Transform,
677  cr::ActorId ParentId,
678  cr::AttachmentType InAttachmentType) -> R<cr::Actor>
679  {
681 
682  auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description));
683  if (Result.Key != EActorSpawnResultStatus::Success)
684  {
686  }
687 
688  FCarlaActor* CarlaActor = Episode->FindCarlaActor(Result.Value->GetActorId());
689  if (!CarlaActor)
690  {
691  RESPOND_ERROR("internal error: actor could not be spawned");
692  }
693 
694  FCarlaActor* ParentCarlaActor = Episode->FindCarlaActor(ParentId);
695 
696  if (!ParentCarlaActor)
697  {
698  RESPOND_ERROR("unable to attach actor: parent actor not found");
699  }
700 
701  CarlaActor->SetParent(ParentId);
702  CarlaActor->SetAttachmentType(InAttachmentType);
703  ParentCarlaActor->AddChildren(CarlaActor->GetActorId());
704 
705  // Only is possible to attach if the actor has been really spawned and
706  // is not in dormant state
707  if(!ParentCarlaActor->IsDormant())
708  {
710  CarlaActor->GetActor(),
711  ParentCarlaActor->GetActor(),
712  static_cast<EAttachmentType>(InAttachmentType));
713  }
714  else
715  {
716  Episode->PutActorToSleep(CarlaActor->GetActorId());
717  }
718 
719  return Episode->SerializeActor(CarlaActor);
720  };
721 
722  BIND_SYNC(destroy_actor) << [this](cr::ActorId ActorId) -> R<bool>
723  {
725  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
726  if ( !CarlaActor )
727  {
728  RESPOND_ERROR("unable to destroy actor: not found");
729  }
730  UE_LOG(LogCarla, Log, TEXT("CarlaServer destroy_actor %d"), ActorId);
731  // We need to force the actor state change, since dormant actors
732  // will ignore the FCarlaActor destruction
733  CarlaActor->SetActorState(cr::ActorState::PendingKill);
735  {
736  RESPOND_ERROR("internal error: unable to destroy actor");
737  }
738  return true;
739  };
740 
741  // ~~ Actor physics ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
742 
743  BIND_SYNC(set_actor_location) << [this](
746  {
748  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
749  if (!CarlaActor)
750  {
751  return RespondError(
752  "set_actor_location",
754  " Actor Id: " + FString::FromInt(ActorId));
755  }
756 
757  CarlaActor->SetActorGlobalLocation(
758  Location, ETeleportType::TeleportPhysics);
759  return R<void>::Success();
760  };
761 
762  BIND_SYNC(set_actor_transform) << [this](
765  {
767  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
768  if (!CarlaActor)
769  {
770  return RespondError(
771  "set_actor_transform",
773  " Actor Id: " + FString::FromInt(ActorId));
774  }
775 
776  CarlaActor->SetActorGlobalTransform(
777  Transform, ETeleportType::TeleportPhysics);
778  return R<void>::Success();
779  };
780 
781  BIND_SYNC(set_walker_state) << [this] (
784  float Speed) -> R<void>
785  {
787  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
788  if (!CarlaActor)
789  {
790  return RespondError(
791  "set_walker_state",
793  " Actor Id: " + FString::FromInt(ActorId));
794  }
795 
796  // apply walker transform
797  ECarlaServerResponse Response =
798  CarlaActor->SetWalkerState(
799  Transform,
800  cr::WalkerControl(
801  Transform.GetForwardVector(), Speed, false));
802  if (Response != ECarlaServerResponse::Success)
803  {
804  return RespondError(
805  "set_walker_state",
806  Response,
807  " Actor Id: " + FString::FromInt(ActorId));
808  }
809  return R<void>::Success();
810  };
811 
812  BIND_SYNC(set_actor_target_velocity) << [this](
814  cr::Vector3D vector) -> R<void>
815  {
817  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
818  if (!CarlaActor)
819  {
820  return RespondError(
821  "set_actor_target_velocity",
823  " Actor Id: " + FString::FromInt(ActorId));
824  }
825  ECarlaServerResponse Response =
826  CarlaActor->SetActorTargetVelocity(vector.ToCentimeters().ToFVector());
827  if (Response != ECarlaServerResponse::Success)
828  {
829  return RespondError(
830  "set_actor_target_velocity",
831  Response,
832  " Actor Id: " + FString::FromInt(ActorId));
833  }
834  return R<void>::Success();
835  };
836 
837  BIND_SYNC(set_actor_target_angular_velocity) << [this](
839  cr::Vector3D vector) -> R<void>
840  {
842  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
843  if (!CarlaActor)
844  {
845  return RespondError(
846  "set_actor_target_angular_velocity",
848  " Actor Id: " + FString::FromInt(ActorId));
849  }
850  ECarlaServerResponse Response =
851  CarlaActor->SetActorTargetAngularVelocity(vector.ToFVector());
852  if (Response != ECarlaServerResponse::Success)
853  {
854  return RespondError(
855  "set_actor_target_angular_velocity",
856  Response,
857  " Actor Id: " + FString::FromInt(ActorId));
858  }
859  return R<void>::Success();
860  };
861 
862  BIND_SYNC(enable_actor_constant_velocity) << [this](
864  cr::Vector3D vector) -> R<void>
865  {
867  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
868  if (!CarlaActor)
869  {
870  return RespondError(
871  "enable_actor_constant_velocity",
873  " Actor Id: " + FString::FromInt(ActorId));
874  }
875 
876  ECarlaServerResponse Response =
877  CarlaActor->EnableActorConstantVelocity(vector.ToCentimeters().ToFVector());
878  if (Response != ECarlaServerResponse::Success)
879  {
880  return RespondError(
881  "enable_actor_constant_velocity",
882  Response,
883  " Actor Id: " + FString::FromInt(ActorId));
884  }
885 
886  return R<void>::Success();
887  };
888 
889  BIND_SYNC(disable_actor_constant_velocity) << [this](
891  {
893  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
894  if (!CarlaActor)
895  {
896  return RespondError(
897  "disable_actor_constant_velocity",
899  " Actor Id: " + FString::FromInt(ActorId));
900  }
901 
902  ECarlaServerResponse Response =
903  CarlaActor->DisableActorConstantVelocity();
904  if (Response != ECarlaServerResponse::Success)
905  {
906  return RespondError(
907  "disable_actor_constant_velocity",
908  Response,
909  " Actor Id: " + FString::FromInt(ActorId));
910  }
911 
912  return R<void>::Success();
913  };
914 
915  BIND_SYNC(add_actor_impulse) << [this](
917  cr::Vector3D vector) -> R<void>
918  {
920  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
921  if (!CarlaActor)
922  {
923  return RespondError(
924  "add_actor_impulse",
926  " Actor Id: " + FString::FromInt(ActorId));
927  }
928 
929  ECarlaServerResponse Response =
930  CarlaActor->AddActorImpulse(vector.ToCentimeters().ToFVector());
931  if (Response != ECarlaServerResponse::Success)
932  {
933  return RespondError(
934  "add_actor_impulse",
935  Response,
936  " Actor Id: " + FString::FromInt(ActorId));
937  }
938  return R<void>::Success();
939  };
940 
941  BIND_SYNC(add_actor_impulse_at_location) << [this](
943  cr::Vector3D impulse,
944  cr::Vector3D location) -> R<void>
945  {
947  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
948  if (!CarlaActor)
949  {
950  return RespondError(
951  "add_actor_impulse_at_location",
953  " Actor Id: " + FString::FromInt(ActorId));
954  }
955  FVector UELocation = location.ToCentimeters().ToFVector();
956  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
957  ALargeMapManager* LargeMap = GameMode->GetLMManager();
958  if (LargeMap)
959  {
960  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
961  }
962  ECarlaServerResponse Response =
963  CarlaActor->AddActorImpulseAtLocation(impulse.ToCentimeters().ToFVector(), UELocation);
964  if (Response != ECarlaServerResponse::Success)
965  {
966  return RespondError(
967  "add_actor_impulse_at_location",
968  Response,
969  " Actor Id: " + FString::FromInt(ActorId));
970  }
971 
972  return R<void>::Success();
973  };
974 
975  BIND_SYNC(add_actor_force) << [this](
977  cr::Vector3D vector) -> R<void>
978  {
980  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
981  if (!CarlaActor)
982  {
983  return RespondError(
984  "add_actor_force",
986  " Actor Id: " + FString::FromInt(ActorId));
987  }
988  ECarlaServerResponse Response =
989  CarlaActor->AddActorForce(vector.ToCentimeters().ToFVector());
990  if (Response != ECarlaServerResponse::Success)
991  {
992  return RespondError(
993  "add_actor_force",
994  Response,
995  " Actor Id: " + FString::FromInt(ActorId));
996  }
997  return R<void>::Success();
998  };
999 
1000  BIND_SYNC(add_actor_force_at_location) << [this](
1002  cr::Vector3D force,
1003  cr::Vector3D location) -> R<void>
1004  {
1006  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1007  if (!CarlaActor)
1008  {
1009  return RespondError(
1010  "add_actor_force_at_location",
1012  " Actor Id: " + FString::FromInt(ActorId));
1013  }
1014  FVector UELocation = location.ToCentimeters().ToFVector();
1015  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
1016  ALargeMapManager* LargeMap = GameMode->GetLMManager();
1017  if (LargeMap)
1018  {
1019  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
1020  }
1021  ECarlaServerResponse Response =
1022  CarlaActor->AddActorForceAtLocation(UELocation, force.ToCentimeters().ToFVector());
1023  if (Response != ECarlaServerResponse::Success)
1024  {
1025  return RespondError(
1026  "add_actor_force_at_location",
1027  Response,
1028  " Actor Id: " + FString::FromInt(ActorId));
1029  }
1030  return R<void>::Success();
1031  };
1032 
1033  BIND_SYNC(add_actor_angular_impulse) << [this](
1035  cr::Vector3D vector) -> R<void>
1036  {
1038  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1039  if (!CarlaActor)
1040  {
1041  return RespondError(
1042  "add_actor_angular_impulse",
1044  " Actor Id: " + FString::FromInt(ActorId));
1045  }
1046  ECarlaServerResponse Response =
1047  CarlaActor->AddActorAngularImpulse(vector.ToFVector());
1048  if (Response != ECarlaServerResponse::Success)
1049  {
1050  return RespondError(
1051  "add_actor_angular_impulse",
1052  Response,
1053  " Actor Id: " + FString::FromInt(ActorId));
1054  }
1055  return R<void>::Success();
1056  };
1057 
1058  BIND_SYNC(add_actor_torque) << [this](
1060  cr::Vector3D vector) -> R<void>
1061  {
1063  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1064  if (!CarlaActor)
1065  {
1066  return RespondError(
1067  "add_actor_torque",
1069  " Actor Id: " + FString::FromInt(ActorId));
1070  }
1071  ECarlaServerResponse Response =
1072  CarlaActor->AddActorTorque(vector.ToFVector());
1073  if (Response != ECarlaServerResponse::Success)
1074  {
1075  return RespondError(
1076  "add_actor_torque",
1077  Response,
1078  " Actor Id: " + FString::FromInt(ActorId));
1079  }
1080  return R<void>::Success();
1081  };
1082 
1083  BIND_SYNC(get_physics_control) << [this](
1085  {
1087  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1088  if (!CarlaActor)
1089  {
1090  return RespondError(
1091  "get_physics_control",
1093  " Actor Id: " + FString::FromInt(ActorId));
1094  }
1096  ECarlaServerResponse Response =
1097  CarlaActor->GetPhysicsControl(PhysicsControl);
1098  if (Response != ECarlaServerResponse::Success)
1099  {
1100  return RespondError(
1101  "get_physics_control",
1102  Response,
1103  " Actor Id: " + FString::FromInt(ActorId));
1104  }
1105  return cr::VehiclePhysicsControl(PhysicsControl);
1106  };
1107 
1108  BIND_SYNC(get_vehicle_light_state) << [this](
1110  {
1112  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1113  if (!CarlaActor)
1114  {
1115  return RespondError(
1116  "get_vehicle_light_state",
1118  " Actor Id: " + FString::FromInt(ActorId));
1119  }
1120  FVehicleLightState LightState;
1121  ECarlaServerResponse Response =
1122  CarlaActor->GetVehicleLightState(LightState);
1123  if (Response != ECarlaServerResponse::Success)
1124  {
1125  return RespondError(
1126  "get_vehicle_light_state",
1127  Response,
1128  " Actor Id: " + FString::FromInt(ActorId));
1129  }
1130  return cr::VehicleLightState(LightState);
1131  };
1132 
1133  BIND_SYNC(apply_physics_control) << [this](
1135  cr::VehiclePhysicsControl PhysicsControl) -> R<void>
1136  {
1138  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1139  if (!CarlaActor)
1140  {
1141  return RespondError(
1142  "apply_physics_control",
1144  " Actor Id: " + FString::FromInt(ActorId));
1145  }
1146  ECarlaServerResponse Response =
1148  if (Response != ECarlaServerResponse::Success)
1149  {
1150  return RespondError(
1151  "apply_physics_control",
1152  Response,
1153  " Actor Id: " + FString::FromInt(ActorId));
1154  }
1155  return R<void>::Success();
1156  };
1157 
1158  BIND_SYNC(set_vehicle_light_state) << [this](
1160  cr::VehicleLightState LightState) -> R<void>
1161  {
1163  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1164  if (!CarlaActor)
1165  {
1166  return RespondError(
1167  "set_vehicle_light_state",
1169  " Actor Id: " + FString::FromInt(ActorId));
1170  }
1171  ECarlaServerResponse Response =
1172  CarlaActor->SetVehicleLightState(FVehicleLightState(LightState));
1173  if (Response != ECarlaServerResponse::Success)
1174  {
1175  return RespondError(
1176  "set_vehicle_light_state",
1177  Response,
1178  " Actor Id: " + FString::FromInt(ActorId));
1179  }
1180  return R<void>::Success();
1181  };
1182 
1183 
1184  BIND_SYNC(open_vehicle_door) << [this](
1186  cr::VehicleDoor DoorIdx) -> R<void>
1187  {
1189  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1190  if (!CarlaActor)
1191  {
1192  return RespondError(
1193  "open_vehicle_door",
1195  " Actor Id: " + FString::FromInt(ActorId));
1196  }
1197  ECarlaServerResponse Response =
1198  CarlaActor->OpenVehicleDoor(static_cast<EVehicleDoor>(DoorIdx));
1199  if (Response != ECarlaServerResponse::Success)
1200  {
1201  return RespondError(
1202  "open_vehicle_door",
1203  Response,
1204  " Actor Id: " + FString::FromInt(ActorId));
1205  }
1206  return R<void>::Success();
1207  };
1208 
1209  BIND_SYNC(close_vehicle_door) << [this](
1211  cr::VehicleDoor DoorIdx) -> R<void>
1212  {
1214  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1215  if (!CarlaActor)
1216  {
1217  return RespondError(
1218  "close_vehicle_door",
1220  " Actor Id: " + FString::FromInt(ActorId));
1221  }
1222  ECarlaServerResponse Response =
1223  CarlaActor->CloseVehicleDoor(static_cast<EVehicleDoor>(DoorIdx));
1224  if (Response != ECarlaServerResponse::Success)
1225  {
1226  return RespondError(
1227  "close_vehicle_door",
1228  Response,
1229  " Actor Id: " + FString::FromInt(ActorId));
1230  }
1231  return R<void>::Success();
1232  };
1233 
1234  BIND_SYNC(set_wheel_steer_direction) << [this](
1236  cr::VehicleWheelLocation WheelLocation,
1237  float AngleInDeg) -> R<void>
1238  {
1240  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1241  if(!CarlaActor){
1242  return RespondError(
1243  "set_wheel_steer_direction",
1245  " Actor Id: " + FString::FromInt(ActorId));
1246  }
1247  ECarlaServerResponse Response =
1248  CarlaActor->SetWheelSteerDirection(
1249  static_cast<EVehicleWheelLocation>(WheelLocation), AngleInDeg);
1250  if (Response != ECarlaServerResponse::Success)
1251  {
1252  return RespondError(
1253  "set_wheel_steer_direction",
1254  Response,
1255  " Actor Id: " + FString::FromInt(ActorId));
1256  }
1257  return R<void>::Success();
1258  };
1259 
1260  BIND_SYNC(get_wheel_steer_angle) << [this](
1261  const cr::ActorId ActorId,
1262  cr::VehicleWheelLocation WheelLocation) -> R<float>
1263  {
1265  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1266  if(!CarlaActor){
1267  return RespondError(
1268  "get_wheel_steer_angle",
1270  " Actor Id: " + FString::FromInt(ActorId));
1271  }
1272  float Angle;
1273  ECarlaServerResponse Response =
1274  CarlaActor->GetWheelSteerAngle(
1275  static_cast<EVehicleWheelLocation>(WheelLocation), Angle);
1276  if (Response != ECarlaServerResponse::Success)
1277  {
1278  return RespondError(
1279  "get_wheel_steer_angle",
1280  Response,
1281  " Actor Id: " + FString::FromInt(ActorId));
1282  }
1283  return Angle;
1284  };
1285 
1286  BIND_SYNC(set_actor_simulate_physics) << [this](
1288  bool bEnabled) -> R<void>
1289  {
1291  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1292  if (!CarlaActor)
1293  {
1294  return RespondError(
1295  "set_actor_simulate_physics",
1297  " Actor Id: " + FString::FromInt(ActorId));
1298  }
1299  ECarlaServerResponse Response =
1300  CarlaActor->SetActorSimulatePhysics(bEnabled);
1301  if (Response != ECarlaServerResponse::Success)
1302  {
1303  return RespondError(
1304  "set_actor_simulate_physics",
1305  Response,
1306  " Actor Id: " + FString::FromInt(ActorId));
1307  }
1308  return R<void>::Success();
1309  };
1310 
1311  BIND_SYNC(set_actor_enable_gravity) << [this](
1313  bool bEnabled) -> R<void>
1314  {
1316  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1317  if (!CarlaActor)
1318  {
1319  return RespondError(
1320  "set_actor_enable_gravity",
1322  " Actor Id: " + FString::FromInt(ActorId));
1323  }
1324  ECarlaServerResponse Response =
1325  CarlaActor->SetActorEnableGravity(bEnabled);
1326  if (Response != ECarlaServerResponse::Success)
1327  {
1328  return RespondError(
1329  "set_actor_enable_gravity",
1330  Response,
1331  " Actor Id: " + FString::FromInt(ActorId));
1332  }
1333  return R<void>::Success();
1334  };
1335 
1336  // ~~ Apply control ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1337 
1338  BIND_SYNC(apply_control_to_vehicle) << [this](
1340  cr::VehicleControl Control) -> R<void>
1341  {
1343  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1344  if (!CarlaActor)
1345  {
1346  return RespondError(
1347  "apply_control_to_vehicle",
1349  " Actor Id: " + FString::FromInt(ActorId));
1350  }
1351  ECarlaServerResponse Response =
1352  CarlaActor->ApplyControlToVehicle(Control, EVehicleInputPriority::Client);
1353  if (Response != ECarlaServerResponse::Success)
1354  {
1355  return RespondError(
1356  "apply_control_to_vehicle",
1357  Response,
1358  " Actor Id: " + FString::FromInt(ActorId));
1359  }
1360  return R<void>::Success();
1361  };
1362 
1363  BIND_SYNC(apply_control_to_walker) << [this](
1365  cr::WalkerControl Control) -> R<void>
1366  {
1368  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1369  if (!CarlaActor)
1370  {
1371  return RespondError(
1372  "apply_control_to_walker",
1374  " Actor Id: " + FString::FromInt(ActorId));
1375  }
1376  ECarlaServerResponse Response =
1377  CarlaActor->ApplyControlToWalker(Control);
1378  if (Response != ECarlaServerResponse::Success)
1379  {
1380  return RespondError(
1381  "apply_control_to_walker",
1382  Response,
1383  " Actor Id: " + FString::FromInt(ActorId));
1384  }
1385  return R<void>::Success();
1386  };
1387 
1388  BIND_SYNC(get_bones_transform) << [this](
1390  {
1392  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1393  if (!CarlaActor)
1394  {
1395  return RespondError(
1396  "get_bones_transform",
1398  " Actor Id: " + FString::FromInt(ActorId));
1399  }
1400  FWalkerBoneControlOut Bones;
1401  ECarlaServerResponse Response =
1402  CarlaActor->GetBonesTransform(Bones);
1403  if (Response != ECarlaServerResponse::Success)
1404  {
1405  return RespondError(
1406  "get_bones_transform",
1407  Response,
1408  " Actor Id: " + FString::FromInt(ActorId));
1409  }
1410 
1411  std::vector<carla::rpc::BoneTransformDataOut> BoneData;
1412  for (auto Bone : Bones.BoneTransforms)
1413  {
1415  Data.bone_name = std::string(TCHAR_TO_UTF8(*Bone.Get<0>()));
1416  FWalkerBoneControlOutData Transforms = Bone.Get<1>();
1417  Data.world = Transforms.World;
1418  Data.component = Transforms.Component;
1419  Data.relative = Transforms.Relative;
1420  BoneData.push_back(Data);
1421  }
1422  return carla::rpc::WalkerBoneControlOut(BoneData);
1423  };
1424 
1425  BIND_SYNC(set_bones_transform) << [this](
1428  {
1430  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1431  if (!CarlaActor)
1432  {
1433  return RespondError(
1434  "set_bones_transform",
1436  " Actor Id: " + FString::FromInt(ActorId));
1437  }
1438 
1440  ECarlaServerResponse Response = CarlaActor->SetBonesTransform(Bones2);
1441  if (Response != ECarlaServerResponse::Success)
1442  {
1443  return RespondError(
1444  "set_bones_transform",
1445  Response,
1446  " Actor Id: " + FString::FromInt(ActorId));
1447  }
1448 
1449  return R<void>::Success();
1450  };
1451 
1452  BIND_SYNC(blend_pose) << [this](
1454  float Blend) -> R<void>
1455  {
1457  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1458  if (!CarlaActor)
1459  {
1460  return RespondError(
1461  "blend_pose",
1463  " Actor Id: " + FString::FromInt(ActorId));
1464  }
1465 
1466  ECarlaServerResponse Response = CarlaActor->BlendPose(Blend);
1467  if (Response != ECarlaServerResponse::Success)
1468  {
1469  return RespondError(
1470  "blend_pose",
1471  Response,
1472  " Actor Id: " + FString::FromInt(ActorId));
1473  }
1474 
1475  return R<void>::Success();
1476  };
1477 
1478  BIND_SYNC(get_pose_from_animation) << [this](
1480  {
1482  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1483  if (!CarlaActor)
1484  {
1485  return RespondError(
1486  "get_pose_from_animation",
1488  " Actor Id: " + FString::FromInt(ActorId));
1489  }
1490 
1491  ECarlaServerResponse Response = CarlaActor->GetPoseFromAnimation();
1492  if (Response != ECarlaServerResponse::Success)
1493  {
1494  return RespondError(
1495  "get_pose_from_animation",
1496  Response,
1497  " Actor Id: " + FString::FromInt(ActorId));
1498  }
1499 
1500  return R<void>::Success();
1501  };
1502 
1503  BIND_SYNC(set_actor_autopilot) << [this](
1505  bool bEnabled) -> R<void>
1506  {
1508  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1509  if (!CarlaActor)
1510  {
1511  return RespondError(
1512  "set_actor_autopilot",
1514  " Actor Id: " + FString::FromInt(ActorId));
1515  }
1516  ECarlaServerResponse Response =
1517  CarlaActor->SetActorAutopilot(bEnabled);
1518  if (Response != ECarlaServerResponse::Success)
1519  {
1520  return RespondError(
1521  "set_actor_autopilot",
1522  Response,
1523  " Actor Id: " + FString::FromInt(ActorId));
1524  }
1525  return R<void>::Success();
1526  };
1527 
1528  BIND_SYNC(show_vehicle_debug_telemetry) << [this](
1530  bool bEnabled) -> R<void>
1531  {
1533  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1534  if (!CarlaActor)
1535  {
1536  return RespondError(
1537  "show_vehicle_debug_telemetry",
1539  " Actor Id: " + FString::FromInt(ActorId));
1540  }
1541  ECarlaServerResponse Response =
1542  CarlaActor->ShowVehicleDebugTelemetry(bEnabled);
1543  if (Response != ECarlaServerResponse::Success)
1544  {
1545  return RespondError(
1546  "show_vehicle_debug_telemetry",
1547  Response,
1548  " Actor Id: " + FString::FromInt(ActorId));
1549  }
1550  return R<void>::Success();
1551  };
1552 
1553  BIND_SYNC(enable_carsim) << [this](
1555  std::string SimfilePath) -> R<void>
1556  {
1558  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1559  if (!CarlaActor)
1560  {
1561  return RespondError(
1562  "enable_carsim",
1564  " Actor Id: " + FString::FromInt(ActorId));
1565  }
1566  ECarlaServerResponse Response =
1567  CarlaActor->EnableCarSim(carla::rpc::ToFString(SimfilePath));
1568  if (Response != ECarlaServerResponse::Success)
1569  {
1570  return RespondError(
1571  "enable_carsim",
1572  Response,
1573  " Actor Id: " + FString::FromInt(ActorId));
1574  }
1575  return R<void>::Success();
1576  };
1577 
1578  BIND_SYNC(use_carsim_road) << [this](
1580  bool bEnabled) -> R<void>
1581  {
1583  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1584  if (!CarlaActor)
1585  {
1586  return RespondError(
1587  "use_carsim_road",
1589  " Actor Id: " + FString::FromInt(ActorId));
1590  }
1591  ECarlaServerResponse Response =
1592  CarlaActor->UseCarSimRoad(bEnabled);
1593  if (Response != ECarlaServerResponse::Success)
1594  {
1595  return RespondError(
1596  "use_carsim_road",
1597  Response,
1598  " Actor Id: " + FString::FromInt(ActorId));
1599  }
1600  return R<void>::Success();
1601  };
1602 
1603  BIND_SYNC(enable_chrono_physics) << [this](
1605  uint64_t MaxSubsteps,
1606  float MaxSubstepDeltaTime,
1607  std::string VehicleJSON,
1608  std::string PowertrainJSON,
1609  std::string TireJSON,
1610  std::string BaseJSONPath) -> R<void>
1611  {
1613  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1614  if (!CarlaActor)
1615  {
1616  return RespondError(
1617  "enable_chrono_physics",
1619  " Actor Id: " + FString::FromInt(ActorId));
1620  }
1621  ECarlaServerResponse Response =
1622  CarlaActor->EnableChronoPhysics(
1623  MaxSubsteps, MaxSubstepDeltaTime,
1624  cr::ToFString(VehicleJSON),
1625  cr::ToFString(PowertrainJSON),
1626  cr::ToFString(TireJSON),
1627  cr::ToFString(BaseJSONPath));
1628  if (Response != ECarlaServerResponse::Success)
1629  {
1630  return RespondError(
1631  "enable_chrono_physics",
1632  Response,
1633  " Actor Id: " + FString::FromInt(ActorId));
1634  }
1635  return R<void>::Success();
1636  };
1637 
1638  // ~~ Traffic lights ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1639 
1640  BIND_SYNC(set_traffic_light_state) << [this](
1642  cr::TrafficLightState trafficLightState) -> R<void>
1643  {
1645  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1646  if (!CarlaActor)
1647  {
1648  return RespondError(
1649  "set_traffic_light_state",
1651  " Actor Id: " + FString::FromInt(ActorId));
1652  }
1653  ECarlaServerResponse Response =
1654  CarlaActor->SetTrafficLightState(
1655  static_cast<ETrafficLightState>(trafficLightState));
1656  if (Response != ECarlaServerResponse::Success)
1657  {
1658  return RespondError(
1659  "set_traffic_light_state",
1660  Response,
1661  " Actor Id: " + FString::FromInt(ActorId));
1662  }
1663  return R<void>::Success();
1664  };
1665 
1666  BIND_SYNC(set_traffic_light_green_time) << [this](
1668  float GreenTime) -> R<void>
1669  {
1671  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1672  if (!CarlaActor)
1673  {
1674  return RespondError(
1675  "set_traffic_light_green_time",
1677  " Actor Id: " + FString::FromInt(ActorId));
1678  }
1679  ECarlaServerResponse Response =
1680  CarlaActor->SetLightGreenTime(GreenTime);
1681  if (Response != ECarlaServerResponse::Success)
1682  {
1683  return RespondError(
1684  "set_traffic_light_green_time",
1685  Response,
1686  " Actor Id: " + FString::FromInt(ActorId));
1687  }
1688  return R<void>::Success();
1689  };
1690 
1691  BIND_SYNC(set_traffic_light_yellow_time) << [this](
1693  float YellowTime) -> R<void>
1694  {
1696  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1697  if (!CarlaActor)
1698  {
1699  return RespondError(
1700  "set_traffic_light_yellow_time",
1702  " Actor Id: " + FString::FromInt(ActorId));
1703  }
1704  ECarlaServerResponse Response =
1705  CarlaActor->SetLightYellowTime(YellowTime);
1706  if (Response != ECarlaServerResponse::Success)
1707  {
1708  return RespondError(
1709  "set_traffic_light_yellow_time",
1710  Response,
1711  " Actor Id: " + FString::FromInt(ActorId));
1712  }
1713  return R<void>::Success();
1714  };
1715 
1716  BIND_SYNC(set_traffic_light_red_time) << [this](
1718  float RedTime) -> R<void>
1719  {
1721  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1722  if (!CarlaActor)
1723  {
1724  return RespondError(
1725  "set_traffic_light_red_time",
1727  " Actor Id: " + FString::FromInt(ActorId));
1728  }
1729  ECarlaServerResponse Response =
1730  CarlaActor->SetLightRedTime(RedTime);
1731  if (Response != ECarlaServerResponse::Success)
1732  {
1733  return RespondError(
1734  "set_traffic_light_red_time",
1735  Response,
1736  " Actor Id: " + FString::FromInt(ActorId));
1737  }
1738  return R<void>::Success();
1739  };
1740 
1741  BIND_SYNC(freeze_traffic_light) << [this](
1743  bool Freeze) -> R<void>
1744  {
1746  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1747  if (!CarlaActor)
1748  {
1749  return RespondError(
1750  "freeze_traffic_light",
1752  " Actor Id: " + FString::FromInt(ActorId));
1753  }
1754  ECarlaServerResponse Response =
1755  CarlaActor->FreezeTrafficLight(Freeze);
1756  if (Response != ECarlaServerResponse::Success)
1757  {
1758  return RespondError(
1759  "freeze_traffic_light",
1760  Response,
1761  " Actor Id: " + FString::FromInt(ActorId));
1762  }
1763  return R<void>::Success();
1764  };
1765 
1766  BIND_SYNC(reset_traffic_light_group) << [this](
1768  {
1770  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1771  if (!CarlaActor)
1772  {
1773  return RespondError(
1774  "reset_traffic_light_group",
1776  " Actor Id: " + FString::FromInt(ActorId));
1777  }
1778  ECarlaServerResponse Response =
1779  CarlaActor->ResetTrafficLightGroup();
1780  if (Response != ECarlaServerResponse::Success)
1781  {
1782  return RespondError(
1783  "reset_traffic_light_group",
1784  Response,
1785  " Actor Id: " + FString::FromInt(ActorId));
1786  }
1787  return R<void>::Success();
1788  };
1789 
1790  BIND_SYNC(reset_all_traffic_lights) << [this]() -> R<void>
1791  {
1793  for (TActorIterator<ATrafficLightGroup> It(Episode->GetWorld()); It; ++It)
1794  {
1795  It->ResetGroup();
1796  }
1797  return R<void>::Success();
1798  };
1799 
1800  BIND_SYNC(freeze_all_traffic_lights) << [this]
1801  (bool frozen) -> R<void>
1802  {
1804  auto* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
1805  if (!GameMode)
1806  {
1807  RESPOND_ERROR("unable to find CARLA game mode");
1808  }
1809  auto* TraffiLightManager = GameMode->GetTrafficLightManager();
1810  TraffiLightManager->SetFrozen(frozen);
1811  return R<void>::Success();
1812  };
1813 
1814  BIND_SYNC(get_vehicle_light_states) << [this]() -> R<cr::VehicleLightStateList>
1815  {
1818 
1819  auto It = Episode->GetActorRegistry().begin();
1820  for (; It != Episode->GetActorRegistry().end(); ++It)
1821  {
1822  const FCarlaActor& View = *(It.Value().Get());
1824  {
1825  if(View.IsDormant())
1826  {
1827  // todo: implement
1828  }
1829  else
1830  {
1831  auto Actor = View.GetActor();
1832  if (!Actor->IsPendingKill())
1833  {
1834  const ACarlaWheeledVehicle *Vehicle = Cast<ACarlaWheeledVehicle>(Actor);
1835  List.emplace_back(
1836  View.GetActorId(),
1837  cr::VehicleLightState(Vehicle->GetVehicleLightState()).GetLightStateAsValue());
1838  }
1839  }
1840  }
1841  }
1842  return List;
1843  };
1844 
1845  BIND_SYNC(get_group_traffic_lights) << [this](
1846  const cr::ActorId ActorId) -> R<std::vector<cr::ActorId>>
1847  {
1849  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1850  if (!CarlaActor)
1851  {
1852  RESPOND_ERROR("unable to get group traffic lights: actor not found");
1853  }
1854  if (CarlaActor->IsDormant())
1855  {
1856  //todo implement
1857  return std::vector<cr::ActorId>();
1858  }
1859  else
1860  {
1861  auto TrafficLight = Cast<ATrafficLightBase>(CarlaActor->GetActor());
1862  if (TrafficLight == nullptr)
1863  {
1864  RESPOND_ERROR("unable to get group traffic lights: actor is not a traffic light");
1865  }
1866  std::vector<cr::ActorId> Result;
1867  for (auto* TLight : TrafficLight->GetGroupTrafficLights())
1868  {
1869  auto* View = Episode->FindCarlaActor(TLight);
1870  if (View)
1871  {
1872  Result.push_back(View->GetActorId());
1873  }
1874  }
1875  return Result;
1876  }
1877  };
1878 
1879  BIND_SYNC(get_light_boxes) << [this](
1880  const cr::ActorId ActorId) -> R<std::vector<cg::BoundingBox>>
1881  {
1883  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1884  if (CarlaActor)
1885  {
1886  return RespondError(
1887  "get_light_boxes",
1889  " Actor Id: " + FString::FromInt(ActorId));
1890  }
1891  if (CarlaActor->IsDormant())
1892  {
1893  return RespondError(
1894  "get_light_boxes",
1896  " Actor Id: " + FString::FromInt(ActorId));
1897  }
1898  else
1899  {
1900  ATrafficLightBase* TrafficLight = Cast<ATrafficLightBase>(CarlaActor->GetActor());
1901  if (!TrafficLight)
1902  {
1903  return RespondError(
1904  "get_light_boxes",
1906  " Actor Id: " + FString::FromInt(ActorId));
1907  }
1908  TArray<FBoundingBox> Result;
1909  TArray<uint8> OutTag;
1911  TrafficLight, Result, OutTag,
1912  static_cast<uint8>(carla::rpc::CityObjectLabel::TrafficLight));
1913  return MakeVectorFromTArray<cg::BoundingBox>(Result);
1914  }
1915  };
1916 
1917  // ~~ Logging and playback ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1918 
1919  BIND_SYNC(start_recorder) << [this](std::string name, bool AdditionalData) -> R<std::string>
1920  {
1922  return R<std::string>(Episode->StartRecorder(name, AdditionalData));
1923  };
1924 
1925  BIND_SYNC(stop_recorder) << [this]() -> R<void>
1926  {
1928  Episode->GetRecorder()->Stop();
1929  return R<void>::Success();
1930  };
1931 
1932  BIND_SYNC(show_recorder_file_info) << [this](
1933  std::string name,
1934  bool show_all) -> R<std::string>
1935  {
1938  name,
1939  show_all));
1940  };
1941 
1942  BIND_SYNC(show_recorder_collisions) << [this](
1943  std::string name,
1944  char type1,
1945  char type2) -> R<std::string>
1946  {
1949  name,
1950  type1,
1951  type2));
1952  };
1953 
1954  BIND_SYNC(show_recorder_actors_blocked) << [this](
1955  std::string name,
1956  double min_time,
1957  double min_distance) -> R<std::string>
1958  {
1961  name,
1962  min_time,
1963  min_distance));
1964  };
1965 
1966  BIND_SYNC(replay_file) << [this](
1967  std::string name,
1968  double start,
1969  double duration,
1970  uint32_t follow_id,
1971  bool replay_sensors) -> R<std::string>
1972  {
1975  name,
1976  start,
1977  duration,
1978  follow_id,
1979  replay_sensors));
1980  };
1981 
1982  BIND_SYNC(set_replayer_time_factor) << [this](double time_factor) -> R<void>
1983  {
1985  Episode->GetRecorder()->SetReplayerTimeFactor(time_factor);
1986  return R<void>::Success();
1987  };
1988 
1989  BIND_SYNC(set_replayer_ignore_hero) << [this](bool ignore_hero) -> R<void>
1990  {
1992  Episode->GetRecorder()->SetReplayerIgnoreHero(ignore_hero);
1993  return R<void>::Success();
1994  };
1995 
1996  BIND_SYNC(stop_replayer) << [this](bool keep_actors) -> R<void>
1997  {
1999  Episode->GetRecorder()->StopReplayer(keep_actors);
2000  return R<void>::Success();
2001  };
2002 
2003  // ~~ Draw debug shapes ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2004 
2005  BIND_SYNC(draw_debug_shape) << [this](const cr::DebugShape &shape) -> R<void>
2006  {
2008  auto *World = Episode->GetWorld();
2009  check(World != nullptr);
2010  FDebugShapeDrawer Drawer(*World);
2011  Drawer.Draw(shape);
2012  return R<void>::Success();
2013  };
2014 
2015  // ~~ Apply commands in batch ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2016 
2017  using C = cr::Command;
2018  using CR = cr::CommandResponse;
2019  using ActorId = carla::ActorId;
2020 
2021  auto parse_result = [](ActorId id, const auto &response) {
2022  return response.HasError() ? CR{response.GetError()} : CR{id};
2023  };
2024 
2025 #define MAKE_RESULT(operation) return parse_result(c.actor, operation);
2026 
2027  auto command_visitor = carla::Functional::MakeRecursiveOverload(
2028  [=](auto self, const C::SpawnActor &c) -> CR {
2029  auto result = c.parent.has_value() ?
2030  spawn_actor_with_parent(
2031  c.description,
2032  c.transform,
2033  *c.parent,
2034  cr::AttachmentType::Rigid) :
2035  spawn_actor(c.description, c.transform);
2036  if (!result.HasError())
2037  {
2038  ActorId id = result.Get().id;
2039  auto set_id = carla::Functional::MakeOverload(
2040  [](C::SpawnActor &) {},
2041  [id](auto &s) { s.actor = id; });
2042  for (auto command : c.do_after)
2043  {
2044  boost::apply_visitor(set_id, command.command);
2045  boost::apply_visitor(self, command.command);
2046  }
2047  return id;
2048  }
2049  return result.GetError();
2050  },
2051  [=](auto, const C::DestroyActor &c) { MAKE_RESULT(destroy_actor(c.actor)); },
2052  [=](auto, const C::ApplyVehicleControl &c) { MAKE_RESULT(apply_control_to_vehicle(c.actor, c.control)); },
2053  [=](auto, const C::ApplyWalkerControl &c) { MAKE_RESULT(apply_control_to_walker(c.actor, c.control)); },
2054  [=](auto, const C::ApplyVehiclePhysicsControl &c) { MAKE_RESULT(apply_physics_control(c.actor, c.physics_control)); },
2055  [=](auto, const C::ApplyTransform &c) { MAKE_RESULT(set_actor_transform(c.actor, c.transform)); },
2056  [=](auto, const C::ApplyTargetVelocity &c) { MAKE_RESULT(set_actor_target_velocity(c.actor, c.velocity)); },
2057  [=](auto, const C::ApplyTargetAngularVelocity &c) { MAKE_RESULT(set_actor_target_angular_velocity(c.actor, c.angular_velocity)); },
2058  [=](auto, const C::ApplyImpulse &c) { MAKE_RESULT(add_actor_impulse(c.actor, c.impulse)); },
2059  [=](auto, const C::ApplyForce &c) { MAKE_RESULT(add_actor_force(c.actor, c.force)); },
2060  [=](auto, const C::ApplyAngularImpulse &c) { MAKE_RESULT(add_actor_angular_impulse(c.actor, c.impulse)); },
2061  [=](auto, const C::ApplyTorque &c) { MAKE_RESULT(add_actor_torque(c.actor, c.torque)); },
2062  [=](auto, const C::SetSimulatePhysics &c) { MAKE_RESULT(set_actor_simulate_physics(c.actor, c.enabled)); },
2063  [=](auto, const C::SetEnableGravity &c) { MAKE_RESULT(set_actor_enable_gravity(c.actor, c.enabled)); },
2064  // TODO: SetAutopilot should be removed. This is the old way to control the vehicles
2065  [=](auto, const C::SetAutopilot &c) { MAKE_RESULT(set_actor_autopilot(c.actor, c.enabled)); },
2066  [=](auto, const C::ShowDebugTelemetry &c) { MAKE_RESULT(show_vehicle_debug_telemetry(c.actor, c.enabled)); },
2067  [=](auto, const C::SetVehicleLightState &c) { MAKE_RESULT(set_vehicle_light_state(c.actor, c.light_state)); },
2068 // [=](auto, const C::OpenVehicleDoor &c) { MAKE_RESULT(open_vehicle_door(c.actor, c.door_idx)); },
2069 // [=](auto, const C::CloseVehicleDoor &c) { MAKE_RESULT(close_vehicle_door(c.actor, c.door_idx)); },
2070  [=](auto, const C::ApplyWalkerState &c) { MAKE_RESULT(set_walker_state(c.actor, c.transform, c.speed)); });
2071 
2072 #undef MAKE_RESULT
2073 
2074  BIND_SYNC(apply_batch) << [=](
2075  const std::vector<cr::Command> &commands,
2076  bool do_tick_cue)
2077  {
2078  std::vector<CR> result;
2079  result.reserve(commands.size());
2080  for (const auto &command : commands)
2081  {
2082  result.emplace_back(boost::apply_visitor(command_visitor, command.command));
2083  }
2084  if (do_tick_cue)
2085  {
2086  tick_cue();
2087  }
2088  return result;
2089  };
2090 
2091  // ~~ Light Subsystem ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2092 
2093  BIND_SYNC(query_lights_state) << [this](std::string client) -> R<std::vector<cr::LightState>>
2094  {
2096  std::vector<cr::LightState> result;
2097  auto *World = Episode->GetWorld();
2098  if(World) {
2099  UCarlaLightSubsystem* CarlaLightSubsystem = World->GetSubsystem<UCarlaLightSubsystem>();
2100  result = CarlaLightSubsystem->GetLights(FString(client.c_str()));
2101  }
2102  return result;
2103  };
2104 
2105  BIND_SYNC(update_lights_state) << [this]
2106  (std::string client, const std::vector<cr::LightState>& lights, bool discard_client) -> R<void>
2107  {
2109  auto *World = Episode->GetWorld();
2110  if(World) {
2111  UCarlaLightSubsystem* CarlaLightSubsystem = World->GetSubsystem<UCarlaLightSubsystem>();
2112  CarlaLightSubsystem->SetLights(FString(client.c_str()), lights, discard_client);
2113  }
2114  return R<void>::Success();
2115  };
2116 
2117 
2118  // ~~ Ray Casting ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2119 
2120  BIND_SYNC(project_point) << [this]
2121  (cr::Location Location, cr::Vector3D Direction, float SearchDistance)
2122  -> R<std::pair<bool,cr::LabelledPoint>>
2123  {
2125  auto *World = Episode->GetWorld();
2126  constexpr float meter_to_centimeter = 100.0f;
2127  FVector UELocation = Location;
2128  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
2129  ALargeMapManager* LargeMap = GameMode->GetLMManager();
2130  if (LargeMap)
2131  {
2132  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
2133  }
2134  return URayTracer::ProjectPoint(UELocation, Direction.ToFVector(),
2135  meter_to_centimeter * SearchDistance, World);
2136  };
2137 
2138  BIND_SYNC(cast_ray) << [this]
2139  (cr::Location StartLocation, cr::Location EndLocation)
2140  -> R<std::vector<cr::LabelledPoint>>
2141  {
2143  auto *World = Episode->GetWorld();
2144  FVector UEStartLocation = StartLocation;
2145  FVector UEEndLocation = EndLocation;
2146  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
2147  ALargeMapManager* LargeMap = GameMode->GetLMManager();
2148  if (LargeMap)
2149  {
2150  UEStartLocation = LargeMap->GlobalToLocalLocation(UEStartLocation);
2151  UEEndLocation = LargeMap->GlobalToLocalLocation(UEEndLocation);
2152  }
2153  return URayTracer::CastRay(StartLocation, EndLocation, World);
2154  };
2155 
2156 }
2157 
2158 // =============================================================================
2159 // -- Undef helper macros ------------------------------------------------------
2160 // =============================================================================
2161 
2162 #undef BIND_ASYNC
2163 #undef BIND_SYNC
2164 #undef REQUIRE_CARLA_EPISODE
2165 #undef RESPOND_ERROR_FSTRING
2166 #undef RESPOND_ERROR
2167 #undef CARLA_ENSURE_GAME_THREAD
2168 
2169 // =============================================================================
2170 // -- FCarlaServer -------------------------------------------------------
2171 // =============================================================================
2172 
2174 
2176 
2177 FDataMultiStream FCarlaServer::Start(uint16_t RPCPort, uint16_t StreamingPort)
2178 {
2179  Pimpl = MakeUnique<FPimpl>(RPCPort, StreamingPort);
2180  StreamingPort = Pimpl->StreamingServer.GetLocalEndpoint().port();
2181  UE_LOG(
2182  LogCarlaServer,
2183  Log,
2184  TEXT("Initialized CarlaServer: Ports(rpc=%d, streaming=%d)"),
2185  RPCPort,
2186  StreamingPort);
2187  return Pimpl->BroadcastStream;
2188 }
2189 
2191 {
2192  check(Pimpl != nullptr);
2193  UE_LOG(LogCarlaServer, Log, TEXT("New episode '%s' started"), *Episode.GetMapName());
2194  Pimpl->Episode = &Episode;
2195 }
2196 
2198 {
2199  check(Pimpl != nullptr);
2200  Pimpl->Episode = nullptr;
2201 }
2202 
2203 void FCarlaServer::AsyncRun(uint32 NumberOfWorkerThreads)
2204 {
2205  check(Pimpl != nullptr);
2206  /// @todo Define better the number of threads each server gets.
2207  int32_t RPCThreads = std::max(2u, NumberOfWorkerThreads / 2u);
2208  int32_t StreamingThreads = std::max(2u, NumberOfWorkerThreads - RPCThreads);
2209 
2210  UE_LOG(LogCarla, Log, TEXT("FCommandLine %s"), FCommandLine::Get());
2211 
2212  if(!FParse::Value(FCommandLine::Get(), TEXT("-RPCThreads="), RPCThreads))
2213  {
2214  RPCThreads = std::max(2u, NumberOfWorkerThreads / 2u);
2215  }
2216  if(!FParse::Value(FCommandLine::Get(), TEXT("-StreamingThreads="), StreamingThreads))
2217  {
2218  StreamingThreads = std::max(2u, NumberOfWorkerThreads - RPCThreads);
2219  }
2220 
2221  UE_LOG(LogCarla, Log, TEXT("FCarlaServer AsyncRun %d, RPCThreads %d, StreamingThreads %d"),
2222  NumberOfWorkerThreads, RPCThreads, StreamingThreads);
2223 
2224  Pimpl->Server.AsyncRun(RPCThreads);
2225  Pimpl->StreamingServer.AsyncRun(StreamingThreads);
2226 
2227 }
2228 
2229 void FCarlaServer::RunSome(uint32 Milliseconds)
2230 {
2231  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
2232  Pimpl->Server.SyncRunFor(carla::time_duration::milliseconds(Milliseconds));
2233 }
2234 
2236 {
2237  if (Pimpl->TickCuesReceived > 0u)
2238  {
2239  --(Pimpl->TickCuesReceived);
2240  return true;
2241  }
2242  return false;
2243 }
2244 
2246 {
2247  check(Pimpl != nullptr);
2248  Pimpl->Server.Stop();
2249 }
2250 
2252 {
2253  check(Pimpl != nullptr);
2254  return Pimpl->StreamingServer.MakeStream();
2255 }
carla::streaming::Stream BroadcastStream
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
Definition: CarlaEpisode.h:152
virtual ECarlaServerResponse SetVehicleLightState(const FVehicleLightState &)
Definition: CarlaActor.h:266
TArray< FEnvironmentObject > GetEnvironmentObjects(uint8 QueriedTag=0xFF) const
void ApplyTextureToActor(AActor *Actor, UTexture2D *Texture, const carla::rpc::MaterialParameter &TextureParam)
FDataStream OpenStream() const
auto end() const noexcept
std::vector< carla::rpc::LightState > GetLights(FString Client)
static FString StatusToString(EActorSpawnResultStatus Status)
bool LoadNewOpendriveEpisode(const FString &OpenDriveString, const carla::rpc::OpendriveGenerationParameters &Params)
Load a new map generating the mesh from OpenDRIVE data and start a new episode.
void StopReplayer(bool KeepActors=false)
bool DestroyActor(AActor *Actor)
Definition: CarlaEpisode.h:223
const char * _name
virtual ECarlaServerResponse SetWheelSteerDirection(const EVehicleWheelLocation &, float)
Definition: CarlaActor.h:271
AWeather * GetWeather() const
Definition: CarlaEpisode.h:129
std::string ShowFileCollisions(std::string Name, char Type1, char Type2)
void SetMapLayer(int32 MapLayer)
auto operator<<(FuncT func)
virtual ECarlaServerResponse SetActorEnableGravity(bool bEnabled)
Definition: CarlaActor.cpp:579
AActor * GetActor()
Definition: CarlaActor.h:90
FVector GlobalToLocalLocation(const FVector &InLocation) const
FString GetStringError(ECarlaServerResponse Response)
carla::rpc::ResponseError RespondError(const FString &FuncName, const FString &ErrorMessage, const FString &ExtraInfo="")
EAttachmentType
Definition: ActorAttacher.h:20
virtual ECarlaServerResponse SetBonesTransform(const FWalkerBoneControlIn &)
Definition: CarlaActor.h:375
The game instance contains elements that must be kept alive in between levels.
static TArray< uint8 > Load(FString MapName)
Return the Navigation Mesh Binary associated to MapName, or empty if the such file wasn&#39;t serialized...
#define MAKE_RESULT(operation)
rpc::ActorId ActorId
Definition: ActorId.h:18
static auto MakeRecursiveOverload(FuncTs &&... fs)
Definition: Functional.h:33
auto begin() const noexcept
carla::rpc::Actor SerializeActor(FCarlaActor *CarlaActor) const
Create a serializable object describing the actor.
void SetReplayerTimeFactor(double TimeFactor)
virtual ECarlaServerResponse DisableActorConstantVelocity()
Definition: CarlaActor.h:236
std::string ShowFileActorsBlocked(std::string Name, double MinTime=30, double MinDistance=10)
A streaming server.
std::vector< std::pair< ActorId, VehicleLightState::flag_type > > VehicleLightStateList
TArray< FTransform > GetRecommendedSpawnPoints() const
Return the list of recommended spawn points for vehicles.
FPimpl(uint16_t RPCPort, uint16_t StreamingPort)
Definition: CarlaServer.cpp:92
#define RESPOND_ERROR_FSTRING(fstr)
std::map< uint16_t, std::string > TrafficManagerInfo
Map of pairs < port , ip > with all the Traffic Managers active in the simulation.
void EnableEnvironmentObjects(const TSet< uint64 > &EnvObjectIds, bool Enable)
static TArray< FString > GetAllMapNames()
virtual ECarlaServerResponse BlendPose(float Blend)
Definition: CarlaActor.h:380
FDataMultiStream Start(uint16_t RPCPort, uint16_t StreamingPort)
void PutActorToSleep(carla::rpc::ActorId ActorId)
Definition: CarlaEpisode.h:246
virtual ECarlaServerResponse GetPhysicsControl(FVehiclePhysicsControl &)
Definition: CarlaActor.h:241
void UnLoadMapLayer(int32 MapLayers)
void OnActorSpawned(const FCarlaActor &CarlaActor)
virtual ECarlaServerResponse ApplyControlToVehicle(const FVehicleControl &, const EVehicleInputPriority &)
Definition: CarlaActor.h:281
AActor * FindActorByName(const FString &ActorName)
UTexture2D * CreateUETexture(const carla::rpc::TextureColor &Texture)
static T Get(carla::rpc::Response< T > &response)
bg::model::box< Point3D > Box
Definition: InMemoryMap.h:45
virtual ECarlaServerResponse SetTrafficLightState(const ETrafficLightState &)
Definition: CarlaActor.h:320
void NotifyEndEpisode()
static ACarlaGameModeBase * GetGameMode(const UObject *WorldContextObject)
Definition: CarlaStatics.h:58
static void GetTrafficLightBoundingBox(const ATrafficLightBase *TrafficLight, TArray< FBoundingBox > &OutBB, TArray< uint8 > &OutTag, uint8 InTagQueried=0xFF)
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
const FString & GetMapName() const
Return the name of the map loaded in this episode.
Definition: CarlaEpisode.h:89
static uint64_t GetFrameCounter()
Definition: CarlaEngine.h:49
#define BIND_ASYNC(name)
virtual ECarlaServerResponse EnableChronoPhysics(uint64_t, float, const FString &, const FString &, const FString &, const FString &)
Definition: CarlaActor.h:312
geom::Location Location
Definition: rpc/Location.h:14
void SetActorGlobalLocation(const FVector &Location, ETeleportType Teleport=ETeleportType::TeleportPhysics)
Definition: CarlaActor.cpp:279
void Draw(const carla::rpc::DebugShape &Shape)
ECarlaServerResponse AddActorImpulse(const FVector &Impulse)
Definition: CarlaActor.cpp:429
#define BIND_SYNC(name)
ECarlaServerResponse SetActorTargetAngularVelocity(const FVector &AngularVelocity)
Definition: CarlaActor.cpp:408
void LoadMapLayer(int32 MapLayers)
void RunSome(uint32 Milliseconds)
const FActorRegistry & GetActorRegistry() const
Definition: CarlaEpisode.h:134
virtual ECarlaServerResponse CloseVehicleDoor(const EVehicleDoor)
Definition: CarlaActor.h:256
std::string StartRecorder(std::string name, bool AdditionalData)
ECarlaServerResponse AddActorForceAtLocation(const FVector &Force, const FVector &Location)
Definition: CarlaActor.cpp:493
ECarlaServerResponse AddActorTorque(const FVector &Torque)
Definition: CarlaActor.cpp:537
static UCarlaGameInstance * GetGameInstance(const UObject *WorldContextObject)
Definition: CarlaStatics.h:63
ECarlaServerResponse
carla::SharedPtr< cc::Actor > Actor
virtual ECarlaServerResponse FreezeTrafficLight(bool)
Definition: CarlaActor.h:390
virtual ECarlaServerResponse EnableActorConstantVelocity(const FVector &)
Definition: CarlaActor.h:231
A simulation episode.
Definition: CarlaEpisode.h:34
An RPC server in which functions can be bind to run synchronously or asynchronously.
Definition: rpc/Server.h:37
virtual ECarlaServerResponse GetPoseFromAnimation()
Definition: CarlaActor.h:385
std::string ReplayFile(std::string Name, double TimeStart, double Duration, uint32_t FollowId, bool ReplaySensors)
TPair< EActorSpawnResultStatus, FCarlaActor * > SpawnActorWithInfo(const FTransform &Transform, FActorDescription thisActorDescription, FCarlaActor::IdType DesiredId=0)
Spawns an actor based on ActorDescription at Transform.
void SetActorState(carla::rpc::ActorState InState)
Definition: CarlaActor.h:110
Response< ActorId > CommandResponse
virtual ECarlaServerResponse ResetTrafficLightGroup()
Definition: CarlaActor.h:395
void SetAttachmentType(carla::rpc::AttachmentType InAttachmentType)
Definition: CarlaActor.h:140
bool LoadNewEpisode(const FString &MapString, bool ResetSettings=true)
Load a new map and start a new episode.
const TArray< FActorDefinition > & GetActorDefinitions() const
Return the list of actor definitions that are available to be spawned this episode.
Definition: CarlaEpisode.h:103
virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControlOut &)
Definition: CarlaActor.h:370
#define RESPOND_ERROR(str)
virtual ECarlaServerResponse ApplyControlToWalker(const FWalkerControl &)
Definition: CarlaActor.h:360
static std::vector< T > MakeVectorFromTArray(const TArray< Other > &Array)
Definition: CarlaServer.cpp:79
Token token() const
Token associated with this stream.
Definition: detail/Stream.h:35
auto GetId() const
Return the unique id of this episode.
Definition: CarlaEpisode.h:82
void SetLights(FString Client, std::vector< carla::rpc::LightState > LightsToSet, bool DiscardClient=false)
void SetReplayerIgnoreHero(bool IgnoreHero)
constexpr size_t milliseconds() const noexcept
Definition: Time.h:58
virtual ECarlaServerResponse SetLightGreenTime(float)
Definition: CarlaActor.h:335
TArray< FString > GetNamesOfAllActors()
static FString GetXODR(const UWorld *World)
Return the OpenDrive XML associated to MapName, or empty if the file is not found.
uint32_t ActorId
Definition: ActorId.h:14
bool TickCueReceived()
ACarlaRecorder * GetRecorder() const
Definition: CarlaEpisode.h:271
virtual ECarlaServerResponse SetLightYellowTime(float)
Definition: CarlaActor.h:340
carla::rpc::Server Server
virtual ECarlaServerResponse SetLightRedTime(float)
Definition: CarlaActor.h:345
ALargeMapManager * GetLMManager() const
static auto MakeOverload(FuncTs &&... fs)
Creates an "overloaded callable object" out of one or more callable objects, each callable object wil...
Definition: Functional.h:27
virtual ECarlaServerResponse SetActorAutopilot(bool, bool bKeepState=false)
Definition: CarlaActor.h:292
carla::rpc::Server & _server
ECarlaServerResponse SetActorTargetVelocity(const FVector &Velocity)
Definition: CarlaActor.cpp:387
virtual ECarlaServerResponse OpenVehicleDoor(const EVehicleDoor)
Definition: CarlaActor.h:251
virtual ECarlaServerResponse EnableCarSim(const FString &)
Definition: CarlaActor.h:302
FTransform LocalToGlobalTransform(const FTransform &InTransform) const
TUniquePtr< FPimpl > Pimpl
Definition: CarlaServer.h:42
APawn * GetSpectatorPawn() const
Definition: CarlaEpisode.h:123
void AttachActors(AActor *Child, AActor *Parent, EAttachmentType InAttachmentType=EAttachmentType::Rigid)
Attach Child to Parent.
void AddChildren(IdType ChildId)
Definition: CarlaActor.h:125
virtual ECarlaServerResponse UseCarSimRoad(bool)
Definition: CarlaActor.h:307
static std::pair< bool, carla::rpc::LabelledPoint > ProjectPoint(FVector StartLocation, FVector Direction, float MaxDistance, UWorld *World)
Definition: RayTracer.cpp:38
std::string ShowFileInfo(std::string Name, bool bShowAll=false)
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
Definition: CarlaStatics.h:100
carla::streaming::Server StreamingServer
ECarlaServerResponse AddActorImpulseAtLocation(const FVector &Impulse, const FVector &Location)
Definition: CarlaActor.cpp:449
TArray< FBoundingBox > GetAllBBsOfLevel(uint8 TagQueried=0xFF) const
bool IsDormant() const
Definition: CarlaActor.h:70
Base class for the CARLA Game Mode.
virtual ECarlaServerResponse SetActorSimulatePhysics(bool bEnabled)
Definition: CarlaActor.cpp:557
virtual ECarlaServerResponse GetVehicleLightState(FVehicleLightState &)
Definition: CarlaActor.h:246
const FEpisodeSettings & GetSettings() const
Definition: CarlaEpisode.h:69
void SetParent(IdType InParentId)
Definition: CarlaActor.h:115
IdType GetActorId() const
Definition: CarlaActor.h:80
ECarlaServerResponse AddActorForce(const FVector &Force)
Definition: CarlaActor.cpp:473
UCarlaEpisode * Episode
Texture< sensor::data::Color > TextureColor
Definition: Texture.h:66
virtual ECarlaServerResponse ApplyPhysicsControl(const FVehiclePhysicsControl &)
Definition: CarlaActor.h:261
const FString GetFullMapPath() const
Base class for CARLA wheeled vehicles.
virtual ECarlaServerResponse GetWheelSteerAngle(const EVehicleWheelLocation &, float &)
Definition: CarlaActor.h:276
Texture< FloatColor > TextureFloatColor
Definition: Texture.h:67
void AsyncRun(uint32 NumberOfWorkerThreads)
void SetActorGlobalTransform(const FTransform &Transform, ETeleportType Teleport=ETeleportType::TeleportPhysics)
Definition: CarlaActor.cpp:330
ECarlaServerResponse AddActorAngularImpulse(const FVector &AngularInpulse)
Definition: CarlaActor.cpp:517
#define REQUIRE_CARLA_EPISODE()
geom::Transform Transform
Definition: rpc/Transform.h:16
FVector LocalToGlobalLocation(const FVector &InLocation) const
constexpr ServerBinder(const char *name, carla::rpc::Server &srv, bool sync)
virtual ECarlaServerResponse ShowVehicleDebugTelemetry(bool)
Definition: CarlaActor.h:297
virtual ECarlaServerResponse SetWalkerState(const FTransform &Transform, carla::rpc::WalkerControl WalkerControl)
Definition: CarlaActor.h:353
ActorType GetActorType() const
Definition: CarlaActor.h:85
A view over an actor and its properties.
Definition: CarlaActor.h:23
FVehicleLightState GetVehicleLightState() const
void ApplySettings(const FEpisodeSettings &Settings)
void SetSynchronousMode(bool is_synchro)
static std::vector< carla::rpc::LabelledPoint > CastRay(FVector StartLocation, FVector EndLocation, UWorld *World)
Definition: RayTracer.cpp:15
void NotifyBeginEpisode(UCarlaEpisode &Episode)