10 #include "EngineUtils.h" 19 #include "GameFramework/CharacterMovementComponent.h" 27 #include "Misc/FileHelper.h" 31 #include <carla/Version.h> 78 template <
typename T,
typename Other>
81 return {Array.GetData(), Array.GetData() + Array.Num()};
92 FPimpl(uint16_t RPCPort, uint16_t StreamingPort)
123 # define CARLA_ENSURE_GAME_THREAD() check(IsInGameThread()); 125 # define CARLA_ENSURE_GAME_THREAD() 126 #endif // WITH_EDITOR 128 #define RESPOND_ERROR(str) { \ 129 UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), TEXT(str)); \ 130 return carla::rpc::ResponseError(str); } 132 #define RESPOND_ERROR_FSTRING(fstr) { \ 133 UE_LOG(LogCarlaServer, Log, TEXT("Responding error: %s"), *fstr); \ 134 return carla::rpc::ResponseError(carla::rpc::FromFString(fstr)); } 136 #define REQUIRE_CARLA_EPISODE() \ 137 CARLA_ENSURE_GAME_THREAD(); \ 138 if (Episode == nullptr) { RESPOND_ERROR("episode not ready"); } 141 const FString& FuncName,
142 const FString& ErrorMessage,
143 const FString& ExtraInfo =
"")
145 FString TotalMessage =
"Responding error from function " + FuncName +
": " +
146 ErrorMessage +
". " + ExtraInfo;
147 UE_LOG(LogCarlaServer, Log, TEXT(
"%s"), *TotalMessage);
152 const FString& FuncName,
154 const FString& ExtraInfo =
"")
168 template <
typename FuncT>
173 _server.BindSync(_name, func);
177 _server.BindAsync(_name, func);
191 #define BIND_SYNC(name) auto name = ServerBinder(# name, Server, true) 192 #define BIND_ASYNC(name) auto name = ServerBinder(# name, Server, false) 211 BIND_SYNC(get_traffic_manager_running) << [
this] (uint16_t port) ->
R<std::pair<std::string, uint16_t>>
215 return std::pair<std::string, uint16_t>(it->second, it->first);
217 return std::pair<std::string, uint16_t>(
"",0);
221 BIND_SYNC(add_traffic_manager_running) << [
this] (std::pair<std::string, uint16_t> trafficManagerInfo) ->
R<bool> 223 uint16_t port = trafficManagerInfo.second;
227 std::pair<uint16_t, std::string>(port, trafficManagerInfo.first));
246 return carla::version();
260 BIND_ASYNC(get_available_maps) << [
this]() ->
R<std::vector<std::string>>
263 std::vector<std::string> result;
264 result.reserve(MapNames.Num());
265 for (
const auto &MapName : MapNames)
267 if (MapName.Contains(
"/Sublevels/"))
269 if (MapName.Contains(
"/BaseMap/"))
271 if (MapName.Contains(
"/BaseLargeMap/"))
273 if (MapName.Contains(
"_Tile_"))
276 result.emplace_back(cr::FromFString(MapName));
290 GameInstance->
SetMapLayer(static_cast<int32>(MapLayers));
328 BIND_SYNC(copy_opendrive_to_file) << [
this](
const std::string &opendrive, cr::OpendriveGenerationParameters Params) ->
R<void> 338 BIND_SYNC(apply_color_texture_to_objects) << [
this](
339 const std::vector<std::string> &actors_name,
349 TArray<AActor*> ActorsToPaint;
350 for(
const std::string& actor_name : actors_name)
355 ActorsToPaint.Add(ActorToPaint);
359 if(!ActorsToPaint.Num())
366 for(
AActor* ActorToPaint : ActorsToPaint)
376 BIND_SYNC(apply_float_color_texture_to_objects) << [
this](
377 const std::vector<std::string> &actors_name,
387 TArray<AActor*> ActorsToPaint;
388 for(
const std::string& actor_name : actors_name)
393 ActorsToPaint.Add(ActorToPaint);
397 if(!ActorsToPaint.Num())
404 for(
AActor* ActorToPaint : ActorsToPaint)
414 BIND_SYNC(get_names_of_all_objects) << [
this]() ->
R<std::vector<std::string>>
423 std::vector<std::string> NamesStd;
424 for (
const FString &Name : NamesFString)
426 NamesStd.emplace_back(cr::FromFString(Name));
445 FString MapDir = FullMapPath.RightChop(FullMapPath.Find(
"Content/", ESearchCase::CaseSensitive) + 8);
448 cr::FromFString(MapDir),
449 MakeVectorFromTArray<cg::Transform>(SpawnPoints)};
458 BIND_SYNC(get_navigation_mesh) << [
this]() ->
R<std::vector<uint8_t>>
463 std::vector<uint8_t> Result(FileContents.Num());
464 memcpy(&Result[0], FileContents.GetData(), FileContents.Num());
468 BIND_SYNC(get_required_files) << [
this](std::string folder =
"") ->
R<std::vector<std::string>>
473 if (folder[folder.size() - 1] !=
'/' && folder[folder.size() - 1] !=
'\\') {
480 const auto folderDir = mapDir +
"/" + folder.c_str();
484 TArray<FString> Files;
485 IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName +
".xodr"),
true,
false,
false);
486 IFileManager::Get().FindFilesRecursive(Files, *folderDir, *(fileName +
".bin"),
true,
false,
false);
489 std::vector<std::string> result;
490 for (
auto File : Files) {
491 File.RemoveFromStart(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
492 result.emplace_back(TCHAR_TO_UTF8(*File));
497 BIND_SYNC(request_file) << [
this](std::string name) ->
R<std::vector<uint8_t>>
502 FString path(FPaths::ConvertRelativePathToFull(FPaths::ProjectContentDir()));
503 path.Append(name.c_str());
506 TArray<uint8_t> Content;
507 FFileHelper::LoadFileToArray(Content, *path, 0);
508 std::vector<uint8_t> Result(Content.Num());
509 memcpy(&Result[0], Content.GetData(), Content.Num());
520 BIND_SYNC(set_episode_settings) << [
this](
521 const cr::EpisodeSettings &settings) ->
R<uint64_t> 529 BIND_SYNC(get_actor_definitions) << [
this]() ->
R<std::vector<cr::ActorDefinition>>
546 BIND_SYNC(get_all_level_BBs) << [
this](uint8 QueriedTag) ->
R<std::vector<cg::BoundingBox>>
549 TArray<FBoundingBox> Result;
559 for(
auto&
Box : Result)
564 return MakeVectorFromTArray<cg::BoundingBox>(Result);
567 BIND_SYNC(get_environment_objects) << [
this](uint8 QueriedTag) ->
R<std::vector<cr::EnvironmentObject>>
579 for(
auto& Object : Result)
584 return MakeVectorFromTArray<cr::EnvironmentObject>(Result);
587 BIND_SYNC(enable_environment_objects) << [
this](std::vector<uint64_t> EnvObjectIds,
bool Enable) ->
R<void> 596 TSet<uint64> EnvObjectIdsSet;
597 for(uint64 Id : EnvObjectIds)
599 EnvObjectIdsSet.Emplace(Id);
612 if (Weather ==
nullptr)
616 return Weather->GetCurrentWeather();
619 BIND_SYNC(set_weather_parameters) << [
this](
620 const cr::WeatherParameters &weather) ->
R<void> 624 if (Weather ==
nullptr)
628 Weather->ApplyWeather(weather);
635 const std::vector<FCarlaActor::IdType> &ids) ->
R<std::vector<cr::Actor>>
638 std::vector<cr::Actor> Result;
639 Result.reserve(ids.size());
640 for (
auto &&Id : ids)
652 cr::ActorDescription Description,
659 if (Result.Key != EActorSpawnResultStatus::Success)
661 UE_LOG(LogCarla, Error, TEXT(
"Actor not Spawned"));
674 BIND_SYNC(spawn_actor_with_parent) << [
this](
675 cr::ActorDescription Description,
683 if (Result.Key != EActorSpawnResultStatus::Success)
696 if (!ParentCarlaActor)
698 RESPOND_ERROR(
"unable to attach actor: parent actor not found");
730 UE_LOG(LogCarla, Log, TEXT(
"CarlaServer destroy_actor %d"),
ActorId);
752 "set_actor_location",
754 " Actor Id: " + FString::FromInt(ActorId));
758 Location, ETeleportType::TeleportPhysics);
762 BIND_SYNC(set_actor_transform) << [
this](
771 "set_actor_transform",
773 " Actor Id: " + FString::FromInt(ActorId));
777 Transform, ETeleportType::TeleportPhysics);
793 " Actor Id: " + FString::FromInt(ActorId));
801 Transform.GetForwardVector(), Speed,
false));
807 " Actor Id: " + FString::FromInt(ActorId));
812 BIND_SYNC(set_actor_target_velocity) << [
this](
821 "set_actor_target_velocity",
823 " Actor Id: " + FString::FromInt(ActorId));
830 "set_actor_target_velocity",
832 " Actor Id: " + FString::FromInt(ActorId));
837 BIND_SYNC(set_actor_target_angular_velocity) << [
this](
846 "set_actor_target_angular_velocity",
848 " Actor Id: " + FString::FromInt(ActorId));
855 "set_actor_target_angular_velocity",
857 " Actor Id: " + FString::FromInt(ActorId));
862 BIND_SYNC(enable_actor_constant_velocity) << [
this](
871 "enable_actor_constant_velocity",
873 " Actor Id: " + FString::FromInt(ActorId));
881 "enable_actor_constant_velocity",
883 " Actor Id: " + FString::FromInt(ActorId));
889 BIND_SYNC(disable_actor_constant_velocity) << [
this](
897 "disable_actor_constant_velocity",
899 " Actor Id: " + FString::FromInt(ActorId));
907 "disable_actor_constant_velocity",
909 " Actor Id: " + FString::FromInt(ActorId));
926 " Actor Id: " + FString::FromInt(ActorId));
936 " Actor Id: " + FString::FromInt(ActorId));
941 BIND_SYNC(add_actor_impulse_at_location) << [
this](
951 "add_actor_impulse_at_location",
953 " Actor Id: " + FString::FromInt(ActorId));
955 FVector UELocation = location.ToCentimeters().ToFVector();
967 "add_actor_impulse_at_location",
969 " Actor Id: " + FString::FromInt(ActorId));
986 " Actor Id: " + FString::FromInt(ActorId));
989 CarlaActor->
AddActorForce(vector.ToCentimeters().ToFVector());
995 " Actor Id: " + FString::FromInt(ActorId));
1000 BIND_SYNC(add_actor_force_at_location) << [
this](
1010 "add_actor_force_at_location",
1012 " Actor Id: " + FString::FromInt(ActorId));
1014 FVector UELocation = location.ToCentimeters().ToFVector();
1026 "add_actor_force_at_location",
1028 " Actor Id: " + FString::FromInt(ActorId));
1033 BIND_SYNC(add_actor_angular_impulse) << [
this](
1042 "add_actor_angular_impulse",
1044 " Actor Id: " + FString::FromInt(ActorId));
1051 "add_actor_angular_impulse",
1053 " Actor Id: " + FString::FromInt(ActorId));
1069 " Actor Id: " + FString::FromInt(ActorId));
1078 " Actor Id: " + FString::FromInt(ActorId));
1083 BIND_SYNC(get_physics_control) << [
this](
1091 "get_physics_control",
1093 " Actor Id: " + FString::FromInt(ActorId));
1101 "get_physics_control",
1103 " Actor Id: " + FString::FromInt(ActorId));
1105 return cr::VehiclePhysicsControl(PhysicsControl);
1108 BIND_SYNC(get_vehicle_light_state) << [
this](
1116 "get_vehicle_light_state",
1118 " Actor Id: " + FString::FromInt(ActorId));
1126 "get_vehicle_light_state",
1128 " Actor Id: " + FString::FromInt(ActorId));
1130 return cr::VehicleLightState(LightState);
1133 BIND_SYNC(apply_physics_control) << [
this](
1142 "apply_physics_control",
1144 " Actor Id: " + FString::FromInt(ActorId));
1151 "apply_physics_control",
1153 " Actor Id: " + FString::FromInt(ActorId));
1158 BIND_SYNC(set_vehicle_light_state) << [
this](
1160 cr::VehicleLightState LightState) ->
R<void> 1167 "set_vehicle_light_state",
1169 " Actor Id: " + FString::FromInt(ActorId));
1176 "set_vehicle_light_state",
1178 " Actor Id: " + FString::FromInt(ActorId));
1193 "open_vehicle_door",
1195 " Actor Id: " + FString::FromInt(ActorId));
1202 "open_vehicle_door",
1204 " Actor Id: " + FString::FromInt(ActorId));
1209 BIND_SYNC(close_vehicle_door) << [
this](
1218 "close_vehicle_door",
1220 " Actor Id: " + FString::FromInt(ActorId));
1227 "close_vehicle_door",
1229 " Actor Id: " + FString::FromInt(ActorId));
1234 BIND_SYNC(set_wheel_steer_direction) << [
this](
1243 "set_wheel_steer_direction",
1245 " Actor Id: " + FString::FromInt(ActorId));
1249 static_cast<EVehicleWheelLocation>(WheelLocation), AngleInDeg);
1253 "set_wheel_steer_direction",
1255 " Actor Id: " + FString::FromInt(ActorId));
1260 BIND_SYNC(get_wheel_steer_angle) << [
this](
1268 "get_wheel_steer_angle",
1270 " Actor Id: " + FString::FromInt(ActorId));
1275 static_cast<EVehicleWheelLocation>(WheelLocation), Angle);
1279 "get_wheel_steer_angle",
1281 " Actor Id: " + FString::FromInt(ActorId));
1286 BIND_SYNC(set_actor_simulate_physics) << [
this](
1295 "set_actor_simulate_physics",
1297 " Actor Id: " + FString::FromInt(ActorId));
1304 "set_actor_simulate_physics",
1306 " Actor Id: " + FString::FromInt(ActorId));
1311 BIND_SYNC(set_actor_enable_gravity) << [
this](
1320 "set_actor_enable_gravity",
1322 " Actor Id: " + FString::FromInt(ActorId));
1329 "set_actor_enable_gravity",
1331 " Actor Id: " + FString::FromInt(ActorId));
1338 BIND_SYNC(apply_control_to_vehicle) << [
this](
1340 cr::VehicleControl Control) ->
R<void> 1347 "apply_control_to_vehicle",
1349 " Actor Id: " + FString::FromInt(ActorId));
1356 "apply_control_to_vehicle",
1358 " Actor Id: " + FString::FromInt(ActorId));
1363 BIND_SYNC(apply_control_to_walker) << [
this](
1365 cr::WalkerControl Control) ->
R<void> 1372 "apply_control_to_walker",
1374 " Actor Id: " + FString::FromInt(ActorId));
1381 "apply_control_to_walker",
1383 " Actor Id: " + FString::FromInt(ActorId));
1388 BIND_SYNC(get_bones_transform) << [
this](
1396 "get_bones_transform",
1398 " Actor Id: " + FString::FromInt(ActorId));
1406 "get_bones_transform",
1408 " Actor Id: " + FString::FromInt(ActorId));
1411 std::vector<carla::rpc::BoneTransformDataOut> BoneData;
1415 Data.
bone_name = std::string(TCHAR_TO_UTF8(*Bone.Get<0>()));
1420 BoneData.push_back(Data);
1425 BIND_SYNC(set_bones_transform) << [
this](
1434 "set_bones_transform",
1436 " Actor Id: " + FString::FromInt(ActorId));
1444 "set_bones_transform",
1446 " Actor Id: " + FString::FromInt(ActorId));
1463 " Actor Id: " + FString::FromInt(ActorId));
1472 " Actor Id: " + FString::FromInt(ActorId));
1478 BIND_SYNC(get_pose_from_animation) << [
this](
1486 "get_pose_from_animation",
1488 " Actor Id: " + FString::FromInt(ActorId));
1495 "get_pose_from_animation",
1497 " Actor Id: " + FString::FromInt(ActorId));
1503 BIND_SYNC(set_actor_autopilot) << [
this](
1512 "set_actor_autopilot",
1514 " Actor Id: " + FString::FromInt(ActorId));
1521 "set_actor_autopilot",
1523 " Actor Id: " + FString::FromInt(ActorId));
1528 BIND_SYNC(show_vehicle_debug_telemetry) << [
this](
1537 "show_vehicle_debug_telemetry",
1539 " Actor Id: " + FString::FromInt(ActorId));
1546 "show_vehicle_debug_telemetry",
1548 " Actor Id: " + FString::FromInt(ActorId));
1555 std::string SimfilePath) ->
R<void> 1564 " Actor Id: " + FString::FromInt(ActorId));
1567 CarlaActor->
EnableCarSim(carla::rpc::ToFString(SimfilePath));
1573 " Actor Id: " + FString::FromInt(ActorId));
1589 " Actor Id: " + FString::FromInt(ActorId));
1598 " Actor Id: " + FString::FromInt(ActorId));
1603 BIND_SYNC(enable_chrono_physics) << [
this](
1605 uint64_t MaxSubsteps,
1606 float MaxSubstepDeltaTime,
1607 std::string VehicleJSON,
1608 std::string PowertrainJSON,
1609 std::string TireJSON,
1610 std::string BaseJSONPath) ->
R<void> 1617 "enable_chrono_physics",
1619 " Actor Id: " + FString::FromInt(ActorId));
1623 MaxSubsteps, MaxSubstepDeltaTime,
1624 cr::ToFString(VehicleJSON),
1625 cr::ToFString(PowertrainJSON),
1626 cr::ToFString(TireJSON),
1627 cr::ToFString(BaseJSONPath));
1631 "enable_chrono_physics",
1633 " Actor Id: " + FString::FromInt(ActorId));
1640 BIND_SYNC(set_traffic_light_state) << [
this](
1649 "set_traffic_light_state",
1651 " Actor Id: " + FString::FromInt(ActorId));
1655 static_cast<ETrafficLightState>(trafficLightState));
1659 "set_traffic_light_state",
1661 " Actor Id: " + FString::FromInt(ActorId));
1666 BIND_SYNC(set_traffic_light_green_time) << [
this](
1675 "set_traffic_light_green_time",
1677 " Actor Id: " + FString::FromInt(ActorId));
1684 "set_traffic_light_green_time",
1686 " Actor Id: " + FString::FromInt(ActorId));
1691 BIND_SYNC(set_traffic_light_yellow_time) << [
this](
1700 "set_traffic_light_yellow_time",
1702 " Actor Id: " + FString::FromInt(ActorId));
1709 "set_traffic_light_yellow_time",
1711 " Actor Id: " + FString::FromInt(ActorId));
1716 BIND_SYNC(set_traffic_light_red_time) << [
this](
1725 "set_traffic_light_red_time",
1727 " Actor Id: " + FString::FromInt(ActorId));
1734 "set_traffic_light_red_time",
1736 " Actor Id: " + FString::FromInt(ActorId));
1741 BIND_SYNC(freeze_traffic_light) << [
this](
1750 "freeze_traffic_light",
1752 " Actor Id: " + FString::FromInt(ActorId));
1759 "freeze_traffic_light",
1761 " Actor Id: " + FString::FromInt(ActorId));
1766 BIND_SYNC(reset_traffic_light_group) << [
this](
1774 "reset_traffic_light_group",
1776 " Actor Id: " + FString::FromInt(ActorId));
1783 "reset_traffic_light_group",
1785 " Actor Id: " + FString::FromInt(ActorId));
1793 for (TActorIterator<ATrafficLightGroup> It(
Episode->GetWorld()); It; ++It)
1800 BIND_SYNC(freeze_all_traffic_lights) << [
this]
1809 auto* TraffiLightManager = GameMode->GetTrafficLightManager();
1810 TraffiLightManager->SetFrozen(frozen);
1832 if (!
Actor->IsPendingKill())
1845 BIND_SYNC(get_group_traffic_lights) << [
this](
1852 RESPOND_ERROR(
"unable to get group traffic lights: actor not found");
1857 return std::vector<cr::ActorId>();
1864 RESPOND_ERROR(
"unable to get group traffic lights: actor is not a traffic light");
1866 std::vector<cr::ActorId> Result;
1867 for (
auto* TLight :
TrafficLight->GetGroupTrafficLights())
1872 Result.push_back(View->GetActorId());
1889 " Actor Id: " + FString::FromInt(ActorId));
1896 " Actor Id: " + FString::FromInt(ActorId));
1906 " Actor Id: " + FString::FromInt(ActorId));
1908 TArray<FBoundingBox> Result;
1909 TArray<uint8> OutTag;
1911 TrafficLight, Result, OutTag,
1913 return MakeVectorFromTArray<cg::BoundingBox>(Result);
1932 BIND_SYNC(show_recorder_file_info) << [
this](
1942 BIND_SYNC(show_recorder_collisions) << [
this](
1954 BIND_SYNC(show_recorder_actors_blocked) << [
this](
1982 BIND_SYNC(set_replayer_time_factor) << [
this](
double time_factor) ->
R<void> 2005 BIND_SYNC(draw_debug_shape) << [
this](
const cr::DebugShape &shape) ->
R<void> 2008 auto *World =
Episode->GetWorld();
2009 check(World !=
nullptr);
2017 using C = cr::Command;
2021 auto parse_result = [](ActorId id,
const auto &response) {
2022 return response.HasError() ? CR{response.GetError()} : CR{
id};
2025 #define MAKE_RESULT(operation) return parse_result(c.actor, operation); 2028 [=](
auto self,
const C::SpawnActor &c) -> CR {
2029 auto result = c.parent.has_value() ?
2030 spawn_actor_with_parent(
2034 cr::AttachmentType::Rigid) :
2035 spawn_actor(c.description, c.transform);
2036 if (!result.HasError())
2038 ActorId
id = result.Get().id;
2040 [](C::SpawnActor &) {},
2041 [id](
auto &s) { s.actor = id; });
2042 for (
auto command : c.do_after)
2044 boost::apply_visitor(set_id, command.command);
2045 boost::apply_visitor(
self, command.command);
2049 return result.GetError();
2051 [=](
auto,
const C::DestroyActor &c) {
MAKE_RESULT(destroy_actor(c.actor)); },
2052 [=](
auto,
const C::ApplyVehicleControl &c) {
MAKE_RESULT(apply_control_to_vehicle(c.actor, c.control)); },
2053 [=](
auto,
const C::ApplyWalkerControl &c) {
MAKE_RESULT(apply_control_to_walker(c.actor, c.control)); },
2054 [=](
auto,
const C::ApplyVehiclePhysicsControl &c) {
MAKE_RESULT(apply_physics_control(c.actor, c.physics_control)); },
2055 [=](
auto,
const C::ApplyTransform &c) {
MAKE_RESULT(set_actor_transform(c.actor, c.transform)); },
2056 [=](
auto,
const C::ApplyTargetVelocity &c) {
MAKE_RESULT(set_actor_target_velocity(c.actor, c.velocity)); },
2057 [=](
auto,
const C::ApplyTargetAngularVelocity &c) {
MAKE_RESULT(set_actor_target_angular_velocity(c.actor, c.angular_velocity)); },
2058 [=](
auto,
const C::ApplyImpulse &c) {
MAKE_RESULT(add_actor_impulse(c.actor, c.impulse)); },
2059 [=](
auto,
const C::ApplyForce &c) {
MAKE_RESULT(add_actor_force(c.actor, c.force)); },
2060 [=](
auto,
const C::ApplyAngularImpulse &c) {
MAKE_RESULT(add_actor_angular_impulse(c.actor, c.impulse)); },
2061 [=](
auto,
const C::ApplyTorque &c) {
MAKE_RESULT(add_actor_torque(c.actor, c.torque)); },
2062 [=](
auto,
const C::SetSimulatePhysics &c) {
MAKE_RESULT(set_actor_simulate_physics(c.actor, c.enabled)); },
2063 [=](
auto,
const C::SetEnableGravity &c) {
MAKE_RESULT(set_actor_enable_gravity(c.actor, c.enabled)); },
2065 [=](
auto,
const C::SetAutopilot &c) {
MAKE_RESULT(set_actor_autopilot(c.actor, c.enabled)); },
2066 [=](
auto,
const C::ShowDebugTelemetry &c) {
MAKE_RESULT(show_vehicle_debug_telemetry(c.actor, c.enabled)); },
2067 [=](
auto,
const C::SetVehicleLightState &c) {
MAKE_RESULT(set_vehicle_light_state(c.actor, c.light_state)); },
2070 [=](
auto,
const C::ApplyWalkerState &c) {
MAKE_RESULT(set_walker_state(c.actor, c.transform, c.speed)); });
2075 const std::vector<cr::Command> &commands,
2078 std::vector<CR> result;
2079 result.reserve(commands.size());
2080 for (
const auto &command : commands)
2082 result.emplace_back(boost::apply_visitor(command_visitor, command.command));
2093 BIND_SYNC(query_lights_state) << [
this](std::string client) ->
R<std::vector<cr::LightState>>
2096 std::vector<cr::LightState> result;
2097 auto *World =
Episode->GetWorld();
2100 result = CarlaLightSubsystem->
GetLights(FString(client.c_str()));
2105 BIND_SYNC(update_lights_state) << [
this]
2106 (std::string client,
const std::vector<cr::LightState>& lights,
bool discard_client) ->
R<void> 2109 auto *World =
Episode->GetWorld();
2112 CarlaLightSubsystem->
SetLights(FString(client.c_str()), lights, discard_client);
2122 ->
R<std::pair<bool,cr::LabelledPoint>>
2125 auto *World =
Episode->GetWorld();
2126 constexpr
float meter_to_centimeter = 100.0f;
2135 meter_to_centimeter * SearchDistance, World);
2140 ->
R<std::vector<cr::LabelledPoint>>
2143 auto *World =
Episode->GetWorld();
2144 FVector UEStartLocation = StartLocation;
2145 FVector UEEndLocation = EndLocation;
2151 UEEndLocation = LargeMap->GlobalToLocalLocation(UEEndLocation);
2164 #undef REQUIRE_CARLA_EPISODE 2165 #undef RESPOND_ERROR_FSTRING 2166 #undef RESPOND_ERROR 2167 #undef CARLA_ENSURE_GAME_THREAD 2179 Pimpl = MakeUnique<FPimpl>(RPCPort, StreamingPort);
2180 StreamingPort =
Pimpl->StreamingServer.GetLocalEndpoint().port();
2184 TEXT(
"Initialized CarlaServer: Ports(rpc=%d, streaming=%d)"),
2187 return Pimpl->BroadcastStream;
2192 check(
Pimpl !=
nullptr);
2193 UE_LOG(LogCarlaServer, Log, TEXT(
"New episode '%s' started"), *Episode.
GetMapName());
2194 Pimpl->Episode = &Episode;
2199 check(
Pimpl !=
nullptr);
2200 Pimpl->Episode =
nullptr;
2205 check(
Pimpl !=
nullptr);
2207 int32_t RPCThreads = std::max(2u, NumberOfWorkerThreads / 2u);
2208 int32_t StreamingThreads = std::max(2u, NumberOfWorkerThreads - RPCThreads);
2214 RPCThreads = std::max(2u, NumberOfWorkerThreads / 2u);
2216 if(!FParse::Value(
FCommandLine::Get(), TEXT(
"-StreamingThreads="), StreamingThreads))
2218 StreamingThreads = std::max(2u, NumberOfWorkerThreads - RPCThreads);
2221 UE_LOG(LogCarla, Log, TEXT(
"FCarlaServer AsyncRun %d, RPCThreads %d, StreamingThreads %d"),
2222 NumberOfWorkerThreads, RPCThreads, StreamingThreads);
2224 Pimpl->Server.AsyncRun(RPCThreads);
2225 Pimpl->StreamingServer.AsyncRun(StreamingThreads);
2231 TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__);
2237 if (
Pimpl->TickCuesReceived > 0u)
2239 --(
Pimpl->TickCuesReceived);
2247 check(
Pimpl !=
nullptr);
2248 Pimpl->Server.Stop();
2253 check(
Pimpl !=
nullptr);
2254 return Pimpl->StreamingServer.MakeStream();
carla::streaming::Stream BroadcastStream
FCarlaActor * FindCarlaActor(FCarlaActor::IdType ActorId)
Find a Carla actor by id.
virtual ECarlaServerResponse SetVehicleLightState(const FVehicleLightState &)
TArray< FEnvironmentObject > GetEnvironmentObjects(uint8 QueriedTag=0xFF) const
void ApplyTextureToActor(AActor *Actor, UTexture2D *Texture, const carla::rpc::MaterialParameter &TextureParam)
FDataStream OpenStream() const
auto end() const noexcept
std::vector< carla::rpc::LightState > GetLights(FString Client)
static FString StatusToString(EActorSpawnResultStatus Status)
bool LoadNewOpendriveEpisode(const FString &OpenDriveString, const carla::rpc::OpendriveGenerationParameters &Params)
Load a new map generating the mesh from OpenDRIVE data and start a new episode.
void StopReplayer(bool KeepActors=false)
bool DestroyActor(AActor *Actor)
virtual ECarlaServerResponse SetWheelSteerDirection(const EVehicleWheelLocation &, float)
AWeather * GetWeather() const
std::string ShowFileCollisions(std::string Name, char Type1, char Type2)
void SetMapLayer(int32 MapLayer)
auto operator<<(FuncT func)
virtual ECarlaServerResponse SetActorEnableGravity(bool bEnabled)
FVector GlobalToLocalLocation(const FVector &InLocation) const
FString GetStringError(ECarlaServerResponse Response)
carla::rpc::ResponseError RespondError(const FString &FuncName, const FString &ErrorMessage, const FString &ExtraInfo="")
virtual ECarlaServerResponse SetBonesTransform(const FWalkerBoneControlIn &)
The game instance contains elements that must be kept alive in between levels.
TMap< FString, FWalkerBoneControlOutData > BoneTransforms
static TArray< uint8 > Load(FString MapName)
Return the Navigation Mesh Binary associated to MapName, or empty if the such file wasn't serialized...
#define MAKE_RESULT(operation)
static auto MakeRecursiveOverload(FuncTs &&... fs)
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()
std::string ShowFileActorsBlocked(std::string Name, double MinTime=30, double MinDistance=10)
std::vector< std::pair< ActorId, VehicleLightState::flag_type > > VehicleLightStateList
TArray< FTransform > GetRecommendedSpawnPoints() const
Return the list of recommended spawn points for vehicles.
FPimpl(uint16_t RPCPort, uint16_t StreamingPort)
#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)
FDataMultiStream Start(uint16_t RPCPort, uint16_t StreamingPort)
void PutActorToSleep(carla::rpc::ActorId ActorId)
virtual ECarlaServerResponse GetPhysicsControl(FVehiclePhysicsControl &)
void UnLoadMapLayer(int32 MapLayers)
void OnActorSpawned(const FCarlaActor &CarlaActor)
virtual ECarlaServerResponse ApplyControlToVehicle(const FVehicleControl &, const EVehicleInputPriority &)
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
virtual ECarlaServerResponse SetTrafficLightState(const ETrafficLightState &)
static ACarlaGameModeBase * GetGameMode(const UObject *WorldContextObject)
static void GetTrafficLightBoundingBox(const ATrafficLightBase *TrafficLight, TArray< FBoundingBox > &OutBB, TArray< uint8 > &OutTag, uint8 InTagQueried=0xFF)
const FString & GetMapName() const
Return the name of the map loaded in this episode.
static uint64_t GetFrameCounter()
virtual ECarlaServerResponse EnableChronoPhysics(uint64_t, float, const FString &, const FString &, const FString &, const FString &)
void SetActorGlobalLocation(const FVector &Location, ETeleportType Teleport=ETeleportType::TeleportPhysics)
void Draw(const carla::rpc::DebugShape &Shape)
ECarlaServerResponse AddActorImpulse(const FVector &Impulse)
ECarlaServerResponse SetActorTargetAngularVelocity(const FVector &AngularVelocity)
void LoadMapLayer(int32 MapLayers)
void RunSome(uint32 Milliseconds)
const FActorRegistry & GetActorRegistry() const
virtual ECarlaServerResponse CloseVehicleDoor(const EVehicleDoor)
std::string StartRecorder(std::string name, bool AdditionalData)
ECarlaServerResponse AddActorForceAtLocation(const FVector &Force, const FVector &Location)
ECarlaServerResponse AddActorTorque(const FVector &Torque)
static UCarlaGameInstance * GetGameInstance(const UObject *WorldContextObject)
carla::SharedPtr< cc::Actor > Actor
virtual ECarlaServerResponse FreezeTrafficLight(bool)
virtual ECarlaServerResponse EnableActorConstantVelocity(const FVector &)
An RPC server in which functions can be bind to run synchronously or asynchronously.
virtual ECarlaServerResponse GetPoseFromAnimation()
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)
Response< ActorId > CommandResponse
virtual ECarlaServerResponse ResetTrafficLightGroup()
void SetAttachmentType(carla::rpc::AttachmentType InAttachmentType)
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.
virtual ECarlaServerResponse GetBonesTransform(FWalkerBoneControlOut &)
#define RESPOND_ERROR(str)
virtual ECarlaServerResponse ApplyControlToWalker(const FWalkerControl &)
static std::vector< T > MakeVectorFromTArray(const TArray< Other > &Array)
Token token() const
Token associated with this stream.
auto GetId() const
Return the unique id of this episode.
void SetLights(FString Client, std::vector< carla::rpc::LightState > LightsToSet, bool DiscardClient=false)
void SetReplayerIgnoreHero(bool IgnoreHero)
constexpr size_t milliseconds() const noexcept
virtual ECarlaServerResponse SetLightGreenTime(float)
TArray< FString > GetNamesOfAllActors()
static FString GetXODR(const UWorld *World)
Return the OpenDrive XML associated to MapName, or empty if the file is not found.
ACarlaRecorder * GetRecorder() const
virtual ECarlaServerResponse SetLightYellowTime(float)
carla::rpc::Server Server
virtual ECarlaServerResponse SetLightRedTime(float)
ALargeMapManager * GetLMManager() const
static auto MakeOverload(FuncTs &&... fs)
Creates an "overloaded callable object" out of one or more callable objects, each callable object wil...
virtual ECarlaServerResponse SetActorAutopilot(bool, bool bKeepState=false)
carla::rpc::Server & _server
ECarlaServerResponse SetActorTargetVelocity(const FVector &Velocity)
virtual ECarlaServerResponse OpenVehicleDoor(const EVehicleDoor)
virtual ECarlaServerResponse EnableCarSim(const FString &)
FTransform LocalToGlobalTransform(const FTransform &InTransform) const
TUniquePtr< FPimpl > Pimpl
APawn * GetSpectatorPawn() const
void AttachActors(AActor *Child, AActor *Parent, EAttachmentType InAttachmentType=EAttachmentType::Rigid)
Attach Child to Parent.
void AddChildren(IdType ChildId)
virtual ECarlaServerResponse UseCarSimRoad(bool)
static std::pair< bool, carla::rpc::LabelledPoint > ProjectPoint(FVector StartLocation, FVector Direction, float MaxDistance, UWorld *World)
std::string ShowFileInfo(std::string Name, bool bShowAll=false)
static ALargeMapManager * GetLargeMapManager(const UObject *WorldContextObject)
carla::streaming::Server StreamingServer
ECarlaServerResponse AddActorImpulseAtLocation(const FVector &Impulse, const FVector &Location)
TArray< FBoundingBox > GetAllBBsOfLevel(uint8 TagQueried=0xFF) const
Base class for the CARLA Game Mode.
virtual ECarlaServerResponse SetActorSimulatePhysics(bool bEnabled)
virtual ECarlaServerResponse GetVehicleLightState(FVehicleLightState &)
const FEpisodeSettings & GetSettings() const
void SetParent(IdType InParentId)
IdType GetActorId() const
ECarlaServerResponse AddActorForce(const FVector &Force)
Texture< sensor::data::Color > TextureColor
virtual ECarlaServerResponse ApplyPhysicsControl(const FVehiclePhysicsControl &)
const FString GetFullMapPath() const
Base class for CARLA wheeled vehicles.
virtual ECarlaServerResponse GetWheelSteerAngle(const EVehicleWheelLocation &, float &)
Texture< FloatColor > TextureFloatColor
void AsyncRun(uint32 NumberOfWorkerThreads)
void SetActorGlobalTransform(const FTransform &Transform, ETeleportType Teleport=ETeleportType::TeleportPhysics)
ECarlaServerResponse AddActorAngularImpulse(const FVector &AngularInpulse)
#define REQUIRE_CARLA_EPISODE()
geom::Transform Transform
FVector LocalToGlobalLocation(const FVector &InLocation) const
constexpr ServerBinder(const char *name, carla::rpc::Server &srv, bool sync)
virtual ECarlaServerResponse ShowVehicleDebugTelemetry(bool)
virtual ECarlaServerResponse SetWalkerState(const FTransform &Transform, carla::rpc::WalkerControl WalkerControl)
ActorType GetActorType() const
A view over an actor and its properties.
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)
void NotifyBeginEpisode(UCarlaEpisode &Episode)