libcarla/include/carla/nav/WalkerManager.cpp
2024-10-18 13:19:59 +08:00

329 lines
10 KiB
C++

// Copyright (c) 2019 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/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() {
}
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;
// save it
_walkers[id] = std::move(info);
return true;
}
// remove a walker route
bool WalkerManager::RemoveWalker(ActorId id) {
// search
auto it = _walkers.find(id);
if (it == _walkers.end())
return false;
_walkers.erase(it);
return true;
}
// update all routes
bool WalkerManager::Update(double delta) {
// check all walkers
for (auto &it : _walkers) {
// get the elements
WalkerInfo &info = it.second;
// in function of the state
switch (info.state) {
case WALKER_IDLE:
break;
case WALKER_WALKING:
{
// get the target point
carla::geom::Location &target = info.route[info.currentIndex].location;
// get current position
carla::geom::Location current;
_nav->GetWalkerPosition(it.first, current);
// check distance to the target point
carla::geom::Vector3D dist(target.x - current.x, target.z - current.z, target.y - current.y);
if (dist.SquaredLength() <= 1) {
info.state = WALKER_IN_EVENT;
}
}
break;
case WALKER_IN_EVENT:
switch (ExecuteEvent(it.first, it.second, delta)) {
case EventResult::Continue:
break;
case EventResult::End:
// next point in route
SetWalkerNextPoint(it.first);
break;
case EventResult::TimeOut:
// unblock changing the route
SetWalkerRoute(it.first);
break;
}
break;
case WALKER_STOP:
info.state = WALKER_IDLE;
break;
}
}
return true;
}
// set a new route from its current position
bool WalkerManager::SetWalkerRoute(ActorId id) {
// check
if (_nav == nullptr)
return false;
// set a new random target
carla::geom::Location location;
_nav->GetRandomLocation(location, nullptr);
// set the route
return SetWalkerRoute(id, location);
}
// set a new route from its current position
bool WalkerManager::SetWalkerRoute(ActorId id, carla::geom::Location to) {
// check
if (_nav == nullptr)
return false;
// search
auto it = _walkers.find(id);
if (it == _walkers.end())
return false;
// get it
WalkerInfo &info = it->second;
std::vector<carla::geom::Location> path;
std::vector<unsigned char> area;
// save both points for the route
_nav->GetWalkerPosition(id, info.from);
info.to = to;
info.currentIndex = 0;
info.state = WALKER_IDLE;
// get a route from navigation
_nav->GetAgentRoute(id, info.from, to, path, area);
// create each point of the route
info.route.clear();
info.route.reserve(path.size());
unsigned char previous_area = CARLA_AREA_SIDEWALK;
for (unsigned int i=0; i<path.size(); ++i) {
// get the type
switch (area[i]) {
// do nothing
case CARLA_AREA_SIDEWALK:
info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
break;
// stop and check
case CARLA_AREA_ROAD:
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(60), std::move(path[i]), area[i]);
break;
default:
info.route.emplace_back(WalkerEventIgnore(), std::move(path[i]), area[i]);
}
previous_area = area[i];
}
// assign the first point to go (second in the list)
SetWalkerNextPoint(id);
return true;
}
// set the next point in the route
bool WalkerManager::SetWalkerNextPoint(ActorId id) {
// check
if (_nav == nullptr)
return false;
// search
auto it = _walkers.find(id);
if (it == _walkers.end())
return false;
// get it
WalkerInfo &info = it->second;
// advance next point
++info.currentIndex;
// check the end
if (info.currentIndex < info.route.size()) {
// change the state
info.state = WALKER_WALKING;
// assign the point to go
_nav->PauseAgent(id, false);
_nav->SetWalkerDirectTarget(id, info.route[info.currentIndex].location);
} else {
// change the state
info.state = WALKER_STOP;
_nav->PauseAgent(id, true);
// we need a new route from here
SetWalkerRoute(id);
}
return true;
}
// get the next point in the route
bool WalkerManager::GetWalkerNextPoint(ActorId id, carla::geom::Location &location) {
// check
if (_nav == nullptr)
return false;
// search
auto it = _walkers.find(id);
if (it == _walkers.end())
return false;
// get it
WalkerInfo &info = it->second;
// check the end
if (info.currentIndex < info.route.size()) {
location = info.route[info.currentIndex].location;
return true;
} else {
return false;
}
}
bool WalkerManager::GetWalkerCrosswalkEnd(ActorId id, carla::geom::Location &location) {
// check
if (_nav == nullptr)
return false;
// search
auto it = _walkers.find(id);
if (it == _walkers.end())
return false;
// get it
WalkerInfo &info = it->second;
// check the end of current crosswalk
unsigned int pos = info.currentIndex;
while (pos < info.route.size()) {
if (info.route[pos].areaType != CARLA_AREA_CROSSWALK) {
location = info.route[pos].location;
return true;
}
++pos;
}
return false;
}
EventResult WalkerManager::ExecuteEvent(ActorId id, WalkerInfo &info, double delta) {
// go to the event
WalkerRoutePoint &rp = info.route[info.currentIndex];
// build the visitor structure
WalkerEventVisitor visitor(this, id, delta);
// run the event
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));
// 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