329 lines
10 KiB
C++
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
|