p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Pedestrian will wait for the trafficlight before crossing

This commit is contained in:
bernatx 2023-03-17 10:40:25 +01:00 committed by bernat
parent c243eb7844
commit 189e4f4c4f
15 changed files with 230 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
LibCarla/source/carla/client/WalkerAIController.cpp
View File

@ -7,6 +7,7 @@
#include "carla/client/WalkerAIController.h"
#include "carla/client/detail/Simulator.h"
#include "carla/client/detail/WalkerNavigation.h"
namespace carla {
namespace client {

1
LibCarla/source/carla/client/WalkerAIController.h
View File

@ -7,7 +7,6 @@
#pragma once
#include "carla/client/Actor.h"
#include "carla/client/detail/WalkerNavigation.h"
#include "carla/geom/Vector3D.h"
#include <boost/optional.hpp>

35
LibCarla/source/carla/client/detail/Episode.cpp
View File

@ -34,12 +34,13 @@ using namespace std::chrono_literals;
return actors.GetActorsById(actor_ids);
}
Episode::Episode(Client &client)
: Episode(client, client.GetEpisodeInfo()) {}
Episode::Episode(Client &client, std::weak_ptr<Simulator> simulator)
: Episode(client, client.GetEpisodeInfo(), simulator) {}
Episode::Episode(Client &client, const rpc::EpisodeInfo &info)
Episode::Episode(Client &client, const rpc::EpisodeInfo &info, std::weak_ptr<Simulator> simulator)
: _client(client),
_state(std::make_shared<EpisodeState>(info.id)),
_simulator(simulator),
_token(info.token) {}
Episode::~Episode() {
@ -121,18 +122,6 @@ using namespace std::chrono_literals;
return actor;
}
std::shared_ptr<WalkerNavigation> Episode::CreateNavigationIfMissing() {
std::shared_ptr<WalkerNavigation> navigation;
do {
navigation = _navigation.load();
if (navigation == nullptr) {
auto new_navigation = std::make_shared<WalkerNavigation>(_client);
_navigation.compare_exchange(&navigation, new_navigation);
}
} while (navigation == nullptr);
return navigation;
}
std::vector<rpc::Actor> Episode::GetActorsById(const std::vector<ActorId> &actor_ids) {
return GetActorsById_Impl(_client, _actors, actor_ids);
}
@ -144,7 +133,7 @@ using namespace std::chrono_literals;
void Episode::OnEpisodeStarted() {
_actors.Clear();
_on_tick_callbacks.Clear();
_navigation.reset();
_walker_navigation.reset();
traffic_manager::TrafficManager::Release();
}
@ -160,12 +149,16 @@ using namespace std::chrono_literals;
return false;
}
void Episode::NavigationTick() {
// tick pedestrian navigation
auto navigation = _navigation.load();
if (navigation != nullptr) {
navigation->Tick(shared_from_this());
std::shared_ptr<WalkerNavigation> Episode::CreateNavigationIfMissing() {
std::shared_ptr<WalkerNavigation> nav;
do {
nav = _walker_navigation.load();
if (nav == nullptr) {
auto new_nav = std::make_shared<WalkerNavigation>(_simulator);
_walker_navigation.compare_exchange(&nav, new_nav);
}
} while (nav == nullptr);
return nav;
}
} // namespace detail

35
LibCarla/source/carla/client/detail/Episode.h
View File

@ -14,7 +14,7 @@
#include "carla/client/detail/CachedActorList.h"
#include "carla/client/detail/CallbackList.h"
#include "carla/client/detail/EpisodeState.h"
#include "carla/client/detail/WalkerNavigation.h"
#include "carla/client/detail/EpisodeProxy.h"
#include "carla/rpc/EpisodeInfo.h"
#include <vector>
@ -24,6 +24,7 @@ namespace client {
namespace detail {
class Client;
class WalkerNavigation;
/// Holds the current episode, and the current episode state.
///
@ -35,7 +36,7 @@ namespace detail {
private NonCopyable {
public:
explicit Episode(Client &client);
explicit Episode(Client &client, std::weak_ptr<Simulator> simulator);
~Episode();
@ -49,14 +50,6 @@ namespace detail {
return _state.load();
}
std::shared_ptr<WalkerNavigation> CreateNavigationIfMissing();
std::shared_ptr<WalkerNavigation> GetNavigation() const {
auto nav = _navigation.load();
DEBUG_ASSERT(nav != nullptr);
return nav;
}
void RegisterActor(rpc::Actor actor) {
_actors.Insert(std::move(actor));
}
@ -95,17 +88,9 @@ namespace detail {
_on_light_update_callbacks.Remove(id);
}
void SetPedestriansCrossFactor(float percentage) {
auto nav = _navigation.load();
DEBUG_ASSERT(nav != nullptr);
nav->SetPedestriansCrossFactor(percentage);
}
void SetPedestriansCrossFactor(float percentage);
void SetPedestriansSeed(unsigned int seed) {
auto nav = _navigation.load();
DEBUG_ASSERT(nav != nullptr);
nav->SetPedestriansSeed(seed);
}
void SetPedestriansSeed(unsigned int seed);
void AddPendingException(std::string e) {
_pending_exceptions = true;
@ -114,11 +99,11 @@ namespace detail {
bool HasMapChangedSinceLastCall();
void NavigationTick();
std::shared_ptr<WalkerNavigation> CreateNavigationIfMissing();
private:
Episode(Client &client, const rpc::EpisodeInfo &info);
Episode(Client &client, const rpc::EpisodeInfo &info, std::weak_ptr<Simulator> simulator);
void OnEpisodeStarted();
@ -128,8 +113,6 @@ namespace detail {
AtomicSharedPtr<const EpisodeState> _state;
AtomicSharedPtr<WalkerNavigation> _navigation;
std::string _pending_exceptions_msg;
CachedActorList _actors;
@ -142,11 +125,15 @@ namespace detail {
RecurrentSharedFuture<WorldSnapshot> _snapshot;
AtomicSharedPtr<WalkerNavigation> _walker_navigation;
const streaming::Token _token;
bool _pending_exceptions = false;
bool _should_update_map = true;
std::weak_ptr<Simulator> _simulator;
};
} // namespace detail

51
LibCarla/source/carla/client/detail/Simulator.cpp
View File

@ -17,6 +17,7 @@
#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"
@ -126,16 +127,20 @@ namespace detail {
// -- Access to current episode ----------------------------------------------
// ===========================================================================
EpisodeProxy Simulator::GetCurrentEpisode() {
void Simulator::GetReadyCurrentEpisode() {
if (_episode == nullptr) {
ValidateVersions(_client);
_episode = std::make_shared<Episode>(_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() {
log_warning("GetCurrentEpisode");
GetReadyCurrentEpisode();
return EpisodeProxy{shared_from_this()};
}
@ -205,7 +210,7 @@ namespace detail {
DEBUG_ASSERT(_episode != nullptr);
// tick pedestrian navigation
_episode->NavigationTick();
NavigationTick();
auto result = _episode->WaitForState(timeout);
if (!result.has_value()) {
@ -218,7 +223,7 @@ namespace detail {
DEBUG_ASSERT(_episode != nullptr);
// tick pedestrian navigation
_episode->NavigationTick();
NavigationTick();
// send tick command
const auto frame = _client.SendTickCue();
@ -281,6 +286,19 @@ namespace detail {
// -- 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) {
@ -288,9 +306,8 @@ namespace detail {
return;
}
DEBUG_ASSERT(_episode != nullptr);
auto navigation = _episode->CreateNavigationIfMissing();
DEBUG_ASSERT(navigation != nullptr);
navigation->RegisterWalker(walker->GetId(), controller.GetId());
auto nav = _episode->CreateNavigationIfMissing();
nav->RegisterWalker(walker->GetId(), controller.GetId());
}
void Simulator::UnregisterAIController(const WalkerAIController &controller) {
@ -300,30 +317,26 @@ namespace detail {
return;
}
DEBUG_ASSERT(_episode != nullptr);
auto navigation = _episode->CreateNavigationIfMissing();
DEBUG_ASSERT(navigation != nullptr);
navigation->UnregisterWalker(walker->GetId(), controller.GetId());
auto nav = _episode->CreateNavigationIfMissing();
nav->UnregisterWalker(walker->GetId(), controller.GetId());
}
boost::optional<geom::Location> Simulator::GetRandomLocationFromNavigation() {
DEBUG_ASSERT(_episode != nullptr);
auto navigation = _episode->CreateNavigationIfMissing();
DEBUG_ASSERT(navigation != nullptr);
return navigation->GetRandomLocation();
auto nav = _episode->CreateNavigationIfMissing();
return nav->GetRandomLocation();
}
void Simulator::SetPedestriansCrossFactor(float percentage) {
DEBUG_ASSERT(_episode != nullptr);
auto navigation = _episode->CreateNavigationIfMissing();
DEBUG_ASSERT(navigation != nullptr);
navigation->SetPedestriansCrossFactor(percentage);
auto nav = _episode->CreateNavigationIfMissing();
nav->SetPedestriansCrossFactor(percentage);
}
void Simulator::SetPedestriansSeed(unsigned int seed) {
DEBUG_ASSERT(_episode != nullptr);
auto navigation = _episode->CreateNavigationIfMissing();
DEBUG_ASSERT(navigation != nullptr);
navigation->SetPedestriansSeed(seed);
auto nav = _episode->CreateNavigationIfMissing();
nav->SetPedestriansSeed(seed);
}
// ===========================================================================

39
LibCarla/source/carla/client/detail/Simulator.h
View File

@ -7,6 +7,7 @@
#pragma once
#include "carla/Debug.h"
#include "carla/Logging.h"
#include "carla/Memory.h"
#include "carla/NonCopyable.h"
#include "carla/client/Actor.h"
@ -14,12 +15,12 @@
#include "carla/client/TrafficLight.h"
#include "carla/client/Vehicle.h"
#include "carla/client/Walker.h"
#include "carla/client/World.h"
#include "carla/client/WorldSnapshot.h"
#include "carla/client/detail/ActorFactory.h"
#include "carla/client/detail/Client.h"
#include "carla/client/detail/Episode.h"
#include "carla/client/detail/EpisodeProxy.h"
#include "carla/client/detail/WalkerNavigation.h"
#include "carla/profiler/LifetimeProfiled.h"
#include "carla/rpc/TrafficLightState.h"
#include "carla/rpc/VehicleLightStateList.h"
@ -40,6 +41,7 @@ namespace client {
class Map;
class Sensor;
class WalkerAIController;
class WalkerNavigation;
namespace detail {
@ -93,11 +95,13 @@ namespace detail {
/// @{
/// @pre Cannot be called previous to GetCurrentEpisode.
auto GetCurrentEpisodeId() const {
auto GetCurrentEpisodeId() {
GetReadyCurrentEpisode();
DEBUG_ASSERT(_episode != nullptr);
return _episode->GetId();
}
void GetReadyCurrentEpisode();
EpisodeProxy GetCurrentEpisode();
/// @}
@ -106,6 +110,16 @@ namespace detail {
// =========================================================================
/// @{
World GetWorld() {
return World{GetCurrentEpisode()};
}
/// @}
// =========================================================================
/// @name World snapshot
// =========================================================================
/// @{
WorldSnapshot GetWorldSnapshot() const {
DEBUG_ASSERT(_episode != nullptr);
return WorldSnapshot{_episode->GetState()};
@ -284,16 +298,16 @@ namespace detail {
// =========================================================================
/// @{
std::shared_ptr<WalkerNavigation> GetNavigation();
void NavigationTick();
void RegisterAIController(const WalkerAIController &controller);
void UnregisterAIController(const WalkerAIController &controller);
boost::optional<geom::Location> GetRandomLocationFromNavigation();
std::shared_ptr<WalkerNavigation> GetNavigation() {
return _episode->GetNavigation();
}
void SetPedestriansCrossFactor(float percentage);
void SetPedestriansSeed(unsigned int seed);
@ -347,6 +361,11 @@ namespace detail {
bool DestroyActor(Actor &actor);
bool DestroyActor(ActorId actor_id)
{
return _client.DestroyActor(actor_id);
}
ActorSnapshot GetActorSnapshot(ActorId actor_id) const {
DEBUG_ASSERT(_episode != nullptr);
return _episode->GetState()->GetActorSnapshot(actor_id);
@ -435,10 +454,18 @@ namespace detail {
_client.SetActorCollisions(actor.GetId(), enabled);
}
void SetActorCollisions(ActorId actor_id, bool enabled) {
_client.SetActorCollisions(actor_id, enabled);
}
void SetActorDead(Actor &actor) {
_client.SetActorDead(actor.GetId());
}
void SetActorDead(ActorId actor_id) {
_client.SetActorDead(actor_id);
}
void SetActorEnableGravity(Actor &actor, bool enabled) {
_client.SetActorEnableGravity(actor.GetId(), enabled);
}

28
LibCarla/source/carla/client/detail/WalkerNavigation.cpp
View File

@ -9,6 +9,7 @@
#include "carla/client/detail/Client.h"
#include "carla/client/detail/Episode.h"
#include "carla/client/detail/EpisodeState.h"
#include "carla/client/detail/Simulator.h"
#include "carla/nav/Navigation.h"
#include "carla/rpc/Command.h"
#include "carla/rpc/DebugShape.h"
@ -20,11 +21,12 @@ namespace carla {
namespace client {
namespace detail {
WalkerNavigation::WalkerNavigation(Client &client) : _client(client), _next_check_index(0) {
WalkerNavigation::WalkerNavigation(std::weak_ptr<Simulator> simulator) : _simulator(simulator), _next_check_index(0) {
_nav.SetSimulator(simulator);
// Here call the server to retrieve the navmesh data.
auto files = _client.GetRequiredFiles("Nav");
auto files = _simulator.lock()->GetRequiredFiles("Nav");
if (!files.empty()) {
_nav.Load(_client.GetCacheFile(files[0]));
_nav.Load(_simulator.lock()->GetCacheFile(files[0], true));
}
}
@ -57,7 +59,7 @@ namespace detail {
commands.emplace_back(Cmd::ApplyWalkerState{ handle.walker, trans, speed });
}
}
_client.ApplyBatchSync(std::move(commands), false);
_simulator.lock()->ApplyBatchSync(std::move(commands), false);
// check if any agent has been killed
bool alive;
@ -65,12 +67,12 @@ namespace detail {
// get the agent state
if (_nav.IsWalkerAlive(handle.walker, alive)) {
if (!alive) {
_client.SetActorCollisions(handle.walker, true);
_client.SetActorDead(handle.walker);
_simulator.lock()->SetActorCollisions(handle.walker, true);
_simulator.lock()->SetActorDead(handle.walker);
// remove from the crowd
_nav.RemoveAgent(handle.walker);
// destroy the controller
_client.DestroyActor(handle.controller);
_simulator.lock()->DestroyActor(handle.controller);
// unregister from list
UnregisterWalker(handle.walker, handle.controller);
}
@ -89,7 +91,7 @@ namespace detail {
// remove from the crowd
_nav.RemoveAgent(walkers[_next_check_index].walker);
// destroy the controller
_client.DestroyActor(walkers[_next_check_index].controller);
_simulator.lock()->DestroyActor(walkers[_next_check_index].controller);
// unregister from list
UnregisterWalker(walkers[_next_check_index].walker, walkers[_next_check_index].controller);
}
@ -148,19 +150,19 @@ namespace detail {
// line 1
line1.primitive = carla::rpc::DebugShape::Line {p1, p2, 0.2f};
line1.color = { 0, 255, 0 };
_client.DrawDebugShape(line1);
_simulator.lock()->DrawDebugShape(line1);
// line 2
line1.primitive = carla::rpc::DebugShape::Line {p2, p3, 0.2f};
line1.color = { 255, 0, 0 };
_client.DrawDebugShape(line1);
_simulator.lock()->DrawDebugShape(line1);
// line 3
line1.primitive = carla::rpc::DebugShape::Line {p3, p4, 0.2f};
line1.color = { 0, 0, 255 };
_client.DrawDebugShape(line1);
_simulator.lock()->DrawDebugShape(line1);
// line 4
line1.primitive = carla::rpc::DebugShape::Line {p4, p1, 0.2f};
line1.color = { 255, 255, 0 };
_client.DrawDebugShape(line1);
_simulator.lock()->DrawDebugShape(line1);
}
}
@ -179,7 +181,7 @@ namespace detail {
text.persistent_lines = false;
text.primitive = carla::rpc::DebugShape::String {p1, out.str(), false};
text.color = { 0, 255, 0 };
_client.DrawDebugShape(text);
_simulator.lock()->DrawDebugShape(text);
}
}
}

7
LibCarla/source/carla/client/detail/WalkerNavigation.h
View File

@ -10,7 +10,6 @@
#include "carla/nav/Navigation.h"
#include "carla/NonCopyable.h"
#include "carla/client/Timestamp.h"
#include "carla/client/detail/EpisodeProxy.h"
#include "carla/rpc/ActorId.h"
#include <memory>
@ -19,16 +18,16 @@ namespace carla {
namespace client {
namespace detail {
class Client;
class Episode;
class EpisodeState;
class Simulator;
class WalkerNavigation
: public std::enable_shared_from_this<WalkerNavigation>,
private NonCopyable {
public:
explicit WalkerNavigation(Client & client);
explicit WalkerNavigation(std::weak_ptr<Simulator> simulator);
void RegisterWalker(ActorId walker_id, ActorId controller_id) {
// add to list
@ -91,7 +90,7 @@ namespace detail {
private:
Client &_client;
std::weak_ptr<Simulator> _simulator;
unsigned long _next_check_index;

4
LibCarla/source/carla/geom/Location.h
View File

@ -41,6 +41,10 @@ namespace geom {
// -- Other methods --------------------------------------------------------
// =========================================================================
auto DistanceSquared(const Location &loc) const {
return Math::DistanceSquared(*this, loc);
}
auto Distance(const Location &loc) const {
return Math::Distance(*this, loc);
}

7
LibCarla/source/carla/nav/Navigation.cpp
View File

@ -67,6 +67,13 @@ namespace nav {
dtFreeNavMesh(_nav_mesh);
}
// reference to the simulator to access API functions
void Navigation::SetSimulator(std::weak_ptr<carla::client::detail::Simulator> simulator)
{
_simulator = simulator;
_walker_manager.SetSimulator(simulator);
}
// set the seed to use with random numbers
void Navigation::SetSeed(unsigned int seed) {
srand(seed);

4
LibCarla/source/carla/nav/Navigation.h
View File

@ -71,6 +71,8 @@ namespace nav {
bool GetAgentRoute(ActorId id, carla::geom::Location from, carla::geom::Location to,
std::vector<carla::geom::Location> &path, std::vector<unsigned char> &area);
/// reference to the simulator to access API functions
void SetSimulator(std::weak_ptr<carla::client::detail::Simulator> simulator);
/// set the seed to use with random numbers
void SetSeed(unsigned int seed);
/// create the crowd object
@ -140,6 +142,8 @@ namespace nav {
/// walker manager for the route planning with events
WalkerManager _walker_manager;
std::weak_ptr<carla::client::detail::Simulator> _simulator;
mutable std::mutex _mutex;
float _probability_crossing { 0.0f };

23
LibCarla/source/carla/nav/WalkerEvent.cpp
View File

@ -4,10 +4,10 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/Logging.h"
#include "carla/nav/Navigation.h"
#include "carla/nav/WalkerManager.h"
#include "carla/nav/WalkerEvent.h"
#include "carla/rpc/TrafficLightState.h"
namespace carla {
namespace nav {
@ -30,12 +30,27 @@ namespace nav {
if (event.time <= 0.0) {
return EventResult::TimeOut;
} else {
// calculate the direction to look for vehicles
carla::geom::Location currentUnrealPos;
carla::geom::Location crosswalkEnd;
carla::geom::Location direction;
_manager->GetNavigation()->PauseAgent(_id, true);
_manager->GetNavigation()->GetWalkerPosition(_id, currentUnrealPos);
// check if we need to check for a trafficlight there (only the first time)
if (event.check_for_trafficlight) {
event.actor = _manager->GetTrafficLightAffecting(currentUnrealPos);
event.check_for_trafficlight = false;
}
// check if we need to wait for a trafficlight
if (event.actor) {
auto state = event.actor->GetState();
if (state == carla::rpc::TrafficLightState::Green ||
state == carla::rpc::TrafficLightState::Yellow) {
return EventResult::Continue;
}
}
_manager->GetNavigation()->PauseAgent(_id, false);
// calculate the direction to look for vehicles
carla::geom::Location crosswalkEnd;
_manager->GetWalkerCrosswalkEnd(_id, crosswalkEnd);
carla::geom::Location direction;
direction.x = crosswalkEnd.x - currentUnrealPos.x;
direction.y = crosswalkEnd.y - currentUnrealPos.y;
direction.z = crosswalkEnd.z - currentUnrealPos.z;

6
LibCarla/source/carla/nav/WalkerEvent.h
View File

@ -44,7 +44,11 @@ namespace nav {
/// event to pause and check for near vehicles
struct WalkerEventStopAndCheck {
double time;
WalkerEventStopAndCheck(double duration) : time(duration) {};
bool check_for_trafficlight;
SharedPtr<carla::client::TrafficLight> actor;
WalkerEventStopAndCheck(double duration) : time(duration),
check_for_trafficlight(true)
{};
};
/// walker event variant

69
LibCarla/source/carla/nav/WalkerManager.cpp
View File

@ -5,20 +5,38 @@
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/nav/WalkerManager.h"
#include "carla/logging.h"
#include "carla/client/ActorSnapshot.h"
#include "carla/client/Waypoint.h"
#include "carla/client/World.h"
#include "carla/client/detail/Simulator.h"
#include "carla/nav/Navigation.h"
#include "carla/rpc/Actor.h"
namespace carla {
namespace nav {
WalkerManager::WalkerManager() {
_nav = nullptr;
}
WalkerManager::~WalkerManager() {
}
// assign the navigation module
void WalkerManager::SetNav(Navigation *nav) {
_nav = nav;
}
// reference to the simulator to access API functions
void WalkerManager::SetSimulator(std::weak_ptr<carla::client::detail::Simulator> simulator) {
_simulator = simulator;
}
// create a new walker route
bool WalkerManager::AddWalker(ActorId id) {
GetAllTrafficLightWaypoints();
WalkerInfo info;
info.state = WALKER_IDLE;
@ -148,7 +166,7 @@ namespace nav {
case CARLA_AREA_CROSSWALK:
// only if we come from a safe area (sidewalks, grass or crosswalk)
if (previous_area != CARLA_AREA_CROSSWALK && previous_area != CARLA_AREA_ROAD)
info.route.emplace_back(WalkerEventStopAndCheck(5), std::move(path[i]), area[i]);
info.route.emplace_back(WalkerEventStopAndCheck(60), std::move(path[i]), area[i]);
break;
default:
@ -256,6 +274,53 @@ namespace nav {
return boost::variant2::visit(visitor, rp.event);
}
void WalkerManager::GetAllTrafficLightWaypoints() {
static bool AlreadyCalculated = false;
if (AlreadyCalculated) return;
// the world
carla::client::World world = _simulator.lock()->GetWorld();
_traffic_lights.clear();
std::vector<carla::rpc::Actor> actors = _simulator.lock()->GetAllTheActorsInTheEpisode();
for (auto actor : actors) {
carla::client::ActorSnapshot snapshot = _simulator.lock()->GetActorSnapshot(actor.id);
// check traffic lights only
if (actor.description.id == "traffic.traffic_light") {
// get the TL actor
SharedPtr<carla::client::TrafficLight> tl = boost::static_pointer_cast<carla::client::TrafficLight>(world.GetActor(actor.id));
// carla::logging::log("TL ", actor.id, " at (", snapshot.transform.location.x, ", ", snapshot.transform.location.y,", ", snapshot.transform.location.z, ") has status ", (uint8_t) snapshot.state.traffic_light_data.state);
// get the waypoints where the TL affects
std::vector<SharedPtr<carla::client::Waypoint>> list = tl->GetStopWaypoints();
for (auto &way : list) {
_traffic_lights.emplace_back(tl, way->GetTransform().location);
}
}
}
AlreadyCalculated = true;
}
// return the trafficlight affecting that position
SharedPtr<carla::client::TrafficLight> WalkerManager::GetTrafficLightAffecting(carla::geom::Location UnrealPos, float max_distance) {
float min_dist = std::numeric_limits<float>::infinity();
SharedPtr<carla::client::TrafficLight> actor;
for (auto &&item : _traffic_lights) {
float dist = UnrealPos.DistanceSquared(item.second);
if (dist < min_dist) {
min_dist = dist;
actor = item.first;
}
}
// if distance is not in the limit, then reject the trafficlight
if (max_distance < 0.0f || min_dist <= max_distance * max_distance) {
return actor;
} else {
return SharedPtr<carla::client::TrafficLight>();
}
}
} // namespace nav
} // namespace carla

15
LibCarla/source/carla/nav/WalkerManager.h
View File

@ -8,9 +8,12 @@
#include "carla/NonCopyable.h"
#include "carla/client/TrafficLight.h"
#include "carla/client/World.h"
#include "carla/geom/Location.h"
#include "carla/nav/WalkerEvent.h"
#include "carla/rpc/ActorId.h"
#include "carla/rpc/TrafficLightState.h"
namespace carla {
namespace nav {
@ -47,7 +50,10 @@ namespace nav {
~WalkerManager();
/// assign the navigation module
void SetNav(Navigation *nav) { _nav = nav; };
void SetNav(Navigation *nav);
/// reference to the simulator to access API functions
void SetSimulator(std::weak_ptr<carla::client::detail::Simulator> simulator);
/// create a new walker route
bool AddWalker(ActorId id);
@ -74,12 +80,19 @@ namespace nav {
/// return the navigation object
Navigation *GetNavigation() { return _nav; };
/// return the trafficlight affecting that position
SharedPtr<carla::client::TrafficLight> GetTrafficLightAffecting(carla::geom::Location UnrealPos, float max_distance = -1.0f);
private:
void GetAllTrafficLightWaypoints();
EventResult ExecuteEvent(ActorId id, WalkerInfo &info, double delta);
std::unordered_map<ActorId, WalkerInfo> _walkers;
std::vector<std::pair<SharedPtr<carla::client::TrafficLight>, carla::geom::Location>> _traffic_lights;
Navigation *_nav { nullptr };
std::weak_ptr<carla::client::detail::Simulator> _simulator;
};
} // namespace nav