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"
11 #include "EngineUtils.h"
12 
16 #include "Carla/Util/RayTracer.h"
20 #include "GameFramework/CharacterMovementComponent.h"
21 #include "Carla/Game/Tagger.h"
26 #include "Carla/Actor/ActorData.h"
27 #include "CarlaServerResponse.h"
29 #include "Misc/FileHelper.h"
30 
32 #include <carla/Functional.h>
33 #include <carla/multigpu/router.h>
34 #include <carla/Version.h>
36 #include <carla/rpc/Actor.h>
40 #include <carla/rpc/Command.h>
42 #include <carla/rpc/DebugShape.h>
44 #include <carla/rpc/EpisodeInfo.h>
47 #include <carla/rpc/LightState.h>
48 #include <carla/rpc/MapInfo.h>
49 #include <carla/rpc/MapLayer.h>
50 #include <carla/rpc/Response.h>
51 #include <carla/rpc/Server.h>
52 #include <carla/rpc/String.h>
53 #include <carla/rpc/Transform.h>
54 #include <carla/rpc/Vector2D.h>
55 #include <carla/rpc/Vector3D.h>
56 #include <carla/rpc/VehicleDoor.h>
68 #include <carla/rpc/Texture.h>
71 
72 #include <vector>
73 #include <atomic>
74 #include <map>
75 #include <tuple>
76 
77 template <typename T>
79 
80 // =============================================================================
81 // -- Static local functions ---------------------------------------------------
82 // =============================================================================
83 
84 template <typename T, typename Other>
85 static std::vector<T> MakeVectorFromTArray(const TArray<Other> &Array)
86 {
87  return {Array.GetData(), Array.GetData() + Array.Num()};
88 }
89 
90 // =============================================================================
91 // -- FCarlaServer::FPimpl -----------------------------------------------
92 // =============================================================================
93 
95 {
96 public:
97 
98  FPimpl(uint16_t RPCPort, uint16_t StreamingPort, uint16_t SecondaryPort)
99  : Server(RPCPort),
100  StreamingServer(StreamingPort),
101  BroadcastStream(StreamingServer.MakeStream())
102  {
103  // we need to create shared_ptr from the router for some handlers to live
104  SecondaryServer = std::make_shared<carla::multigpu::Router>(SecondaryPort);
105  SecondaryServer->SetCallbacks();
106  BindActions();
107  }
108 
109  std::shared_ptr<carla::multigpu::Router> GetSecondaryServer() {
110  return SecondaryServer;
111  }
112 
113  /// Map of pairs < port , ip > with all the Traffic Managers active in the simulation
114  std::map<uint16_t, std::string> TrafficManagerInfo;
115 
117 
119 
121 
122  std::shared_ptr<carla::multigpu::Router> SecondaryServer;
123 
124  UCarlaEpisode *Episode = nullptr;
125 
126  std::atomic_size_t TickCuesReceived { 0u };
127 
128 private:
129 
130  void BindActions();
131 };
132 
133 // =============================================================================
134 // -- Define helper macros -----------------------------------------------------
135 // =============================================================================
136 
137 #if WITH_EDITOR
138 # define CARLA_ENSURE_GAME_THREAD() check(IsInGameThread());
139 #else
140 # define CARLA_ENSURE_GAME_THREAD()
141 #endif // WITH_EDITOR
142 
143 #define RESPOND_ERROR(str) { \
144  UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), TEXT(str)); \
145  return carla::rpc::ResponseError(str); }
146 
147 #define RESPOND_ERROR_FSTRING(fstr) { \
148  UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), *fstr); \
149  return carla::rpc::ResponseError(carla::rpc::FromFString(fstr)); }
150 
151 #define REQUIRE_CARLA_EPISODE() \
152  CARLA_ENSURE_GAME_THREAD(); \
153  if (Episode == nullptr) { RESPOND_ERROR("episode not ready"); }
154 
156  const FString& FuncName,
157  const FString& ErrorMessage,
158  const FString& ExtraInfo = "")
159 {
160  FString TotalMessage = "Responding error from function " + FuncName + ": " +
161  ErrorMessage + ". " + ExtraInfo;
162  UE_LOG(LogCarlaServer, Log, TEXT("%s"), *TotalMessage);
163  return carla::rpc::ResponseError(carla::rpc::FromFString(TotalMessage));
164 }
165 
167  const FString& FuncName,
168  const ECarlaServerResponse& Error,
169  const FString& ExtraInfo = "")
170 {
171  return RespondError(FuncName, CarlaGetStringError(Error), ExtraInfo);
172 }
173 
175 {
176 public:
177 
178  constexpr ServerBinder(const char *name, carla::rpc::Server &srv, bool sync)
179  : _name(name),
180  _server(srv),
181  _sync(sync) {}
182 
183  template <typename FuncT>
184  auto operator<<(FuncT func)
185  {
186  if (_sync)
187  {
188  _server.BindSync(_name, func);
189  }
190  else
191  {
192  _server.BindAsync(_name, func);
193  }
194  return func;
195  }
196 
197 private:
198 
199  const char *_name;
200 
202 
203  bool _sync;
204 };
205 
206 #define BIND_SYNC(name) auto name = ServerBinder(# name, Server, true)
207 #define BIND_ASYNC(name) auto name = ServerBinder(# name, Server, false)
208 
209 // =============================================================================
210 // -- Bind Actions -------------------------------------------------------------
211 // =============================================================================
212 
214 {
215  namespace cr = carla::rpc;
216  namespace cg = carla::geom;
217 
218  /// Looks for a Traffic Manager running on port
219  BIND_SYNC(is_traffic_manager_running) << [this] (uint16_t port) ->R<bool>
220  {
221  return (TrafficManagerInfo.find(port) != TrafficManagerInfo.end());
222  };
223 
224  /// Gets a pair filled with the <IP, port> of the Trafic Manager running on port.
225  /// If there is no Traffic Manager running the pair will be ("", 0)
226  BIND_SYNC(get_traffic_manager_running) << [this] (uint16_t port) ->R<std::pair<std::string, uint16_t>>
227  {
228  auto it = TrafficManagerInfo.find(port);
229  if(it != TrafficManagerInfo.end()) {
230  return std::pair<std::string, uint16_t>(it->second, it->first);
231  }
232  return std::pair<std::string, uint16_t>("",0);
233  };
234 
235  /// Add a new Traffic Manager running on <IP, port>
236  BIND_SYNC(add_traffic_manager_running) << [this] (std::pair<std::string, uint16_t> trafficManagerInfo) ->R<bool>
237  {
238  uint16_t port = trafficManagerInfo.second;
239  auto it = TrafficManagerInfo.find(port);
240  if(it == TrafficManagerInfo.end()) {
241  TrafficManagerInfo.insert(
242  std::pair<uint16_t, std::string>(port, trafficManagerInfo.first));
243  return true;
244  }
245  return false;
246 
247  };
248 
249  BIND_SYNC(destroy_traffic_manager) << [this] (uint16_t port) ->R<bool>
250  {
251  auto it = TrafficManagerInfo.find(port);
252  if(it != TrafficManagerInfo.end()) {
253  TrafficManagerInfo.erase(it);
254  return true;
255  }
256  return false;
257  };
258 
259  BIND_ASYNC(version) << [] () -> R<std::string>
260  {
261  return carla::version();
262  };
263 
264  // ~~ Tick ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
265 
266  BIND_SYNC(tick_cue) << [this]() -> R<uint64_t>
267  {
268  TRACE_CPUPROFILER_EVENT_SCOPE(TickCueReceived);
269  auto Current = FCarlaEngine::GetFrameCounter();
270  (void)TickCuesReceived.fetch_add(1, std::memory_order_release);
271  return Current + 1;
272  };
273 
274  // ~~ Load new episode ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
275 
276  BIND_ASYNC(get_available_maps) << [this]() -> R<std::vector<std::string>>
277  {
278  const auto MapNames = UCarlaStatics::GetAllMapNames();
279  std::vector<std::string> result;
280  result.reserve(MapNames.Num());
281  for (const auto &MapName : MapNames)
282  {
283  if (MapName.Contains("/Sublevels/"))
284  continue;
285  if (MapName.Contains("/BaseMap/"))
286  continue;
287  if (MapName.Contains("/BaseLargeMap/"))
288  continue;
289  if (MapName.Contains("_Tile_"))
290  continue;
291 
292  result.emplace_back(cr::FromFString(MapName));
293  }
294  return result;
295  };
296 
297  BIND_SYNC(load_new_episode) << [this](const std::string &map_name, const bool reset_settings, cr::MapLayer MapLayers) -> R<void>
298  {
300 
301  UCarlaGameInstance* GameInstance = UCarlaStatics::GetGameInstance(Episode->GetWorld());
302  if (!GameInstance)
303  {
304  RESPOND_ERROR("unable to find CARLA game instance");
305  }
306  GameInstance->SetMapLayer(static_cast<int32>(MapLayers));
307 
308  if(!Episode->LoadNewEpisode(cr::ToFString(map_name), reset_settings))
309  {
310  FString Str(TEXT("Map '"));
311  Str += cr::ToFString(map_name);
312  Str += TEXT("' not found");
314  }
315 
316  return R<void>::Success();
317  };
318 
319  BIND_SYNC(load_map_layer) << [this](cr::MapLayer MapLayers) -> R<void>
320  {
322 
323  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
324  if (!GameMode)
325  {
326  RESPOND_ERROR("unable to find CARLA game mode");
327  }
328  GameMode->LoadMapLayer(static_cast<int32>(MapLayers));
329 
330  return R<void>::Success();
331  };
332 
333  BIND_SYNC(unload_map_layer) << [this](cr::MapLayer MapLayers) -> R<void>
334  {
336 
337  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
338  if (!GameMode)
339  {
340  RESPOND_ERROR("unable to find CARLA game mode");
341  }
342  GameMode->UnLoadMapLayer(static_cast<int32>(MapLayers));
343 
344  return R<void>::Success();
345  };
346 
347  BIND_SYNC(copy_opendrive_to_file) << [this](const std::string &opendrive, cr::OpendriveGenerationParameters Params) -> R<void>
348  {
350  if (!Episode->LoadNewOpendriveEpisode(cr::ToLongFString(opendrive), Params))
351  {
352  RESPOND_ERROR("opendrive could not be correctly parsed");
353  }
354  return R<void>::Success();
355  };
356 
357  BIND_SYNC(apply_color_texture_to_objects) << [this](
358  const std::vector<std::string> &actors_name,
359  const cr::MaterialParameter& parameter,
360  const cr::TextureColor& Texture) -> R<void>
361  {
363  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
364  if (!GameMode)
365  {
366  RESPOND_ERROR("unable to find CARLA game mode");
367  }
368  TArray<AActor*> ActorsToPaint;
369  for(const std::string& actor_name : actors_name)
370  {
371  AActor* ActorToPaint = GameMode->FindActorByName(cr::ToFString(actor_name));
372  if (ActorToPaint)
373  {
374  ActorsToPaint.Add(ActorToPaint);
375  }
376  }
377 
378  if(!ActorsToPaint.Num())
379  {
380  RESPOND_ERROR("unable to find Actor to apply the texture");
381  }
382 
383  UTexture2D* UETexture = GameMode->CreateUETexture(Texture);
384 
385  for(AActor* ActorToPaint : ActorsToPaint)
386  {
387  GameMode->ApplyTextureToActor(
388  ActorToPaint,
389  UETexture,
390  parameter);
391  }
392  return R<void>::Success();
393  };
394 
395  BIND_SYNC(apply_float_color_texture_to_objects) << [this](
396  const std::vector<std::string> &actors_name,
397  const cr::MaterialParameter& parameter,
398  const cr::TextureFloatColor& Texture) -> R<void>
399  {
401  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
402  if (!GameMode)
403  {
404  RESPOND_ERROR("unable to find CARLA game mode");
405  }
406  TArray<AActor*> ActorsToPaint;
407  for(const std::string& actor_name : actors_name)
408  {
409  AActor* ActorToPaint = GameMode->FindActorByName(cr::ToFString(actor_name));
410  if (ActorToPaint)
411  {
412  ActorsToPaint.Add(ActorToPaint);
413  }
414  }
415 
416  if(!ActorsToPaint.Num())
417  {
418  RESPOND_ERROR("unable to find Actor to apply the texture");
419  }
420 
421  UTexture2D* UETexture = GameMode->CreateUETexture(Texture);
422 
423  for(AActor* ActorToPaint : ActorsToPaint)
424  {
425  GameMode->ApplyTextureToActor(
426  ActorToPaint,
427  UETexture,
428  parameter);
429  }
430  return R<void>::Success();
431  };
432 
433  BIND_SYNC(get_names_of_all_objects) << [this]() -> R<std::vector<std::string>>
434  {
436  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
437  if (!GameMode)
438  {
439  RESPOND_ERROR("unable to find CARLA game mode");
440  }
441  TArray<FString> NamesFString = GameMode->GetNamesOfAllActors();
442  std::vector<std::string> NamesStd;
443  for (const FString &Name : NamesFString)
444  {
445  NamesStd.emplace_back(cr::FromFString(Name));
446  }
447  return NamesStd;
448  };
449 
450  // ~~ Episode settings and info ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
451 
452  BIND_SYNC(get_episode_info) << [this]() -> R<cr::EpisodeInfo>
453  {
455  return cr::EpisodeInfo{Episode->GetId(), BroadcastStream.token()};
456  };
457 
458  BIND_SYNC(get_map_info) << [this]() -> R<cr::MapInfo>
459  {
461  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
462  const auto &SpawnPoints = Episode->GetRecommendedSpawnPoints();
463  FString FullMapPath = GameMode->GetFullMapPath();
464  FString MapDir = FullMapPath.RightChop(FullMapPath.Find("Content/", ESearchCase::CaseSensitive) + 8);
465  MapDir += "/" + Episode->GetMapName();
466  return cr::MapInfo{
467  cr::FromFString(MapDir),
468  MakeVectorFromTArray<cg::Transform>(SpawnPoints)};
469  };
470 
471  BIND_SYNC(get_map_data) << [this]() -> R<std::string>
472  {
474  return cr::FromLongFString(UOpenDrive::GetXODR(Episode->GetWorld()));
475  };
476 
477  BIND_SYNC(get_navigation_mesh) << [this]() -> R<std::vector<uint8_t>>
478  {
480  auto FileContents = FNavigationMesh::Load(Episode->GetMapName());
481  // make a mem copy (from TArray to std::vector)
482  std::vector<uint8_t> Result(FileContents.Num());
483  memcpy(&Result[0], FileContents.GetData(), FileContents.Num());
484  return Result;
485  };
486 
487  BIND_SYNC(get_required_files) << [this](std::string folder = "") -> R<std::vector<std::string>>
488  {
490 
491  // Check that the path ends in a slash, add it otherwise
492  if (folder[folder.size() - 1] != '/' && folder[folder.size() - 1] != '\\') {
493  folder += "/";
494  }
495 
496  // Get the map's folder absolute path and check if it's in its own folder
497  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
498  const auto mapDir = GameMode->GetFullMapPath();
499  const auto folderDir = mapDir + "/" + folder.c_str();
500  const auto fileName = mapDir.EndsWith(Episode->GetMapName()) ? "*" : Episode->GetMapName();
501 
502  // Find all the xodr and bin files from the map
503  TArray<FString> Files;
504  IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName + ".xodr"), true, false, false);
505  IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName + ".bin"), true, false, false);
506 
507  // Remove the start of the path until the content folder and put each file in the result
508  std::vector<std::string> result;
509  for (auto File : Files) {
510  File.RemoveFromStart(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
511  result.emplace_back(TCHAR_TO_UTF8(*File));
512  }
513 
514  return result;
515  };
516  BIND_SYNC(request_file) << [this](std::string name) -> R<std::vector<uint8_t>>
517  {
519 
520  // Get the absolute path of the file
521  FString path(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
522  path.Append(name.c_str());
523 
524  // Copy the binary data of the file into the result and return it
525  TArray<uint8_t> Content;
526  FFileHelper::LoadFileToArray(Content, *path, 0);
527  std::vector<uint8_t> Result(Content.Num());
528  memcpy(&Result[0], Content.GetData(), Content.Num());
529 
530  return Result;
531  };
532 
533  BIND_SYNC(get_episode_settings) << [this]() -> R<cr::EpisodeSettings>
534  {
536  return cr::EpisodeSettings{Episode->GetSettings()};
537  };
538 
539  BIND_SYNC(set_episode_settings) << [this](
540  const cr::EpisodeSettings &settings) -> R<uint64_t>
541  {
543  Episode->ApplySettings(settings);
544  StreamingServer.SetSynchronousMode(settings.synchronous_mode);
545 
546  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
547  if (!GameMode)
548  {
549  RESPOND_ERROR("unable to find CARLA game mode");
550  }
551  ALargeMapManager* LargeMap = GameMode->GetLMManager();
552  if (LargeMap)
553  {
554  LargeMap->ConsiderSpectatorAsEgo(settings.spectator_as_ego);
555  }
556 
558  };
559 
560  BIND_SYNC(get_actor_definitions) << [this]() -> R<std::vector<cr::ActorDefinition>>
561  {
563  return MakeVectorFromTArray<cr::ActorDefinition>(Episode->GetActorDefinitions());
564  };
565 
566  BIND_SYNC(get_spectator) << [this]() -> R<cr::Actor>
567  {
570  if (!CarlaActor)
571  {
572  RESPOND_ERROR("internal error: unable to find spectator");
573  }
574  return Episode->SerializeActor(CarlaActor);
575  };
576 
577  BIND_SYNC(get_all_level_BBs) << [this](uint8 QueriedTag) -> R<std::vector<cg::BoundingBox>>
578  {
580  TArray<FBoundingBox> Result;
581  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
582  if (!GameMode)
583  {
584  RESPOND_ERROR("unable to find CARLA game mode");
585  }
586  Result = GameMode->GetAllBBsOfLevel(QueriedTag);
587  ALargeMapManager* LargeMap = GameMode->GetLMManager();
588  if (LargeMap)
589  {
590  for(auto& Box : Result)
591  {
592  Box.Origin = LargeMap->LocalToGlobalLocation(Box.Origin);
593  }
594  }
595  return MakeVectorFromTArray<cg::BoundingBox>(Result);
596  };
597 
598  BIND_SYNC(get_environment_objects) << [this](uint8 QueriedTag) -> R<std::vector<cr::EnvironmentObject>>
599  {
601  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
602  if (!GameMode)
603  {
604  RESPOND_ERROR("unable to find CARLA game mode");
605  }
606  TArray<FEnvironmentObject> Result = GameMode->GetEnvironmentObjects(QueriedTag);
607  ALargeMapManager* LargeMap = GameMode->GetLMManager();
608  if (LargeMap)
609  {
610  for(auto& Object : Result)
611  {
612  Object.Transform = LargeMap->LocalToGlobalTransform(Object.Transform);
613  }
614  }
615  return MakeVectorFromTArray<cr::EnvironmentObject>(Result);
616  };
617 
618  BIND_SYNC(enable_environment_objects) << [this](std::vector<uint64_t> EnvObjectIds, bool Enable) -> R<void>
619  {
621  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
622  if (!GameMode)
623  {
624  RESPOND_ERROR("unable to find CARLA game mode");
625  }
626 
627  TSet<uint64> EnvObjectIdsSet;
628  for(uint64 Id : EnvObjectIds)
629  {
630  EnvObjectIdsSet.Emplace(Id);
631  }
632 
633  GameMode->EnableEnvironmentObjects(EnvObjectIdsSet, Enable);
634  return R<void>::Success();
635  };
636 
637  // ~~ Weather ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
638 
639  BIND_SYNC(get_weather_parameters) << [this]() -> R<cr::WeatherParameters>
640  {
642  auto *Weather = Episode->GetWeather();
643  if (Weather == nullptr)
644  {
645  RESPOND_ERROR("internal error: unable to find weather");
646  }
647  return Weather->GetCurrentWeather();
648  };
649 
650  BIND_SYNC(set_weather_parameters) << [this](
651  const cr::WeatherParameters &weather) -> R<void>
652  {
654  auto *Weather = Episode->GetWeather();
655  if (Weather == nullptr)
656  {
657  RESPOND_ERROR("internal error: unable to find weather");
658  }
659  Weather->ApplyWeather(weather);
660  return R<void>::Success();
661  };
662 
663  // ~~ Actor operations ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
664 
665  BIND_SYNC(get_actors_by_id) << [this](
666  const std::vector<FCarlaActor::IdType> &ids) -> R<std::vector<cr::Actor>>
667  {
669  std::vector<cr::Actor> Result;
670  Result.reserve(ids.size());
671  for (auto &&Id : ids)
672  {
673  FCarlaActor* View = Episode->FindCarlaActor(Id);
674  if (View)
675  {
676  Result.emplace_back(Episode->SerializeActor(View));
677  }
678  }
679  return Result;
680  };
681 
682  BIND_SYNC(spawn_actor) << [this](
683  cr::ActorDescription Description,
685  {
687 
688  auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description));
689 
690  if (Result.Key != EActorSpawnResultStatus::Success)
691  {
692  UE_LOG(LogCarla, Error, TEXT("Actor not Spawned"));
694  }
695 
697  if(LargeMap)
698  {
699  LargeMap->OnActorSpawned(*Result.Value);
700  }
701 
702  return Episode->SerializeActor(Result.Value);
703  };
704 
705  BIND_SYNC(spawn_actor_with_parent) << [this](
706  cr::ActorDescription Description,
707  const cr::Transform &Transform,
708  cr::ActorId ParentId,
709  cr::AttachmentType InAttachmentType) -> R<cr::Actor>
710  {
712 
713  auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description));
714  if (Result.Key != EActorSpawnResultStatus::Success)
715  {
717  }
718 
719  FCarlaActor* CarlaActor = Episode->FindCarlaActor(Result.Value->GetActorId());
720  if (!CarlaActor)
721  {
722  RESPOND_ERROR("internal error: actor could not be spawned");
723  }
724 
725  FCarlaActor* ParentCarlaActor = Episode->FindCarlaActor(ParentId);
726 
727  if (!ParentCarlaActor)
728  {
729  RESPOND_ERROR("unable to attach actor: parent actor not found");
730  }
731 
732  CarlaActor->SetParent(ParentId);
733  CarlaActor->SetAttachmentType(InAttachmentType);
734  ParentCarlaActor->AddChildren(CarlaActor->GetActorId());
735 
736  #if defined(WITH_ROS2)
737  auto ROS2 = carla::ros2::ROS2::GetInstance();
738  if (ROS2->IsEnabled())
739  {
740  FCarlaActor* CurrentActor = ParentCarlaActor;
741  while(CurrentActor)
742  {
743  for (const auto &Attr : CurrentActor->GetActorInfo()->Description.Variations)
744  {
745  if (Attr.Key == "ros_name")
746  {
747  const std::string value = std::string(TCHAR_TO_UTF8(*Attr.Value.Value));
748  ROS2->AddActorParentRosName(static_cast<void*>(CarlaActor->GetActor()), static_cast<void*>(CurrentActor->GetActor()));
749  }
750  }
751  CurrentActor = Episode->FindCarlaActor(CurrentActor->GetParent());
752  }
753  }
754  #endif
755 
756  // Only is possible to attach if the actor has been really spawned and
757  // is not in dormant state
758  if(!ParentCarlaActor->IsDormant())
759  {
761  CarlaActor->GetActor(),
762  ParentCarlaActor->GetActor(),
763  static_cast<EAttachmentType>(InAttachmentType));
764  }
765  else
766  {
767  Episode->PutActorToSleep(CarlaActor->GetActorId());
768  }
769 
770  return Episode->SerializeActor(CarlaActor);
771  };
772 
773  BIND_SYNC(destroy_actor) << [this](cr::ActorId ActorId) -> R<bool>
774  {
776  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
777  if ( !CarlaActor )
778  {
779  RESPOND_ERROR("unable to destroy actor: not found");
780  }
781  UE_LOG(LogCarla, Log, TEXT("CarlaServer destroy_actor %d"), ActorId);
782  // We need to force the actor state change, since dormant actors
783  // will ignore the FCarlaActor destruction
784  CarlaActor->SetActorState(cr::ActorState::PendingKill);
786  {
787  RESPOND_ERROR("internal error: unable to destroy actor");
788  }
789  return true;
790  };
791 
792  BIND_SYNC(console_command) << [this](std::string cmd) -> R<bool>
793  {
795  APlayerController* PController= UGameplayStatics::GetPlayerController(Episode->GetWorld(), 0);
796  if( PController )
797  {
798  auto result = PController->ConsoleCommand(UTF8_TO_TCHAR(cmd.c_str()), true);
799  return !(
800  result.Contains(FString(TEXT("Command not recognized"))) ||
801  result.Contains(FString(TEXT("Error")))
802  );
803  }
804  return GEngine->Exec(Episode->GetWorld(), UTF8_TO_TCHAR(cmd.c_str()));
805  };
806 
807  BIND_SYNC(get_sensor_token) << [this](carla::streaming::detail::stream_id_type sensor_id) ->
809  {
811  bool ForceInPrimary = false;
812 
813  // check for the world observer (always in primary server)
814  if (sensor_id == 1)
815  {
816  ForceInPrimary = true;
817  }
818 
819  // collision sensor always in primary server in multi-gpu
820  FString Desc = Episode->GetActorDescriptionFromStream(sensor_id);
821  if (Desc == "" || Desc == "sensor.other.collision")
822  {
823  ForceInPrimary = true;
824  }
825 
826  if (SecondaryServer->HasClientsConnected() && !ForceInPrimary)
827  {
828  // multi-gpu
829  UE_LOG(LogCarla, Log, TEXT("Sensor %d '%s' created in secondary server"), sensor_id, *Desc);
830  return SecondaryServer->GetCommander().GetToken(sensor_id);
831  }
832  else
833  {
834  // single-gpu
835  UE_LOG(LogCarla, Log, TEXT("Sensor %d '%s' created in primary server"), sensor_id, *Desc);
836  return StreamingServer.GetToken(sensor_id);
837  }
838  };
839 
840  BIND_SYNC(enable_sensor_for_ros) << [this](carla::streaming::detail::stream_id_type sensor_id) ->
841  R<void>
842  {
844  bool ForceInPrimary = false;
845 
846  // check for the world observer (always in primary server)
847  if (sensor_id == 1)
848  {
849  ForceInPrimary = true;
850  }
851 
852  // collision sensor always in primary server in multi-gpu
853  FString Desc = Episode->GetActorDescriptionFromStream(sensor_id);
854  if (Desc == "" || Desc == "sensor.other.collision")
855  {
856  ForceInPrimary = true;
857  }
858 
859  if (SecondaryServer->HasClientsConnected() && !ForceInPrimary)
860  {
861  // multi-gpu
862  SecondaryServer->GetCommander().EnableForROS(sensor_id);
863  }
864  else
865  {
866  // single-gpu
867  StreamingServer.EnableForROS(sensor_id);
868  }
869  return R<void>::Success();
870  };
871 
872  BIND_SYNC(disable_sensor_for_ros) << [this](carla::streaming::detail::stream_id_type sensor_id) ->
873  R<void>
874  {
876  bool ForceInPrimary = false;
877 
878  // check for the world observer (always in primary server)
879  if (sensor_id == 1)
880  {
881  ForceInPrimary = true;
882  }
883 
884  // collision sensor always in primary server in multi-gpu
885  FString Desc = Episode->GetActorDescriptionFromStream(sensor_id);
886  if (Desc == "" || Desc == "sensor.other.collision")
887  {
888  ForceInPrimary = true;
889  }
890 
891  if (SecondaryServer->HasClientsConnected() && !ForceInPrimary)
892  {
893  // multi-gpu
894  SecondaryServer->GetCommander().DisableForROS(sensor_id);
895  }
896  else
897  {
898  // single-gpu
899  StreamingServer.DisableForROS(sensor_id);
900  }
901  return R<void>::Success();
902  };
903 
904 BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_id_type sensor_id) ->
905  R<bool>
906  {
908  bool ForceInPrimary = false;
909 
910  // check for the world observer (always in primary server)
911  if (sensor_id == 1)
912  {
913  ForceInPrimary = true;
914  }
915 
916  // collision sensor always in primary server in multi-gpu
917  FString Desc = Episode->GetActorDescriptionFromStream(sensor_id);
918  if (Desc == "" || Desc == "sensor.other.collision")
919  {
920  ForceInPrimary = true;
921  }
922 
923  if (SecondaryServer->HasClientsConnected() && !ForceInPrimary)
924  {
925  // multi-gpu
926  return SecondaryServer->GetCommander().IsEnabledForROS(sensor_id);
927  }
928  else
929  {
930  // single-gpu
931  return StreamingServer.IsEnabledForROS(sensor_id);
932  }
933  };
934 
935  // ~~ Actor physics ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
936 
937  BIND_SYNC(set_actor_location) << [this](
940  {
942  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
943  if (!CarlaActor)
944  {
945  return RespondError(
946  "set_actor_location",
948  " Actor Id: " + FString::FromInt(ActorId));
949  }
950 
951  CarlaActor->SetActorGlobalLocation(
952  Location, ETeleportType::TeleportPhysics);
953  return R<void>::Success();
954  };
955 
956  BIND_SYNC(set_actor_transform) << [this](
959  {
961  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
962  if (!CarlaActor)
963  {
964  return RespondError(
965  "set_actor_transform",
967  " Actor Id: " + FString::FromInt(ActorId));
968  }
969 
970  CarlaActor->SetActorGlobalTransform(
971  Transform, ETeleportType::TeleportPhysics);
972  return R<void>::Success();
973  };
974 
975  BIND_SYNC(set_walker_state) << [this] (
978  float Speed) -> R<void>
979  {
981  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
982  if (!CarlaActor)
983  {
984  return RespondError(
985  "set_walker_state",
987  " Actor Id: " + FString::FromInt(ActorId));
988  }
989 
990  // apply walker transform
991  ECarlaServerResponse Response =
992  CarlaActor->SetWalkerState(
993  Transform,
994  cr::WalkerControl(
995  Transform.GetForwardVector(), Speed, false));
996  if (Response != ECarlaServerResponse::Success)
997  {
998  return RespondError(
999  "set_walker_state",
1000  Response,
1001  " Actor Id: " + FString::FromInt(ActorId));
1002  }
1003  return R<void>::Success();
1004  };
1005 
1006  BIND_SYNC(set_actor_target_velocity) << [this](
1008  cr::Vector3D vector) -> R<void>
1009  {
1011  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1012  if (!CarlaActor)
1013  {
1014  return RespondError(
1015  "set_actor_target_velocity",
1017  " Actor Id: " + FString::FromInt(ActorId));
1018  }
1019  ECarlaServerResponse Response =
1020  CarlaActor->SetActorTargetVelocity(vector.ToCentimeters().ToFVector());
1021  if (Response != ECarlaServerResponse::Success)
1022  {
1023  return RespondError(
1024  "set_actor_target_velocity",
1025  Response,
1026  " Actor Id: " + FString::FromInt(ActorId));
1027  }
1028  return R<void>::Success();
1029  };
1030 
1031  BIND_SYNC(set_actor_target_angular_velocity) << [this](
1033  cr::Vector3D vector) -> R<void>
1034  {
1036  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1037  if (!CarlaActor)
1038  {
1039  return RespondError(
1040  "set_actor_target_angular_velocity",
1042  " Actor Id: " + FString::FromInt(ActorId));
1043  }
1044  ECarlaServerResponse Response =
1045  CarlaActor->SetActorTargetAngularVelocity(vector.ToFVector());
1046  if (Response != ECarlaServerResponse::Success)
1047  {
1048  return RespondError(
1049  "set_actor_target_angular_velocity",
1050  Response,
1051  " Actor Id: " + FString::FromInt(ActorId));
1052  }
1053  return R<void>::Success();
1054  };
1055 
1056  BIND_SYNC(enable_actor_constant_velocity) << [this](
1058  cr::Vector3D vector) -> R<void>
1059  {
1061  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1062  if (!CarlaActor)
1063  {
1064  return RespondError(
1065  "enable_actor_constant_velocity",
1067  " Actor Id: " + FString::FromInt(ActorId));
1068  }
1069 
1070  ECarlaServerResponse Response =
1071  CarlaActor->EnableActorConstantVelocity(vector.ToCentimeters().ToFVector());
1072  if (Response != ECarlaServerResponse::Success)
1073  {
1074  return RespondError(
1075  "enable_actor_constant_velocity",
1076  Response,
1077  " Actor Id: " + FString::FromInt(ActorId));
1078  }
1079 
1080  return R<void>::Success();
1081  };
1082 
1083  BIND_SYNC(disable_actor_constant_velocity) << [this](
1085  {
1087  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1088  if (!CarlaActor)
1089  {
1090  return RespondError(
1091  "disable_actor_constant_velocity",
1093  " Actor Id: " + FString::FromInt(ActorId));
1094  }
1095 
1096  ECarlaServerResponse Response =
1097  CarlaActor->DisableActorConstantVelocity();
1098  if (Response != ECarlaServerResponse::Success)
1099  {
1100  return RespondError(
1101  "disable_actor_constant_velocity",
1102  Response,
1103  " Actor Id: " + FString::FromInt(ActorId));
1104  }
1105 
1106  return R<void>::Success();
1107  };
1108 
1109  BIND_SYNC(add_actor_impulse) << [this](
1111  cr::Vector3D vector) -> R<void>
1112  {
1114  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1115  if (!CarlaActor)
1116  {
1117  return RespondError(
1118  "add_actor_impulse",
1120  " Actor Id: " + FString::FromInt(ActorId));
1121  }
1122 
1123  ECarlaServerResponse Response =
1124  CarlaActor->AddActorImpulse(vector.ToCentimeters().ToFVector());
1125  if (Response != ECarlaServerResponse::Success)
1126  {
1127  return RespondError(
1128  "add_actor_impulse",
1129  Response,
1130  " Actor Id: " + FString::FromInt(ActorId));
1131  }
1132  return R<void>::Success();
1133  };
1134 
1135  BIND_SYNC(add_actor_impulse_at_location) << [this](
1137  cr::Vector3D impulse,
1138  cr::Vector3D location) -> R<void>
1139  {
1141  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1142  if (!CarlaActor)
1143  {
1144  return RespondError(
1145  "add_actor_impulse_at_location",
1147  " Actor Id: " + FString::FromInt(ActorId));
1148  }
1149  FVector UELocation = location.ToCentimeters().ToFVector();
1150  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
1151  ALargeMapManager* LargeMap = GameMode->GetLMManager();
1152  if (LargeMap)
1153  {
1154  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
1155  }
1156  ECarlaServerResponse Response =
1157  CarlaActor->AddActorImpulseAtLocation(impulse.ToCentimeters().ToFVector(), UELocation);
1158  if (Response != ECarlaServerResponse::Success)
1159  {
1160  return RespondError(
1161  "add_actor_impulse_at_location",
1162  Response,
1163  " Actor Id: " + FString::FromInt(ActorId));
1164  }
1165 
1166  return R<void>::Success();
1167  };
1168 
1169  BIND_SYNC(add_actor_force) << [this](
1171  cr::Vector3D vector) -> R<void>
1172  {
1174  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1175  if (!CarlaActor)
1176  {
1177  return RespondError(
1178  "add_actor_force",
1180  " Actor Id: " + FString::FromInt(ActorId));
1181  }
1182  ECarlaServerResponse Response =
1183  CarlaActor->AddActorForce(vector.ToCentimeters().ToFVector());
1184  if (Response != ECarlaServerResponse::Success)
1185  {
1186  return RespondError(
1187  "add_actor_force",
1188  Response,
1189  " Actor Id: " + FString::FromInt(ActorId));
1190  }
1191  return R<void>::Success();
1192  };
1193 
1194  BIND_SYNC(add_actor_force_at_location) << [this](
1196  cr::Vector3D force,
1197  cr::Vector3D location) -> R<void>
1198  {
1200  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1201  if (!CarlaActor)
1202  {
1203  return RespondError(
1204  "add_actor_force_at_location",
1206  " Actor Id: " + FString::FromInt(ActorId));
1207  }
1208  FVector UELocation = location.ToCentimeters().ToFVector();
1209  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
1210  ALargeMapManager* LargeMap = GameMode->GetLMManager();
1211  if (LargeMap)
1212  {
1213  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
1214  }
1215  ECarlaServerResponse Response =
1216  CarlaActor->AddActorForceAtLocation(UELocation, force.ToCentimeters().ToFVector());
1217  if (Response != ECarlaServerResponse::Success)
1218  {
1219  return RespondError(
1220  "add_actor_force_at_location",
1221  Response,
1222  " Actor Id: " + FString::FromInt(ActorId));
1223  }
1224  return R<void>::Success();
1225  };
1226 
1227  BIND_SYNC(add_actor_angular_impulse) << [this](
1229  cr::Vector3D vector) -> R<void>
1230  {
1232  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1233  if (!CarlaActor)
1234  {
1235  return RespondError(
1236  "add_actor_angular_impulse",
1238  " Actor Id: " + FString::FromInt(ActorId));
1239  }
1240  ECarlaServerResponse Response =
1241  CarlaActor->AddActorAngularImpulse(vector.ToFVector());
1242  if (Response != ECarlaServerResponse::Success)
1243  {
1244  return RespondError(
1245  "add_actor_angular_impulse",
1246  Response,
1247  " Actor Id: " + FString::FromInt(ActorId));
1248  }
1249  return R<void>::Success();
1250  };
1251 
1252  BIND_SYNC(add_actor_torque) << [this](
1254  cr::Vector3D vector) -> R<void>
1255  {
1257  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1258  if (!CarlaActor)
1259  {
1260  return RespondError(
1261  "add_actor_torque",
1263  " Actor Id: " + FString::FromInt(ActorId));
1264  }
1265  ECarlaServerResponse Response =
1266  CarlaActor->AddActorTorque(vector.ToFVector());
1267  if (Response != ECarlaServerResponse::Success)
1268  {
1269  return RespondError(
1270  "add_actor_torque",
1271  Response,
1272  " Actor Id: " + FString::FromInt(ActorId));
1273  }
1274  return R<void>::Success();
1275  };
1276 
1277  BIND_SYNC(get_physics_control) << [this](
1279  {
1281  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1282  if (!CarlaActor)
1283  {
1284  return RespondError(
1285  "get_physics_control",
1287  " Actor Id: " + FString::FromInt(ActorId));
1288  }
1290  ECarlaServerResponse Response =
1291  CarlaActor->GetPhysicsControl(PhysicsControl);
1292  if (Response != ECarlaServerResponse::Success)
1293  {
1294  return RespondError(
1295  "get_physics_control",
1296  Response,
1297  " Actor Id: " + FString::FromInt(ActorId));
1298  }
1299  return cr::VehiclePhysicsControl(PhysicsControl);
1300  };
1301 
1302  BIND_SYNC(get_vehicle_light_state) << [this](
1304  {
1306  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1307  if (!CarlaActor)
1308  {
1309  return RespondError(
1310  "get_vehicle_light_state",
1312  " Actor Id: " + FString::FromInt(ActorId));
1313  }
1314  FVehicleLightState LightState;
1315  ECarlaServerResponse Response =
1316  CarlaActor->GetVehicleLightState(LightState);
1317  if (Response != ECarlaServerResponse::Success)
1318  {
1319  return RespondError(
1320  "get_vehicle_light_state",
1321  Response,
1322  " Actor Id: " + FString::FromInt(ActorId));
1323  }
1324  return cr::VehicleLightState(LightState);
1325  };
1326 
1327  BIND_SYNC(apply_physics_control) << [this](
1329  cr::VehiclePhysicsControl PhysicsControl) -> R<void>
1330  {
1332  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1333  if (!CarlaActor)
1334  {
1335  return RespondError(
1336  "apply_physics_control",
1338  " Actor Id: " + FString::FromInt(ActorId));
1339  }
1340  ECarlaServerResponse Response =
1342  if (Response != ECarlaServerResponse::Success)
1343  {
1344  return RespondError(
1345  "apply_physics_control",
1346  Response,
1347  " Actor Id: " + FString::FromInt(ActorId));
1348  }
1349  return R<void>::Success();
1350  };
1351 
1352  BIND_SYNC(set_vehicle_light_state) << [this](
1354  cr::VehicleLightState LightState) -> R<void>
1355  {
1357  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1358  if (!CarlaActor)
1359  {
1360  return RespondError(
1361  "set_vehicle_light_state",
1363  " Actor Id: " + FString::FromInt(ActorId));
1364  }
1365  ECarlaServerResponse Response =
1366  CarlaActor->SetVehicleLightState(FVehicleLightState(LightState));
1367  if (Response != ECarlaServerResponse::Success)
1368  {
1369  return RespondError(
1370  "set_vehicle_light_state",
1371  Response,
1372  " Actor Id: " + FString::FromInt(ActorId));
1373  }
1374  return R<void>::Success();
1375  };
1376 
1377 
1378  BIND_SYNC(open_vehicle_door) << [this](
1380  cr::VehicleDoor DoorIdx) -> R<void>
1381  {
1383  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1384  if (!CarlaActor)
1385  {
1386  return RespondError(
1387  "open_vehicle_door",
1389  " Actor Id: " + FString::FromInt(ActorId));
1390  }
1391  ECarlaServerResponse Response =
1392  CarlaActor->OpenVehicleDoor(static_cast<EVehicleDoor>(DoorIdx));
1393  if (Response != ECarlaServerResponse::Success)
1394  {
1395  return RespondError(
1396  "open_vehicle_door",
1397  Response,
1398  " Actor Id: " + FString::FromInt(ActorId));
1399  }
1400  return R<void>::Success();
1401  };
1402 
1403  BIND_SYNC(close_vehicle_door) << [this](
1405  cr::VehicleDoor DoorIdx) -> R<void>
1406  {
1408  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1409  if (!CarlaActor)
1410  {
1411  return RespondError(
1412  "close_vehicle_door",
1414  " Actor Id: " + FString::FromInt(ActorId));
1415  }
1416  ECarlaServerResponse Response =
1417  CarlaActor->CloseVehicleDoor(static_cast<EVehicleDoor>(DoorIdx));
1418  if (Response != ECarlaServerResponse::Success)
1419  {
1420  return RespondError(
1421  "close_vehicle_door",
1422  Response,
1423  " Actor Id: " + FString::FromInt(ActorId));
1424  }
1425  return R<void>::Success();
1426  };
1427 
1428  BIND_SYNC(set_wheel_steer_direction) << [this](
1430  cr::VehicleWheelLocation WheelLocation,
1431  float AngleInDeg) -> R<void>
1432  {
1434  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1435  if(!CarlaActor){
1436  return RespondError(
1437  "set_wheel_steer_direction",
1439  " Actor Id: " + FString::FromInt(ActorId));
1440  }
1441  ECarlaServerResponse Response =
1442  CarlaActor->SetWheelSteerDirection(
1443  static_cast<EVehicleWheelLocation>(WheelLocation), AngleInDeg);
1444  if (Response != ECarlaServerResponse::Success)
1445  {
1446  return RespondError(
1447  "set_wheel_steer_direction",
1448  Response,
1449  " Actor Id: " + FString::FromInt(ActorId));
1450  }
1451  return R<void>::Success();
1452  };
1453 
1454  BIND_SYNC(get_wheel_steer_angle) << [this](
1455  const cr::ActorId ActorId,
1456  cr::VehicleWheelLocation WheelLocation) -> R<float>
1457  {
1459  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1460  if(!CarlaActor){
1461  return RespondError(
1462  "get_wheel_steer_angle",
1464  " Actor Id: " + FString::FromInt(ActorId));
1465  }
1466  float Angle;
1467  ECarlaServerResponse Response =
1468  CarlaActor->GetWheelSteerAngle(
1469  static_cast<EVehicleWheelLocation>(WheelLocation), Angle);
1470  if (Response != ECarlaServerResponse::Success)
1471  {
1472  return RespondError(
1473  "get_wheel_steer_angle",
1474  Response,
1475  " Actor Id: " + FString::FromInt(ActorId));
1476  }
1477  return Angle;
1478  };
1479 
1480  BIND_SYNC(set_actor_simulate_physics) << [this](
1482  bool bEnabled) -> R<void>
1483  {
1485  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1486  if (!CarlaActor)
1487  {
1488  return RespondError(
1489  "set_actor_simulate_physics",
1491  " Actor Id: " + FString::FromInt(ActorId));
1492  }
1493  ECarlaServerResponse Response =
1494  CarlaActor->SetActorSimulatePhysics(bEnabled);
1495  if (Response != ECarlaServerResponse::Success)
1496  {
1497  return RespondError(
1498  "set_actor_simulate_physics",
1499  Response,
1500  " Actor Id: " + FString::FromInt(ActorId));
1501  }
1502  return R<void>::Success();
1503  };
1504 
1505  BIND_SYNC(set_actor_collisions) << [this](
1507  bool bEnabled) -> R<void>
1508  {
1510  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1511  if (!CarlaActor)
1512  {
1513  return RespondError(
1514  "set_actor_collisions",
1516  " Actor Id: " + FString::FromInt(ActorId));
1517  }
1518  ECarlaServerResponse Response =
1519  CarlaActor->SetActorCollisions(bEnabled);
1520  if (Response != ECarlaServerResponse::Success)
1521  {
1522  return RespondError(
1523  "set_actor_collisions",
1524  Response,
1525  " Actor Id: " + FString::FromInt(ActorId));
1526  }
1527  return R<void>::Success();
1528  };
1529 
1530  BIND_SYNC(set_actor_dead) << [this](
1532  {
1534  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1535  if (!CarlaActor)
1536  {
1537  return RespondError(
1538  "set_actor_dead",
1540  " Actor Id: " + FString::FromInt(ActorId));
1541  }
1542  ECarlaServerResponse Response =
1543  CarlaActor->SetActorDead();
1544  if (Response != ECarlaServerResponse::Success)
1545  {
1546  return RespondError(
1547  "set_actor_dead",
1548  Response,
1549  " Actor Id: " + FString::FromInt(ActorId));
1550  }
1551  return R<void>::Success();
1552  };
1553 
1554  BIND_SYNC(set_actor_enable_gravity) << [this](
1556  bool bEnabled) -> R<void>
1557  {
1559  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1560  if (!CarlaActor)
1561  {
1562  return RespondError(
1563  "set_actor_enable_gravity",
1565  " Actor Id: " + FString::FromInt(ActorId));
1566  }
1567  ECarlaServerResponse Response =
1568  CarlaActor->SetActorEnableGravity(bEnabled);
1569  if (Response != ECarlaServerResponse::Success)
1570  {
1571  return RespondError(
1572  "set_actor_enable_gravity",
1573  Response,
1574  " Actor Id: " + FString::FromInt(ActorId));
1575  }
1576  return R<void>::Success();
1577  };
1578 
1579  // ~~ Apply control ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1580 
1581  BIND_SYNC(apply_control_to_vehicle) << [this](
1583  cr::VehicleControl Control) -> R<void>
1584  {
1586  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1587  if (!CarlaActor)
1588  {
1589  return RespondError(
1590  "apply_control_to_vehicle",
1592  " Actor Id: " + FString::FromInt(ActorId));
1593  }
1594  ECarlaServerResponse Response =
1595  CarlaActor->ApplyControlToVehicle(Control, EVehicleInputPriority::Client);
1596  if (Response != ECarlaServerResponse::Success)
1597  {
1598  return RespondError(
1599  "apply_control_to_vehicle",
1600  Response,
1601  " Actor Id: " + FString::FromInt(ActorId));
1602  }
1603  return R<void>::Success();
1604  };
1605 
1606  BIND_SYNC(apply_ackermann_control_to_vehicle) << [this](
1608  cr::VehicleAckermannControl Control) -> R<void>
1609  {
1611  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1612  if (!CarlaActor)
1613  {
1614  return RespondError(
1615  "apply_ackermann_control_to_vehicle",
1617  " Actor Id: " + FString::FromInt(ActorId));
1618  }
1619  ECarlaServerResponse Response =
1620  CarlaActor->ApplyAckermannControlToVehicle(Control, EVehicleInputPriority::Client);
1621  if (Response != ECarlaServerResponse::Success)
1622  {
1623  return RespondError(
1624  "apply_ackermann_control_to_vehicle",
1625  Response,
1626  " Actor Id: " + FString::FromInt(ActorId));
1627  }
1628  return R<void>::Success();
1629  };
1630 
1631  BIND_SYNC(get_ackermann_controller_settings) << [this](
1633  {
1635  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1636  if (!CarlaActor)
1637  {
1638  return RespondError(
1639  "get_ackermann_controller_settings",
1641  " Actor Id: " + FString::FromInt(ActorId));
1642  }
1644  ECarlaServerResponse Response =
1645  CarlaActor->GetAckermannControllerSettings(Settings);
1646  if (Response != ECarlaServerResponse::Success)
1647  {
1648  return RespondError(
1649  "get_ackermann_controller_settings",
1650  Response,
1651  " Actor Id: " + FString::FromInt(ActorId));
1652  }
1653  return cr::AckermannControllerSettings(Settings);
1654  };
1655 
1656  BIND_SYNC(apply_ackermann_controller_settings) << [this](
1658  cr::AckermannControllerSettings AckermannSettings) -> R<void>
1659  {
1661  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1662  if (!CarlaActor)
1663  {
1664  return RespondError(
1665  "apply_ackermann_controller_settings",
1667  " Actor Id: " + FString::FromInt(ActorId));
1668  }
1669  ECarlaServerResponse Response =
1670  CarlaActor->ApplyAckermannControllerSettings(FAckermannControllerSettings(AckermannSettings));
1671  if (Response != ECarlaServerResponse::Success)
1672  {
1673  return RespondError(
1674  "apply_ackermann_controller_settings",
1675  Response,
1676  " Actor Id: " + FString::FromInt(ActorId));
1677  }
1678  return R<void>::Success();
1679  };
1680 
1681  BIND_SYNC(apply_control_to_walker) << [this](
1683  cr::WalkerControl Control) -> R<void>
1684  {
1686  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1687  if (!CarlaActor)
1688  {
1689  return RespondError(
1690  "apply_control_to_walker",
1692  " Actor Id: " + FString::FromInt(ActorId));
1693  }
1694  ECarlaServerResponse Response =
1695  CarlaActor->ApplyControlToWalker(Control);
1696  if (Response != ECarlaServerResponse::Success)
1697  {
1698  return RespondError(
1699  "apply_control_to_walker",
1700  Response,
1701  " Actor Id: " + FString::FromInt(ActorId));
1702  }
1703  return R<void>::Success();
1704  };
1705 
1706  BIND_SYNC(get_bones_transform) << [this](
1708  {
1710  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1711  if (!CarlaActor)
1712  {
1713  return RespondError(
1714  "get_bones_transform",
1716  " Actor Id: " + FString::FromInt(ActorId));
1717  }
1718  FWalkerBoneControlOut Bones;
1719  ECarlaServerResponse Response =
1720  CarlaActor->GetBonesTransform(Bones);
1721  if (Response != ECarlaServerResponse::Success)
1722  {
1723  return RespondError(
1724  "get_bones_transform",
1725  Response,
1726  " Actor Id: " + FString::FromInt(ActorId));
1727  }
1728 
1729  std::vector<carla::rpc::BoneTransformDataOut> BoneData;
1730  for (auto Bone : Bones.BoneTransforms)
1731  {
1733  Data.bone_name = std::string(TCHAR_TO_UTF8(*Bone.Get<0>()));
1734  FWalkerBoneControlOutData Transforms = Bone.Get<1>();
1735  Data.world = Transforms.World;
1736  Data.component = Transforms.Component;
1737  Data.relative = Transforms.Relative;
1738  BoneData.push_back(Data);
1739  }
1740  return carla::rpc::WalkerBoneControlOut(BoneData);
1741  };
1742 
1743  BIND_SYNC(set_bones_transform) << [this](
1746  {
1748  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1749  if (!CarlaActor)
1750  {
1751  return RespondError(
1752  "set_bones_transform",
1754  " Actor Id: " + FString::FromInt(ActorId));
1755  }
1756 
1758  ECarlaServerResponse Response = CarlaActor->SetBonesTransform(Bones2);
1759  if (Response != ECarlaServerResponse::Success)
1760  {
1761  return RespondError(
1762  "set_bones_transform",
1763  Response,
1764  " Actor Id: " + FString::FromInt(ActorId));
1765  }
1766 
1767  return R<void>::Success();
1768  };
1769 
1770  BIND_SYNC(blend_pose) << [this](
1772  float Blend) -> R<void>
1773  {
1775  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1776  if (!CarlaActor)
1777  {
1778  return RespondError(
1779  "blend_pose",
1781  " Actor Id: " + FString::FromInt(ActorId));
1782  }
1783 
1784  ECarlaServerResponse Response = CarlaActor->BlendPose(Blend);
1785  if (Response != ECarlaServerResponse::Success)
1786  {
1787  return RespondError(
1788  "blend_pose",
1789  Response,
1790  " Actor Id: " + FString::FromInt(ActorId));
1791  }
1792 
1793  return R<void>::Success();
1794  };
1795 
1796  BIND_SYNC(get_pose_from_animation) << [this](
1798  {
1800  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1801  if (!CarlaActor)
1802  {
1803  return RespondError(
1804  "get_pose_from_animation",
1806  " Actor Id: " + FString::FromInt(ActorId));
1807  }
1808 
1809  ECarlaServerResponse Response = CarlaActor->GetPoseFromAnimation();
1810  if (Response != ECarlaServerResponse::Success)
1811  {
1812  return RespondError(
1813  "get_pose_from_animation",
1814  Response,
1815  " Actor Id: " + FString::FromInt(ActorId));
1816  }
1817 
1818  return R<void>::Success();
1819  };
1820 
1821  BIND_SYNC(set_actor_autopilot) << [this](
1823  bool bEnabled) -> R<void>
1824  {
1826  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1827  if (!CarlaActor)
1828  {
1829  return RespondError(
1830  "set_actor_autopilot",
1832  " Actor Id: " + FString::FromInt(ActorId));
1833  }
1834  ECarlaServerResponse Response =
1835  CarlaActor->SetActorAutopilot(bEnabled);
1836  if (Response != ECarlaServerResponse::Success)
1837  {
1838  return RespondError(
1839  "set_actor_autopilot",
1840  Response,
1841  " Actor Id: " + FString::FromInt(ActorId));
1842  }
1843  return R<void>::Success();
1844  };
1845 
1846  BIND_SYNC(show_vehicle_debug_telemetry) << [this](
1848  bool bEnabled) -> R<void>
1849  {
1851  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1852  if (!CarlaActor)
1853  {
1854  return RespondError(
1855  "show_vehicle_debug_telemetry",
1857  " Actor Id: " + FString::FromInt(ActorId));
1858  }
1859  ECarlaServerResponse Response =
1860  CarlaActor->ShowVehicleDebugTelemetry(bEnabled);
1861  if (Response != ECarlaServerResponse::Success)
1862  {
1863  return RespondError(
1864  "show_vehicle_debug_telemetry",
1865  Response,
1866  " Actor Id: " + FString::FromInt(ActorId));
1867  }
1868  return R<void>::Success();
1869  };
1870 
1871  BIND_SYNC(enable_carsim) << [this](
1873  std::string SimfilePath) -> R<void>
1874  {
1876  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1877  if (!CarlaActor)
1878  {
1879  return RespondError(
1880  "enable_carsim",
1882  " Actor Id: " + FString::FromInt(ActorId));
1883  }
1884  ECarlaServerResponse Response =
1885  CarlaActor->EnableCarSim(carla::rpc::ToFString(SimfilePath));
1886  if (Response != ECarlaServerResponse::Success)
1887  {
1888  return RespondError(
1889  "enable_carsim",
1890  Response,
1891  " Actor Id: " + FString::FromInt(ActorId));
1892  }
1893  return R<void>::Success();
1894  };
1895 
1896  BIND_SYNC(use_carsim_road) << [this](
1898  bool bEnabled) -> R<void>
1899  {
1901  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1902  if (!CarlaActor)
1903  {
1904  return RespondError(
1905  "use_carsim_road",
1907  " Actor Id: " + FString::FromInt(ActorId));
1908  }
1909  ECarlaServerResponse Response =
1910  CarlaActor->UseCarSimRoad(bEnabled);
1911  if (Response != ECarlaServerResponse::Success)
1912  {
1913  return RespondError(
1914  "use_carsim_road",
1915  Response,
1916  " Actor Id: " + FString::FromInt(ActorId));
1917  }
1918  return R<void>::Success();
1919  };
1920 
1921  BIND_SYNC(enable_chrono_physics) << [this](
1923  uint64_t MaxSubsteps,
1924  float MaxSubstepDeltaTime,
1925  std::string VehicleJSON,
1926  std::string PowertrainJSON,
1927  std::string TireJSON,
1928  std::string BaseJSONPath) -> R<void>
1929  {
1931  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1932  if (!CarlaActor)
1933  {
1934  return RespondError(
1935  "enable_chrono_physics",
1937  " Actor Id: " + FString::FromInt(ActorId));
1938  }
1939  ECarlaServerResponse Response =
1940  CarlaActor->EnableChronoPhysics(
1941  MaxSubsteps, MaxSubstepDeltaTime,
1942  cr::ToFString(VehicleJSON),
1943  cr::ToFString(PowertrainJSON),
1944  cr::ToFString(TireJSON),
1945  cr::ToFString(BaseJSONPath));
1946  if (Response != ECarlaServerResponse::Success)
1947  {
1948  return RespondError(
1949  "enable_chrono_physics",
1950  Response,
1951  " Actor Id: " + FString::FromInt(ActorId));
1952  }
1953  return R<void>::Success();
1954  };
1955 
1956  // ~~ Traffic lights ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1957 
1958  BIND_SYNC(set_traffic_light_state) << [this](
1960  cr::TrafficLightState trafficLightState) -> R<void>
1961  {
1963  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1964  if (!CarlaActor)
1965  {
1966  return RespondError(
1967  "set_traffic_light_state",
1969  " Actor Id: " + FString::FromInt(ActorId));
1970  }
1971  ECarlaServerResponse Response =
1972  CarlaActor->SetTrafficLightState(
1973  static_cast<ETrafficLightState>(trafficLightState));
1974  if (Response != ECarlaServerResponse::Success)
1975  {
1976  return RespondError(
1977  "set_traffic_light_state",
1978  Response,
1979  " Actor Id: " + FString::FromInt(ActorId));
1980  }
1981  return R<void>::Success();
1982  };
1983 
1984  BIND_SYNC(set_traffic_light_green_time) << [this](
1986  float GreenTime) -> R<void>
1987  {
1989  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
1990  if (!CarlaActor)
1991  {
1992  return RespondError(
1993  "set_traffic_light_green_time",
1995  " Actor Id: " + FString::FromInt(ActorId));
1996  }
1997  ECarlaServerResponse Response =
1998  CarlaActor->SetLightGreenTime(GreenTime);
1999  if (Response != ECarlaServerResponse::Success)
2000  {
2001  return RespondError(
2002  "set_traffic_light_green_time",
2003  Response,
2004  " Actor Id: " + FString::FromInt(ActorId));
2005  }
2006  return R<void>::Success();
2007  };
2008 
2009  BIND_SYNC(set_traffic_light_yellow_time) << [this](
2011  float YellowTime) -> R<void>
2012  {
2014  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2015  if (!CarlaActor)
2016  {
2017  return RespondError(
2018  "set_traffic_light_yellow_time",
2020  " Actor Id: " + FString::FromInt(ActorId));
2021  }
2022  ECarlaServerResponse Response =
2023  CarlaActor->SetLightYellowTime(YellowTime);
2024  if (Response != ECarlaServerResponse::Success)
2025  {
2026  return RespondError(
2027  "set_traffic_light_yellow_time",
2028  Response,
2029  " Actor Id: " + FString::FromInt(ActorId));
2030  }
2031  return R<void>::Success();
2032  };
2033 
2034  BIND_SYNC(set_traffic_light_red_time) << [this](
2036  float RedTime) -> R<void>
2037  {
2039  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2040  if (!CarlaActor)
2041  {
2042  return RespondError(
2043  "set_traffic_light_red_time",
2045  " Actor Id: " + FString::FromInt(ActorId));
2046  }
2047  ECarlaServerResponse Response =
2048  CarlaActor->SetLightRedTime(RedTime);
2049  if (Response != ECarlaServerResponse::Success)
2050  {
2051  return RespondError(
2052  "set_traffic_light_red_time",
2053  Response,
2054  " Actor Id: " + FString::FromInt(ActorId));
2055  }
2056  return R<void>::Success();
2057  };
2058 
2059  BIND_SYNC(freeze_traffic_light) << [this](
2061  bool Freeze) -> R<void>
2062  {
2064  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2065  if (!CarlaActor)
2066  {
2067  return RespondError(
2068  "freeze_traffic_light",
2070  " Actor Id: " + FString::FromInt(ActorId));
2071  }
2072  ECarlaServerResponse Response =
2073  CarlaActor->FreezeTrafficLight(Freeze);
2074  if (Response != ECarlaServerResponse::Success)
2075  {
2076  return RespondError(
2077  "freeze_traffic_light",
2078  Response,
2079  " Actor Id: " + FString::FromInt(ActorId));
2080  }
2081  return R<void>::Success();
2082  };
2083 
2084  BIND_SYNC(reset_traffic_light_group) << [this](
2086  {
2088  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2089  if (!CarlaActor)
2090  {
2091  return RespondError(
2092  "reset_traffic_light_group",
2094  " Actor Id: " + FString::FromInt(ActorId));
2095  }
2096  ECarlaServerResponse Response =
2097  CarlaActor->ResetTrafficLightGroup();
2098  if (Response != ECarlaServerResponse::Success)
2099  {
2100  return RespondError(
2101  "reset_traffic_light_group",
2102  Response,
2103  " Actor Id: " + FString::FromInt(ActorId));
2104  }
2105  return R<void>::Success();
2106  };
2107 
2108  BIND_SYNC(reset_all_traffic_lights) << [this]() -> R<void>
2109  {
2111  for (TActorIterator<ATrafficLightGroup> It(Episode->GetWorld()); It; ++It)
2112  {
2113  It->ResetGroup();
2114  }
2115  return R<void>::Success();
2116  };
2117 
2118  BIND_SYNC(freeze_all_traffic_lights) << [this]
2119  (bool frozen) -> R<void>
2120  {
2122  auto* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
2123  if (!GameMode)
2124  {
2125  RESPOND_ERROR("unable to find CARLA game mode");
2126  }
2127  auto* TraffiLightManager = GameMode->GetTrafficLightManager();
2128  TraffiLightManager->SetFrozen(frozen);
2129  return R<void>::Success();
2130  };
2131 
2132  BIND_SYNC(get_vehicle_light_states) << [this]() -> R<cr::VehicleLightStateList>
2133  {
2136 
2137  auto It = Episode->GetActorRegistry().begin();
2138  for (; It != Episode->GetActorRegistry().end(); ++It)
2139  {
2140  const FCarlaActor& View = *(It.Value().Get());
2142  {
2143  if(View.IsDormant())
2144  {
2145  // todo: implement
2146  }
2147  else
2148  {
2149  auto Actor = View.GetActor();
2150  if (!Actor->IsPendingKill())
2151  {
2152  const ACarlaWheeledVehicle *Vehicle = Cast<ACarlaWheeledVehicle>(Actor);
2153  List.emplace_back(
2154  View.GetActorId(),
2155  cr::VehicleLightState(Vehicle->GetVehicleLightState()).GetLightStateAsValue());
2156  }
2157  }
2158  }
2159  }
2160  return List;
2161  };
2162 
2163  BIND_SYNC(get_group_traffic_lights) << [this](
2164  const cr::ActorId ActorId) -> R<std::vector<cr::ActorId>>
2165  {
2167  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2168  if (!CarlaActor)
2169  {
2170  RESPOND_ERROR("unable to get group traffic lights: actor not found");
2171  }
2172  if (CarlaActor->IsDormant())
2173  {
2174  //todo implement
2175  return std::vector<cr::ActorId>();
2176  }
2177  else
2178  {
2179  auto TrafficLight = Cast<ATrafficLightBase>(CarlaActor->GetActor());
2180  if (TrafficLight == nullptr)
2181  {
2182  RESPOND_ERROR("unable to get group traffic lights: actor is not a traffic light");
2183  }
2184  std::vector<cr::ActorId> Result;
2185  for (auto* TLight : TrafficLight->GetGroupTrafficLights())
2186  {
2187  auto* View = Episode->FindCarlaActor(TLight);
2188  if (View)
2189  {
2190  Result.push_back(View->GetActorId());
2191  }
2192  }
2193  return Result;
2194  }
2195  };
2196 
2197  BIND_SYNC(get_light_boxes) << [this](
2198  const cr::ActorId ActorId) -> R<std::vector<cg::BoundingBox>>
2199  {
2201  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2202  if (!CarlaActor)
2203  {
2204  return RespondError(
2205  "get_light_boxes",
2207  " Actor Id: " + FString::FromInt(ActorId));
2208  }
2209  if (CarlaActor->IsDormant())
2210  {
2211  return RespondError(
2212  "get_light_boxes",
2214  " Actor Id: " + FString::FromInt(ActorId));
2215  }
2216  else
2217  {
2218  ATrafficLightBase* TrafficLight = Cast<ATrafficLightBase>(CarlaActor->GetActor());
2219  if (!TrafficLight)
2220  {
2221  return RespondError(
2222  "get_light_boxes",
2224  " Actor Id: " + FString::FromInt(ActorId));
2225  }
2226  TArray<FBoundingBox> Result;
2227  TArray<uint8> OutTag;
2229  TrafficLight, Result, OutTag,
2230  static_cast<uint8>(carla::rpc::CityObjectLabel::TrafficLight));
2231  return MakeVectorFromTArray<cg::BoundingBox>(Result);
2232  }
2233  };
2234 
2235  // ~~ GBuffer tokens ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2236  BIND_SYNC(get_gbuffer_token) << [this](const cr::ActorId ActorId, uint32_t GBufferId) -> R<std::vector<unsigned char>>
2237  {
2239  FCarlaActor* CarlaActor = Episode->FindCarlaActor(ActorId);
2240  if(!CarlaActor)
2241  {
2242  return RespondError(
2243  "get_gbuffer_token",
2245  " Actor Id: " + FString::FromInt(ActorId));
2246  }
2247  if (CarlaActor->IsDormant())
2248  {
2249  return RespondError(
2250  "get_gbuffer_token",
2252  " Actor Id: " + FString::FromInt(ActorId));
2253  }
2254  ASceneCaptureSensor* Sensor = Cast<ASceneCaptureSensor>(CarlaActor->GetActor());
2255  if (!Sensor)
2256  {
2257  return RespondError(
2258  "get_gbuffer_token",
2260  " Actor Id: " + FString::FromInt(ActorId));
2261  }
2262 
2263  switch (GBufferId)
2264  {
2265  case 0:
2266  {
2267  const auto &Token = Sensor->CameraGBuffers.SceneColor.GetToken();
2268  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2269  }
2270  case 1:
2271  {
2272  const auto &Token = Sensor->CameraGBuffers.SceneDepth.GetToken();
2273  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2274  }
2275  case 2:
2276  {
2277  const auto& Token = Sensor->CameraGBuffers.SceneStencil.GetToken();
2278  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2279  }
2280  case 3:
2281  {
2282  const auto &Token = Sensor->CameraGBuffers.GBufferA.GetToken();
2283  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2284  }
2285  case 4:
2286  {
2287  const auto &Token = Sensor->CameraGBuffers.GBufferB.GetToken();
2288  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2289  }
2290  case 5:
2291  {
2292  const auto &Token = Sensor->CameraGBuffers.GBufferC.GetToken();
2293  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2294  }
2295  case 6:
2296  {
2297  const auto &Token = Sensor->CameraGBuffers.GBufferD.GetToken();
2298  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2299  }
2300  case 7:
2301  {
2302  const auto &Token = Sensor->CameraGBuffers.GBufferE.GetToken();
2303  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2304  }
2305  case 8:
2306  {
2307  const auto &Token = Sensor->CameraGBuffers.GBufferF.GetToken();
2308  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2309  }
2310  case 9:
2311  {
2312  const auto &Token = Sensor->CameraGBuffers.Velocity.GetToken();
2313  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2314  }
2315  case 10:
2316  {
2317  const auto &Token = Sensor->CameraGBuffers.SSAO.GetToken();
2318  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2319  }
2320  case 11:
2321  {
2322  const auto& Token = Sensor->CameraGBuffers.CustomDepth.GetToken();
2323  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2324  }
2325  case 12:
2326  {
2327  const auto& Token = Sensor->CameraGBuffers.CustomStencil.GetToken();
2328  return std::vector<unsigned char>(std::begin(Token.data), std::end(Token.data));
2329  }
2330  default:
2331  UE_LOG(LogCarla, Error, TEXT("Requested invalid GBuffer ID %u"), GBufferId);
2332  return {};
2333  }
2334  };
2335 
2336  // ~~ Logging and playback ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2337 
2338  BIND_SYNC(start_recorder) << [this](std::string name, bool AdditionalData) -> R<std::string>
2339  {
2341  return R<std::string>(Episode->StartRecorder(name, AdditionalData));
2342  };
2343 
2344  BIND_SYNC(stop_recorder) << [this]() -> R<void>
2345  {
2347  Episode->GetRecorder()->Stop();
2348  return R<void>::Success();
2349  };
2350 
2351  BIND_SYNC(show_recorder_file_info) << [this](
2352  std::string name,
2353  bool show_all) -> R<std::string>
2354  {
2357  name,
2358  show_all));
2359  };
2360 
2361  BIND_SYNC(show_recorder_collisions) << [this](
2362  std::string name,
2363  char type1,
2364  char type2) -> R<std::string>
2365  {
2368  name,
2369  type1,
2370  type2));
2371  };
2372 
2373  BIND_SYNC(show_recorder_actors_blocked) << [this](
2374  std::string name,
2375  double min_time,
2376  double min_distance) -> R<std::string>
2377  {
2380  name,
2381  min_time,
2382  min_distance));
2383  };
2384 
2385  BIND_SYNC(replay_file) << [this](
2386  std::string name,
2387  double start,
2388  double duration,
2389  uint32_t follow_id,
2390  bool replay_sensors) -> R<std::string>
2391  {
2394  name,
2395  start,
2396  duration,
2397  follow_id,
2398  replay_sensors));
2399  };
2400 
2401  BIND_SYNC(set_replayer_time_factor) << [this](double time_factor) -> R<void>
2402  {
2404  Episode->GetRecorder()->SetReplayerTimeFactor(time_factor);
2405  return R<void>::Success();
2406  };
2407 
2408  BIND_SYNC(set_replayer_ignore_hero) << [this](bool ignore_hero) -> R<void>
2409  {
2411  Episode->GetRecorder()->SetReplayerIgnoreHero(ignore_hero);
2412  return R<void>::Success();
2413  };
2414 
2415  BIND_SYNC(set_replayer_ignore_spectator) << [this](bool ignore_spectator) -> R<void>
2416  {
2418  Episode->GetRecorder()->SetReplayerIgnoreSpectator(ignore_spectator);
2419  return R<void>::Success();
2420  };
2421 
2422  BIND_SYNC(stop_replayer) << [this](bool keep_actors) -> R<void>
2423  {
2425  Episode->GetRecorder()->StopReplayer(keep_actors);
2426  return R<void>::Success();
2427  };
2428 
2429  // ~~ Draw debug shapes ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2430 
2431  BIND_SYNC(draw_debug_shape) << [this](const cr::DebugShape &shape) -> R<void>
2432  {
2434  auto *World = Episode->GetWorld();
2435  check(World != nullptr);
2436  FDebugShapeDrawer Drawer(*World);
2437  Drawer.Draw(shape);
2438  return R<void>::Success();
2439  };
2440 
2441  // ~~ Apply commands in batch ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2442 
2443  using C = cr::Command;
2444  using CR = cr::CommandResponse;
2445  using ActorId = carla::ActorId;
2446 
2447  auto parse_result = [](ActorId id, const auto &response) {
2448  return response.HasError() ? CR{response.GetError()} : CR{id};
2449  };
2450 
2451 #define MAKE_RESULT(operation) return parse_result(c.actor, operation);
2452 
2453  auto command_visitor = carla::Functional::MakeRecursiveOverload(
2454  [=](auto self, const C::SpawnActor &c) -> CR {
2455  auto result = c.parent.has_value() ?
2456  spawn_actor_with_parent(
2457  c.description,
2458  c.transform,
2459  *c.parent,
2460  cr::AttachmentType::Rigid) :
2461  spawn_actor(c.description, c.transform);
2462  if (!result.HasError())
2463  {
2464  ActorId id = result.Get().id;
2465  auto set_id = carla::Functional::MakeOverload(
2466  [](C::SpawnActor &) {},
2467  [](C::ConsoleCommand &) {},
2468  [id](auto &s) { s.actor = id; });
2469  for (auto command : c.do_after)
2470  {
2471  boost::variant2::visit(set_id, command.command);
2472  boost::variant2::visit(self, command.command);
2473  }
2474  return id;
2475  }
2476  return result.GetError();
2477  },
2478  [=](auto, const C::DestroyActor &c) { MAKE_RESULT(destroy_actor(c.actor)); },
2479  [=](auto, const C::ApplyVehicleControl &c) { MAKE_RESULT(apply_control_to_vehicle(c.actor, c.control)); },
2480  [=](auto, const C::ApplyVehicleAckermannControl &c) { MAKE_RESULT(apply_ackermann_control_to_vehicle(c.actor, c.control)); },
2481  [=](auto, const C::ApplyWalkerControl &c) { MAKE_RESULT(apply_control_to_walker(c.actor, c.control)); },
2482  [=](auto, const C::ApplyVehiclePhysicsControl &c) { MAKE_RESULT(apply_physics_control(c.actor, c.physics_control)); },
2483  [=](auto, const C::ApplyTransform &c) { MAKE_RESULT(set_actor_transform(c.actor, c.transform)); },
2484  [=](auto, const C::ApplyTargetVelocity &c) { MAKE_RESULT(set_actor_target_velocity(c.actor, c.velocity)); },
2485  [=](auto, const C::ApplyTargetAngularVelocity &c) { MAKE_RESULT(set_actor_target_angular_velocity(c.actor, c.angular_velocity)); },
2486  [=](auto, const C::ApplyImpulse &c) { MAKE_RESULT(add_actor_impulse(c.actor, c.impulse)); },
2487  [=](auto, const C::ApplyForce &c) { MAKE_RESULT(add_actor_force(c.actor, c.force)); },
2488  [=](auto, const C::ApplyAngularImpulse &c) { MAKE_RESULT(add_actor_angular_impulse(c.actor, c.impulse)); },
2489  [=](auto, const C::ApplyTorque &c) { MAKE_RESULT(add_actor_torque(c.actor, c.torque)); },
2490  [=](auto, const C::SetSimulatePhysics &c) { MAKE_RESULT(set_actor_simulate_physics(c.actor, c.enabled)); },
2491  [=](auto, const C::SetEnableGravity &c) { MAKE_RESULT(set_actor_enable_gravity(c.actor, c.enabled)); },
2492  // TODO: SetAutopilot should be removed. This is the old way to control the vehicles
2493  [=](auto, const C::SetAutopilot &c) { MAKE_RESULT(set_actor_autopilot(c.actor, c.enabled)); },
2494  [=](auto, const C::ShowDebugTelemetry &c) { MAKE_RESULT(show_vehicle_debug_telemetry(c.actor, c.enabled)); },
2495  [=](auto, const C::SetVehicleLightState &c) { MAKE_RESULT(set_vehicle_light_state(c.actor, c.light_state)); },
2496 // [=](auto, const C::OpenVehicleDoor &c) { MAKE_RESULT(open_vehicle_door(c.actor, c.door_idx)); },
2497 // [=](auto, const C::CloseVehicleDoor &c) { MAKE_RESULT(close_vehicle_door(c.actor, c.door_idx)); },
2498  [=](auto, const C::ApplyWalkerState &c) { MAKE_RESULT(set_walker_state(c.actor, c.transform, c.speed)); },
2499  [=](auto, const C::ConsoleCommand& c) -> CR { return console_command(c.cmd); },
2500  [=](auto, const C::SetTrafficLightState& c) { MAKE_RESULT(set_traffic_light_state(c.actor, c.traffic_light_state)); },
2501  [=](auto, const C::ApplyLocation& c) { MAKE_RESULT(set_actor_location(c.actor, c.location)); }
2502  );
2503 
2504 #undef MAKE_RESULT
2505 
2506  BIND_SYNC(apply_batch) << [=](
2507  const std::vector<cr::Command> &commands,
2508  bool do_tick_cue)
2509  {
2510  std::vector<CR> result;
2511  result.reserve(commands.size());
2512  for (const auto &command : commands)
2513  {
2514  result.emplace_back(boost::variant2::visit(command_visitor, command.command));
2515  }
2516  if (do_tick_cue)
2517  {
2518  tick_cue();
2519  }
2520  return result;
2521  };
2522 
2523  // ~~ Light Subsystem ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2524 
2525  BIND_SYNC(query_lights_state) << [this](std::string client) -> R<std::vector<cr::LightState>>
2526  {
2528  std::vector<cr::LightState> result;
2529  auto *World = Episode->GetWorld();
2530  if(World) {
2531  UCarlaLightSubsystem* CarlaLightSubsystem = World->GetSubsystem<UCarlaLightSubsystem>();
2532  result = CarlaLightSubsystem->GetLights(FString(client.c_str()));
2533  }
2534  return result;
2535  };
2536 
2537  BIND_SYNC(update_lights_state) << [this]
2538  (std::string client, const std::vector<cr::LightState>& lights, bool discard_client) -> R<void>
2539  {
2541  auto *World = Episode->GetWorld();
2542  if(World) {
2543  UCarlaLightSubsystem* CarlaLightSubsystem = World->GetSubsystem<UCarlaLightSubsystem>();
2544  CarlaLightSubsystem->SetLights(FString(client.c_str()), lights, discard_client);
2545  }
2546  return R<void>::Success();
2547  };
2548 
2549  BIND_SYNC(update_day_night_cycle) << [this]
2550  (std::string client, const bool active) -> R<void>
2551  {
2553  auto *World = Episode->GetWorld();
2554  if(World) {
2555  UCarlaLightSubsystem* CarlaLightSubsystem = World->GetSubsystem<UCarlaLightSubsystem>();
2556  CarlaLightSubsystem->SetDayNightCycle(active);
2557  }
2558  return R<void>::Success();
2559  };
2560 
2561 
2562  // ~~ Ray Casting ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2563 
2564  BIND_SYNC(project_point) << [this]
2565  (cr::Location Location, cr::Vector3D Direction, float SearchDistance)
2566  -> R<std::pair<bool,cr::LabelledPoint>>
2567  {
2569  auto *World = Episode->GetWorld();
2570  constexpr float meter_to_centimeter = 100.0f;
2571  FVector UELocation = Location;
2572  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
2573  ALargeMapManager* LargeMap = GameMode->GetLMManager();
2574  if (LargeMap)
2575  {
2576  UELocation = LargeMap->GlobalToLocalLocation(UELocation);
2577  }
2578  return URayTracer::ProjectPoint(UELocation, Direction.ToFVector(),
2579  meter_to_centimeter * SearchDistance, World);
2580  };
2581 
2582  BIND_SYNC(cast_ray) << [this]
2583  (cr::Location StartLocation, cr::Location EndLocation)
2584  -> R<std::vector<cr::LabelledPoint>>
2585  {
2587  auto *World = Episode->GetWorld();
2588  FVector UEStartLocation = StartLocation;
2589  FVector UEEndLocation = EndLocation;
2590  ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld());
2591  ALargeMapManager* LargeMap = GameMode->GetLMManager();
2592  if (LargeMap)
2593  {
2594  UEStartLocation = LargeMap->GlobalToLocalLocation(UEStartLocation);
2595  UEEndLocation = LargeMap->GlobalToLocalLocation(UEEndLocation);
2596  }
2597  return URayTracer::CastRay(StartLocation, EndLocation, World);
2598  };
2599 
2600 }
2601 
2602 // =============================================================================
2603 // -- Undef helper macros ------------------------------------------------------
2604 // =============================================================================
2605 
2606 #undef BIND_ASYNC
2607 #undef BIND_SYNC
2608 #undef REQUIRE_CARLA_EPISODE
2609 #undef RESPOND_ERROR_FSTRING
2610 #undef RESPOND_ERROR
2611 #undef CARLA_ENSURE_GAME_THREAD
2612 
2613 // =============================================================================
2614 // -- FCarlaServer -------------------------------------------------------
2615 // =============================================================================
2616 
2618 
2620  Stop();
2621 }
2622 
2623 FDataMultiStream FCarlaServer::Start(uint16_t RPCPort, uint16_t StreamingPort, uint16_t SecondaryPort)
2624 {
2625  Pimpl = MakeUnique<FPimpl>(RPCPort, StreamingPort, SecondaryPort);
2626  StreamingPort = Pimpl->StreamingServer.GetLocalEndpoint().port();
2627  SecondaryPort = Pimpl->SecondaryServer->GetLocalEndpoint().port();
2628 
2629  UE_LOG(
2630  LogCarlaServer,
2631  Log,
2632  TEXT("Initialized CarlaServer: Ports(rpc=%d, streaming=%d, secondary=%d)"),
2633  RPCPort,
2634  StreamingPort,
2635  SecondaryPort);
2636  return Pimpl->BroadcastStream;
2637 }
2638 
2640 {
2641  check(Pimpl != nullptr);
2642  UE_LOG(LogCarlaServer, Log, TEXT("New episode '%s' started"), *Episode.GetMapName());
2643  Pimpl->Episode = &Episode;
2644 }
2645 
2647 {
2648  check(Pimpl != nullptr);
2649  Pimpl->Episode = nullptr;
2650 }
2651 
2652 void FCarlaServer::AsyncRun(uint32 NumberOfWorkerThreads)
2653 {
2654  check(Pimpl != nullptr);
2655  /// @todo Define better the number of threads each server gets.
2656  int ThreadsPerServer = std::max(2u, NumberOfWorkerThreads / 3u);
2657  int32_t RPCThreads;
2658  int32_t StreamingThreads;
2659  int32_t SecondaryThreads;
2660 
2661  UE_LOG(LogCarla, Log, TEXT("FCommandLine %s"), FCommandLine::Get());
2662 
2663  if(!FParse::Value(FCommandLine::Get(), TEXT("-RPCThreads="), RPCThreads))
2664  {
2665  RPCThreads = ThreadsPerServer;
2666  }
2667  if(!FParse::Value(FCommandLine::Get(), TEXT("-StreamingThreads="), StreamingThreads))
2668  {
2669  StreamingThreads = ThreadsPerServer;
2670  }
2671  if(!FParse::Value(FCommandLine::Get(), TEXT("-SecondaryThreads="), SecondaryThreads))
2672  {
2673  SecondaryThreads = ThreadsPerServer;
2674  }
2675 
2676  UE_LOG(LogCarla, Log, TEXT("FCarlaServer AsyncRun %d, RPCThreads %d, StreamingThreads %d, SecondaryThreads %d"),
2677  NumberOfWorkerThreads, RPCThreads, StreamingThreads, SecondaryThreads);
2678 
2679  Pimpl->Server.AsyncRun(RPCThreads);
2680  Pimpl->StreamingServer.AsyncRun(StreamingThreads);
2681  Pimpl->SecondaryServer->AsyncRun(SecondaryThreads);
2682 }
2683 
2684 void FCarlaServer::RunSome(uint32 Milliseconds)
2685 {
2686  TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
2687  Pimpl->Server.SyncRunFor(carla::time_duration::milliseconds(Milliseconds));
2688 }
2689 
2691 {
2692  (void)Pimpl->TickCuesReceived.fetch_add(1, std::memory_order_release);
2693 }
2694 
2696 {
2697  auto k = Pimpl->TickCuesReceived.fetch_sub(1, std::memory_order_acquire);
2698  bool flag = (k > 0);
2699  if (!flag)
2700  (void)Pimpl->TickCuesReceived.fetch_add(1, std::memory_order_release);
2701  return flag;
2702 }
2703 
2705 {
2706  if (Pimpl)
2707  {
2708  Pimpl->Server.Stop();
2709  Pimpl->SecondaryServer->Stop();
2710  }
2711 }
2712 
2714 {
2715  check(Pimpl != nullptr);
2716  return Pimpl->StreamingServer.MakeStream();
2717 }
2718 
2719 std::shared_ptr<carla::multigpu::Router> FCarlaServer::GetSecondaryServer()
2720 {
2721  return Pimpl->GetSecondaryServer();
2722 }
2723 
2725 {
2726  return Pimpl->StreamingServer;
2727 }
carla::streaming::Stream BroadcastStream
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
Definition: CarlaEpisode.h:173
carla::streaming::Server & GetStreamingServer()
virtual ECarlaServerResponse SetVehicleLightState(const FVehicleLightState &)
Definition: CarlaActor.h:273
TArray< FEnvironmentObject > GetEnvironmentObjects(uint8 QueriedTag=0xFF) const
void ApplyTextureToActor(AActor *Actor, UTexture2D *Texture, const carla::rpc::MaterialParameter &TextureParam)
FDataStream OpenStream() const
FCameraGBufferUint8 GBufferF
auto end() const noexcept
std::vector< carla::rpc::LightState > GetLights(FString Client)
FPimpl(uint16_t RPCPort, uint16_t StreamingPort, uint16_t SecondaryPort)
Definition: CarlaServer.cpp:98
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)
void DisableForROS(stream_id sensor_id)
bool DestroyActor(AActor *Actor)
Definition: CarlaEpisode.h:252
const char * _name
FCameraGBufferUint8 SceneDepth
virtual ECarlaServerResponse SetWheelSteerDirection(const EVehicleWheelLocation &, float)
Definition: CarlaActor.h:278
AWeather * GetWeather() const
Definition: CarlaEpisode.h:150
std::string ShowFileCollisions(std::string Name, char Type1, char Type2)
void SetDayNightCycle(const bool active)
void SetMapLayer(int32 MapLayer)
auto operator<<(FuncT func)
void ConsiderSpectatorAsEgo(bool _SpectatorAsEgo)
virtual ECarlaServerResponse SetActorEnableGravity(bool bEnabled)
Definition: CarlaActor.cpp:594
AActor * GetActor()
Definition: CarlaActor.h:90
FVector GlobalToLocalLocation(const FVector &InLocation) const
virtual ECarlaServerResponse SetActorCollisions(bool bEnabled)
Definition: CarlaActor.cpp:582
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:403
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)
FCameraGBufferUint8 CustomStencil
rpc::ActorId ActorId
Definition: ActorId.h:18
FCameraGBufferUint8 SceneStencil
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:238
FDataMultiStream Start(uint16_t RPCPort, uint16_t StreamingPort, uint16_t SecondaryPort)
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.
#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.
auto GetToken() const
Return the token that allows subscribing to this sensor&#39;s stream.
void EnableEnvironmentObjects(const TSet< uint64 > &EnvObjectIds, bool Enable)
static TArray< FString > GetAllMapNames()
FCameraGBufferUint8 GBufferE
virtual ECarlaServerResponse BlendPose(float Blend)
Definition: CarlaActor.h:408
void PutActorToSleep(carla::rpc::ActorId ActorId)
Definition: CarlaEpisode.h:280
virtual ECarlaServerResponse GetPhysicsControl(FVehiclePhysicsControl &)
Definition: CarlaActor.h:243
virtual ECarlaServerResponse ApplyAckermannControllerSettings(const FAckermannControllerSettings &)
Definition: CarlaActor.h:315
void UnLoadMapLayer(int32 MapLayers)
void OnActorSpawned(const FCarlaActor &CarlaActor)
bool IsEnabledForROS(stream_id sensor_id)
uint32_t stream_id_type
Definition: Types.h:18
virtual ECarlaServerResponse ApplyControlToVehicle(const FVehicleControl &, const EVehicleInputPriority &)
Definition: CarlaActor.h:288
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:52
virtual ECarlaServerResponse SetTrafficLightState(const ETrafficLightState &)
Definition: CarlaActor.h:348
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:93
std::atomic_size_t TickCuesReceived
static uint64_t GetFrameCounter()
Definition: CarlaEngine.h:65
#define BIND_ASYNC(name)
token_type GetToken(stream_id sensor_id)
virtual ECarlaServerResponse EnableChronoPhysics(uint64_t, float, const FString &, const FString &, const FString &, const FString &)
Definition: CarlaActor.h:340
geom::Location Location
Definition: rpc/Location.h:14
void SetActorGlobalLocation(const FVector &Location, ETeleportType Teleport=ETeleportType::TeleportPhysics)
Definition: CarlaActor.cpp:282
void Draw(const carla::rpc::DebugShape &Shape)
ECarlaServerResponse AddActorImpulse(const FVector &Impulse)
Definition: CarlaActor.cpp:432
#define BIND_SYNC(name)
ECarlaServerResponse SetActorTargetAngularVelocity(const FVector &AngularVelocity)
Definition: CarlaActor.cpp:411
void LoadMapLayer(int32 MapLayers)
void RunSome(uint32 Milliseconds)
const FActorRegistry & GetActorRegistry() const
Definition: CarlaEpisode.h:155
virtual ECarlaServerResponse CloseVehicleDoor(const EVehicleDoor)
Definition: CarlaActor.h:263
TMap< FString, FActorAttribute > Variations
User selected variations of the actor.
std::string StartRecorder(std::string name, bool AdditionalData)
void EnableForROS(stream_id sensor_id)
ECarlaServerResponse AddActorForceAtLocation(const FVector &Force, const FVector &Location)
Definition: CarlaActor.cpp:496
ECarlaServerResponse AddActorTorque(const FVector &Torque)
Definition: CarlaActor.cpp:540
static UCarlaGameInstance * GetGameInstance(const UObject *WorldContextObject)
Definition: CarlaStatics.h:63
ECarlaServerResponse
carla::SharedPtr< cc::Actor > Actor
virtual ECarlaServerResponse FreezeTrafficLight(bool)
Definition: CarlaActor.h:423
virtual ECarlaServerResponse EnableActorConstantVelocity(const FVector &)
Definition: CarlaActor.h:233
A simulation episode.
Definition: CarlaEpisode.h:38
std::shared_ptr< carla::multigpu::Router > SecondaryServer
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:413
FCameraGBufferUint8 GBufferD
std::string ReplayFile(std::string Name, double TimeStart, double Duration, uint32_t FollowId, bool ReplaySensors)
FCameraGBufferUint8 SSAO
TPair< EActorSpawnResultStatus, FCarlaActor * > SpawnActorWithInfo(const FTransform &Transform, FActorDescription thisActorDescription, FCarlaActor::IdType DesiredId=0)
Spawns an actor based on ActorDescription at Transform.
FCameraGBufferUint8 GBufferA
void SetActorState(carla::rpc::ActorState InState)
Definition: CarlaActor.h:110
Response< ActorId > CommandResponse
virtual ECarlaServerResponse ResetTrafficLightGroup()
Definition: CarlaActor.h:428
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:124
std::shared_ptr< carla::multigpu::Router > GetSecondaryServer()
virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControlOut &)
Definition: CarlaActor.h:398
#define RESPOND_ERROR(str)
virtual ECarlaServerResponse ApplyControlToWalker(const FWalkerControl &)
Definition: CarlaActor.h:388
static std::vector< T > MakeVectorFromTArray(const TArray< Other > &Array)
Definition: CarlaServer.cpp:85
Token token() const
Token associated with this stream.
Definition: detail/Stream.h:35
FCameraGBufferUint8 SceneColor
auto GetId() const
Return the unique id of this episode.
Definition: CarlaEpisode.h:86
std::shared_ptr< carla::multigpu::Router > GetSecondaryServer()
void SetLights(FString Client, std::vector< carla::rpc::LightState > LightsToSet, bool DiscardClient=false)
FActorDescription Description
Definition: ActorInfo.h:26
void SetReplayerIgnoreHero(bool IgnoreHero)
constexpr size_t milliseconds() const noexcept
Definition: Time.h:58
virtual ECarlaServerResponse SetLightGreenTime(float)
Definition: CarlaActor.h:363
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
const FActorInfo * GetActorInfo() const
Definition: CarlaActor.h:100
FCameraGBufferUint8 CustomDepth
bool TickCueReceived()
ACarlaRecorder * GetRecorder() const
Definition: CarlaEpisode.h:305
virtual ECarlaServerResponse SetLightYellowTime(float)
Definition: CarlaActor.h:368
carla::rpc::Server Server
virtual ECarlaServerResponse SetActorDead()
Definition: CarlaActor.h:418
virtual ECarlaServerResponse SetLightRedTime(float)
Definition: CarlaActor.h:373
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
Base class for sensors using a USceneCaptureComponent2D for rendering the scene.
virtual ECarlaServerResponse SetActorAutopilot(bool, bool bKeepState=false)
Definition: CarlaActor.h:320
FString GetActorDescriptionFromStream(carla::streaming::detail::stream_id_type StreamId)
Get the description of the Carla actor (sensor) using specific stream id.
Definition: CarlaEpisode.h:190
carla::rpc::Server & _server
ECarlaServerResponse SetActorTargetVelocity(const FVector &Velocity)
Definition: CarlaActor.cpp:390
void SetReplayerIgnoreSpectator(bool IgnoreSpectator)
virtual ECarlaServerResponse OpenVehicleDoor(const EVehicleDoor)
Definition: CarlaActor.h:258
virtual ECarlaServerResponse EnableCarSim(const FString &)
Definition: CarlaActor.h:330
FTransform LocalToGlobalTransform(const FTransform &InTransform) const
TUniquePtr< FPimpl > Pimpl
Definition: CarlaServer.h:53
FString CarlaGetStringError(ECarlaServerResponse Response)
APawn * GetSpectatorPawn() const
Definition: CarlaEpisode.h:144
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:335
static std::pair< bool, carla::rpc::LabelledPoint > ProjectPoint(FVector StartLocation, FVector Direction, float MaxDistance, UWorld *World)
Definition: RayTracer.cpp:46
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:452
TArray< FBoundingBox > GetAllBBsOfLevel(uint8 TagQueried=0xFF) const
FCameraGBufferUint8 GBufferC
FCameraGBufferUint8 Velocity
bool IsDormant() const
Definition: CarlaActor.h:70
Base class for the CARLA Game Mode.
virtual ECarlaServerResponse SetActorSimulatePhysics(bool bEnabled)
Definition: CarlaActor.cpp:560
virtual ECarlaServerResponse GetVehicleLightState(FVehicleLightState &)
Definition: CarlaActor.h:253
const FEpisodeSettings & GetSettings() const
Definition: CarlaEpisode.h:73
void SetParent(IdType InParentId)
Definition: CarlaActor.h:115
static std::shared_ptr< ROS2 > GetInstance()
Definition: ROS2.h:51
IdType GetActorId() const
Definition: CarlaActor.h:80
virtual ECarlaServerResponse ApplyAckermannControlToVehicle(const FVehicleAckermannControl &, const EVehicleInputPriority &)
Definition: CarlaActor.h:294
ECarlaServerResponse AddActorForce(const FVector &Force)
Definition: CarlaActor.cpp:476
UCarlaEpisode * Episode
Texture< sensor::data::Color > TextureColor
Definition: Texture.h:66
virtual ECarlaServerResponse ApplyPhysicsControl(const FVehiclePhysicsControl &)
Definition: CarlaActor.h:268
const FString GetFullMapPath() const
Base class for CARLA wheeled vehicles.
virtual ECarlaServerResponse GetWheelSteerAngle(const EVehicleWheelLocation &, float &)
Definition: CarlaActor.h:283
struct ASceneCaptureSensor::@0 CameraGBuffers
Texture< FloatColor > TextureFloatColor
Definition: Texture.h:67
void AsyncRun(uint32 NumberOfWorkerThreads)
virtual ECarlaServerResponse GetAckermannControllerSettings(FAckermannControllerSettings &)
Definition: CarlaActor.h:310
void SetActorGlobalTransform(const FTransform &Transform, ETeleportType Teleport=ETeleportType::TeleportPhysics)
Definition: CarlaActor.cpp:333
FCameraGBufferUint8 GBufferB
ECarlaServerResponse AddActorAngularImpulse(const FVector &AngularInpulse)
Definition: CarlaActor.cpp:520
#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:325
virtual ECarlaServerResponse SetWalkerState(const FTransform &Transform, carla::rpc::WalkerControl WalkerControl)
Definition: CarlaActor.h:381
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)
IdType GetParent() const
Definition: CarlaActor.h:120
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)