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