LiXiaoqi/libcarla
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
963a8f99a0
libcarla/include/carla/client/detail/Simulator.cpp
LiXiaoqi 220ecf68c6 init
2024-10-18 13:19:59 +08:00

467 lines
17 KiB
C++
Raw Blame History

// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/client/detail/Simulator.h"
#include "carla/Debug.h"
#include "carla/Exception.h"
#include "carla/Logging.h"
#include "carla/RecurrentSharedFuture.h"
#include "carla/client/BlueprintLibrary.h"
#include "carla/client/FileTransfer.h"
#include "carla/client/Map.h"
#include "carla/client/Sensor.h"
#include "carla/client/TimeoutException.h"
#include "carla/client/WalkerAIController.h"
#include "carla/client/detail/ActorFactory.h"
#include "carla/client/detail/WalkerNavigation.h"
#include "carla/trafficmanager/TrafficManager.h"
#include "carla/sensor/Deserializer.h"
#include <exception>
#include <thread>
using namespace std::string_literals;
namespace carla {
namespace client {
namespace detail {
// ===========================================================================
// -- Static local methods ---------------------------------------------------
// ===========================================================================
static void ValidateVersions(Client &client) {
const auto vc = client.GetClientVersion();
const auto vs = client.GetServerVersion();
if (vc != vs) {
log_warning(
"Version mismatch detected: You are trying to connect to a simulator",
"that might be incompatible with this API");
log_warning("Client API version =", vc);
log_warning("Simulator API version =", vs);
}
}
static bool SynchronizeFrame(uint64_t frame, const Episode &episode, time_duration timeout) {
bool result = true;
auto start = std::chrono::system_clock::now();
while (frame > episode.GetState()->GetTimestamp().frame) {
std::this_thread::yield();
auto end = std::chrono::system_clock::now();
auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(end-start);
if(timeout.to_chrono() < diff) {
result = false;
break;
}
}
if(result) {
carla::traffic_manager::TrafficManager::Tick();
}
return result;
}
// ===========================================================================
// -- Constructor ------------------------------------------------------------
// ===========================================================================
Simulator::Simulator(
const std::string &host,
const uint16_t port,
const size_t worker_threads,
const bool enable_garbage_collection)
: LIBCARLA_INITIALIZE_LIFETIME_PROFILER("SimulatorClient("s + host + ":" + std::to_string(port) + ")"),
_client(host, port, worker_threads),
_light_manager(new LightManager()),
_gc_policy(enable_garbage_collection ?
GarbageCollectionPolicy::Enabled : GarbageCollectionPolicy::Disabled) {}
// ===========================================================================
// -- Load a new episode -----------------------------------------------------
// ===========================================================================
EpisodeProxy Simulator::LoadEpisode(std::string map_name, bool reset_settings, rpc::MapLayer map_layers) {
const auto id = GetCurrentEpisode().GetId();
_client.LoadEpisode(std::move(map_name), reset_settings, map_layers);
// We are waiting 50ms for the server to reload the episode.
// If in this time we have not detected a change of episode, we try again
// 'number_of_attempts' times.
// TODO This time is completly arbitrary so we need to improve
// this pipeline to not depend in this time because this timeout
// could result that the client resume the simulation in different
// initial ticks when loading a map in syncronous mode.
size_t number_of_attempts = _client.GetTimeout().milliseconds() / 50u;
for (auto i = 0u; i < number_of_attempts; ++i) {
using namespace std::literals::chrono_literals;
if (_client.GetEpisodeSettings().synchronous_mode)
_client.SendTickCue();
_episode->WaitForState(50ms);
auto episode = GetCurrentEpisode();
if (episode.GetId() != id) {
return episode;
}
}
throw_exception(std::runtime_error("failed to connect to newly created map"));
}
EpisodeProxy Simulator::LoadOpenDriveEpisode(
std::string opendrive,
const rpc::OpendriveGenerationParameters & params, bool reset_settings) {
// The "OpenDriveMap" is an ".umap" located in:
// "carla/Unreal/CarlaUE4/Content/Carla/Maps/"
// It will load the last sended OpenDRIVE by client's "LoadOpenDriveEpisode()"
constexpr auto custom_opendrive_map = "OpenDriveMap";
_client.CopyOpenDriveToServer(std::move(opendrive), params);
return LoadEpisode(custom_opendrive_map, reset_settings);
}
// ===========================================================================
// -- Access to current episode ----------------------------------------------
// ===========================================================================
void Simulator::GetReadyCurrentEpisode() {
if (_episode == nullptr) {
ValidateVersions(_client);
_episode = std::make_shared<Episode>(_client, std::weak_ptr<Simulator>(shared_from_this()));
_episode->Listen();
if (!GetEpisodeSettings().synchronous_mode) {
WaitForTick(_client.GetTimeout());
}
_light_manager->SetEpisode(WeakEpisodeProxy{shared_from_this()});
}
}
EpisodeProxy Simulator::GetCurrentEpisode() {
GetReadyCurrentEpisode();
return EpisodeProxy{shared_from_this()};
}
bool Simulator::ShouldUpdateMap(rpc::MapInfo& map_info) {
if (!_cached_map) {
return true;
}
if (map_info.name != _cached_map->GetName() ||
_open_drive_file.size() != _cached_map->GetOpenDrive().size()) {
return true;
}
return false;
}
SharedPtr<Map> Simulator::GetCurrentMap() {
DEBUG_ASSERT(_episode != nullptr);
if (!_cached_map || _episode->HasMapChangedSinceLastCall()) {
rpc::MapInfo map_info = _client.GetMapInfo();
std::string map_name;
std::string map_base_path;
bool fill_base_string = false;
for (int i = map_info.name.size() - 1; i >= 0; --i) {
if (fill_base_string == false && map_info.name[i] != '/') {
map_name += map_info.name[i];
} else {
map_base_path += map_info.name[i];
fill_base_string = true;
}
}
std::reverse(map_name.begin(), map_name.end());
std::reverse(map_base_path.begin(), map_base_path.end());
std::string XODRFolder = map_base_path + "/OpenDrive/" + map_name + ".xodr";
if (FileTransfer::FileExists(XODRFolder) == false) _client.GetRequiredFiles();
_open_drive_file = _client.GetMapData();
_cached_map = MakeShared<Map>(map_info, _open_drive_file);
}
return _cached_map;
}
// ===========================================================================
// -- Required files ---------------------------------------------------------
// ===========================================================================
bool Simulator::SetFilesBaseFolder(const std::string &path) {
return _client.SetFilesBaseFolder(path);
}
std::vector<std::string> Simulator::GetRequiredFiles(const std::string &folder, const bool download) const {
return _client.GetRequiredFiles(folder, download);
}
void Simulator::RequestFile(const std::string &name) const {
_client.RequestFile(name);
}
std::vector<uint8_t> Simulator::GetCacheFile(const std::string &name, const bool request_otherwise) const {
return _client.GetCacheFile(name, request_otherwise);
}
// ===========================================================================
// -- Tick -------------------------------------------------------------------
// ===========================================================================
WorldSnapshot Simulator::WaitForTick(time_duration timeout) {
DEBUG_ASSERT(_episode != nullptr);
// tick pedestrian navigation
NavigationTick();
auto result = _episode->WaitForState(timeout);
if (!result.has_value()) {
throw_exception(TimeoutException(_client.GetEndpoint(), timeout));
}
return *result;
}
uint64_t Simulator::Tick(time_duration timeout) {
DEBUG_ASSERT(_episode != nullptr);
// tick pedestrian navigation
NavigationTick();
// send tick command
const auto frame = _client.SendTickCue();
// waits until new episode is received
bool result = SynchronizeFrame(frame, *_episode, timeout);
if (!result) {
throw_exception(TimeoutException(_client.GetEndpoint(), timeout));
}
return frame;
}
// ===========================================================================
// -- Access to global objects in the episode --------------------------------
// ===========================================================================
SharedPtr<BlueprintLibrary> Simulator::GetBlueprintLibrary() {
auto defs = _client.GetActorDefinitions();
return MakeShared<BlueprintLibrary>(std::move(defs));
}
rpc::VehicleLightStateList Simulator::GetVehiclesLightStates() {
return _client.GetVehiclesLightStates();
}
SharedPtr<Actor> Simulator::GetSpectator() {
return MakeActor(_client.GetSpectator());
}
uint64_t Simulator::SetEpisodeSettings(const rpc::EpisodeSettings &settings) {
if (settings.synchronous_mode && !settings.fixed_delta_seconds) {
log_warning(
"synchronous mode enabled with variable delta seconds. It is highly "
"recommended to set 'fixed_delta_seconds' when running on synchronous mode.");
}
else if (settings.synchronous_mode && settings.substepping) {
if(settings.max_substeps < 1 || settings.max_substeps > 16) {
log_warning(
"synchronous mode and substepping are enabled but the number of substeps is not valid. "
"Please be aware that this value needs to be in the range [1-16].");
}
double n_substeps = settings.fixed_delta_seconds.get() / settings.max_substep_delta_time;
if (n_substeps > static_cast<double>(settings.max_substeps)) {
log_warning(
"synchronous mode and substepping are enabled but the values for the simulation are not valid. "
"The values should fulfil fixed_delta_seconds <= max_substep_delta_time * max_substeps. "
"Be very careful about that, the time deltas are not guaranteed.");
}
}
const auto frame = _client.SetEpisodeSettings(settings);
using namespace std::literals::chrono_literals;
SynchronizeFrame(frame, *_episode, 1s);
return frame;
}
// ===========================================================================
// -- AI ---------------------------------------------------------------------
// ===========================================================================
std::shared_ptr<WalkerNavigation> Simulator::GetNavigation() {
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
return nav;
}
// tick pedestrian navigation
void Simulator::NavigationTick() {
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
nav->Tick(_episode);
}
void Simulator::RegisterAIController(const WalkerAIController &controller) {
auto walker = controller.GetParent();
if (walker == nullptr) {
throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
return;
}
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
nav->RegisterWalker(walker->GetId(), controller.GetId());
}
void Simulator::UnregisterAIController(const WalkerAIController &controller) {
auto walker = controller.GetParent();
if (walker == nullptr) {
throw_exception(std::runtime_error(controller.GetDisplayId() + ": not attached to walker"));
return;
}
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
nav->UnregisterWalker(walker->GetId(), controller.GetId());
}
boost::optional<geom::Location> Simulator::GetRandomLocationFromNavigation() {
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
return nav->GetRandomLocation();
}
void Simulator::SetPedestriansCrossFactor(float percentage) {
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
nav->SetPedestriansCrossFactor(percentage);
}
void Simulator::SetPedestriansSeed(unsigned int seed) {
DEBUG_ASSERT(_episode != nullptr);
auto nav = _episode->CreateNavigationIfMissing();
nav->SetPedestriansSeed(seed);
}
// ===========================================================================
// -- General operations with actors -----------------------------------------
// ===========================================================================
SharedPtr<Actor> Simulator::SpawnActor(
const ActorBlueprint &blueprint,
const geom::Transform &transform,
Actor *parent,
rpc::AttachmentType attachment_type,
GarbageCollectionPolicy gc) {
rpc::Actor actor;
if (parent != nullptr) {
actor = _client.SpawnActorWithParent(
blueprint.MakeActorDescription(),
transform,
parent->GetId(),
attachment_type);
} else {
actor = _client.SpawnActor(
blueprint.MakeActorDescription(),
transform);
}
DEBUG_ASSERT(_episode != nullptr);
_episode->RegisterActor(actor);
const auto gca = (gc == GarbageCollectionPolicy::Inherit ? _gc_policy : gc);
auto result = ActorFactory::MakeActor(GetCurrentEpisode(), actor, gca);
log_debug(
result->GetDisplayId(),
"created",
gca == GarbageCollectionPolicy::Enabled ? "with" : "without",
"garbage collection");
return result;
}
bool Simulator::DestroyActor(Actor &actor) {
bool success = true;
success = _client.DestroyActor(actor.GetId());
if (success) {
// Remove it's persistent state so it cannot access the client anymore.
actor.GetEpisode().Clear();
log_debug(actor.GetDisplayId(), "destroyed.");
} else {
log_debug("failed to destroy", actor.GetDisplayId());
}
return success;
}
// ===========================================================================
// -- Operations with sensors ------------------------------------------------
// ===========================================================================
void Simulator::SubscribeToSensor(
const Sensor &sensor,
std::function<void(SharedPtr<sensor::SensorData>)> callback) {
DEBUG_ASSERT(_episode != nullptr);
_client.SubscribeToStream(
sensor.GetActorDescription().GetStreamToken(),
[cb=std::move(callback), ep=WeakEpisodeProxy{shared_from_this()}](auto buffer) {
auto data = sensor::Deserializer::Deserialize(std::move(buffer));
data->_episode = ep.TryLock();
cb(std::move(data));
});
}
void Simulator::UnSubscribeFromSensor(Actor &sensor) {
_client.UnSubscribeFromStream(sensor.GetActorDescription().GetStreamToken());
// If in the future we need to unsubscribe from each gbuffer individually, it should be done here.
}
void Simulator::EnableForROS(const Sensor &sensor) {
_client.EnableForROS(sensor.GetActorDescription().GetStreamToken());
}
void Simulator::DisableForROS(const Sensor &sensor) {
_client.DisableForROS(sensor.GetActorDescription().GetStreamToken());
}
bool Simulator::IsEnabledForROS(const Sensor &sensor) {
return _client.IsEnabledForROS(sensor.GetActorDescription().GetStreamToken());
}
void Simulator::SubscribeToGBuffer(
Actor &actor,
uint32_t gbuffer_id,
std::function<void(SharedPtr<sensor::SensorData>)> callback) {
_client.SubscribeToGBuffer(actor.GetId(), gbuffer_id,
[cb=std::move(callback), ep=WeakEpisodeProxy{shared_from_this()}](auto buffer) {
auto data = sensor::Deserializer::Deserialize(std::move(buffer));
data->_episode = ep.TryLock();
cb(std::move(data));
});
}
void Simulator::UnSubscribeFromGBuffer(Actor &actor, uint32_t gbuffer_id) {
_client.UnSubscribeFromGBuffer(actor.GetId(), gbuffer_id);
}
void Simulator::FreezeAllTrafficLights(bool frozen) {
_client.FreezeAllTrafficLights(frozen);
}
// =========================================================================
/// -- Texture updating operations
// =========================================================================
void Simulator::ApplyColorTextureToObjects(
const std::vector<std::string> &objects_name,
const rpc::MaterialParameter& parameter,
const rpc::TextureColor& Texture) {
_client.ApplyColorTextureToObjects(objects_name, parameter, Texture);
}
void Simulator::ApplyColorTextureToObjects(
const std::vector<std::string> &objects_name,
const rpc::MaterialParameter& parameter,
const rpc::TextureFloatColor& Texture) {
_client.ApplyColorTextureToObjects(objects_name, parameter, Texture);
}
std::vector<std::string> Simulator::GetNamesOfAllObjects() const {
return _client.GetNamesOfAllObjects();
}
} // namespace detail
} // namespace client
} // namespace carla
Reference in New Issue View Git Blame Copy Permalink