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

Added more checks on code, and removed some logs

This commit is contained in:
bernatx 2019-06-20 10:24:07 +02:00 committed by Néstor Subirón
parent a3b7594ef8
commit 38fef38f62
3 changed files with 120 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,6 +54,7 @@ namespace nav {
}
Navigation::~Navigation() {
_ready = false;
_mappedId.clear();
_baseHeight.clear();
_yaw_walkers.clear();
@ -96,6 +97,12 @@ namespace nav {
};
#pragma pack(pop)
// check size for header
if (content.size() < sizeof(header)) {
logging::log("Nav: failed loading binary");
return false;
}
// read the file header
unsigned long pos = 0;
memcpy(&header, &content[pos], sizeof(header));
@ -161,23 +168,30 @@ namespace nav {
_navQuery = dtAllocNavMeshQuery();
_navQuery->init(_navMesh, MAX_QUERY_SEARCH_NODES);
// create and init the crowd manager
CreateCrowd();
// copy
_binaryMesh = content;
_ready = true;
// create and init the crowd manager
CreateCrowd();
return true;
}
void Navigation::CreateCrowd(void) {
if (_crowd != nullptr)
// check if all is ready
if (!_ready)
return;
DEBUG_ASSERT(_crowd != nullptr);
// create and init
_crowd = dtAllocCrowd();
_crowd->init(MAX_AGENTS, AGENT_RADIUS, _navMesh);
if (!_crowd->init(MAX_AGENTS, AGENT_RADIUS, _navMesh)) {
logging::log("Nav: failed to create crowd");
return;
}
// make polygons with 'disabled' flag invalid
_crowd->getEditableFilter(0)->setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
@ -229,11 +243,16 @@ namespace nav {
dtPolyRef m_polys[MAX_POLYS];
int m_npolys;
// check to load the binary _navMesh from server
if (_binaryMesh.size() == 0) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_navQuery != nullptr);
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// point extension
float m_polyPickExt[3];
m_polyPickExt[0] = 2;
@ -294,10 +313,13 @@ namespace nav {
bool Navigation::AddWalker(ActorId id, carla::geom::Location from, float base_offset) {
dtCrowdAgentParams params;
if (_crowd == nullptr) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_crowd != nullptr);
// set parameters
memset(&params, 0, sizeof(params));
params.radius = AGENT_RADIUS;
@ -317,6 +339,8 @@ namespace nav {
params.obstacleAvoidanceType = 3;
params.separationWeight = 0.5f;
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// from Unreal coordinates
float PointFrom[3] = { from.x, from.z, from.y };
@ -339,11 +363,22 @@ namespace nav {
// remove a walker
bool Navigation::RemoveWalker(ActorId id) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_crowd != nullptr);
// get the internal index
auto it = _mappedId.find(id);
if (it == _mappedId.end())
return false;
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// remove from crowd
_crowd->removeAgent(it->second);
@ -355,11 +390,22 @@ namespace nav {
// set new max speed
bool Navigation::SetWalkerMaxSpeed(ActorId id, float max_speed) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_crowd != nullptr);
// get the internal index
auto it = _mappedId.find(id);
if (it == _mappedId.end())
return false;
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// get the agent
dtCrowdAgent *agent = _crowd->getEditableAgent(it->second);
if (agent) {
@ -372,6 +418,12 @@ namespace nav {
// set a new target point to go
bool Navigation::SetWalkerTarget(ActorId id, carla::geom::Location to) {
// check if all is ready
if (!_ready) {
return false;
}
// get the internal index
auto it = _mappedId.find(id);
if (it == _mappedId.end())
@ -381,10 +433,24 @@ namespace nav {
}
// set a new target point to go
bool Navigation::SetWalkerTargetIndex(int index, carla::geom::Location to) {
bool Navigation::SetWalkerTargetIndex(int index, carla::geom::Location to, bool use_lock) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_crowd != nullptr);
DEBUG_ASSERT(_navQuery != nullptr);
if (index == -1)
return false;
if (use_lock) {
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
}
// set target position
float pointTo[3] = { to.x, to.z, to.y };
float nearest[3];
@ -400,10 +466,13 @@ namespace nav {
// update all walkers in crowd
void Navigation::UpdateCrowd(const client::detail::EpisodeState &state) {
if (!_navMesh || !_crowd) {
// check if all is ready
if (!_ready) {
return;
}
DEBUG_ASSERT(_crowd != nullptr);
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
@ -424,8 +493,8 @@ namespace nav {
if (dist.SquaredLength() <= 2) {
// set a new random target
carla::geom::Location location;
GetRandomLocationWithoutLock(location, 1);
SetWalkerTargetIndex(i, location);
GetRandomLocation(location, 1, nullptr, false);
SetWalkerTargetIndex(i, location, false);
}
}
}
@ -433,6 +502,13 @@ namespace nav {
// get the walker current transform
bool Navigation::GetWalkerTransform(ActorId id, carla::geom::Transform &trans) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_crowd != nullptr);
// get the internal index
auto it = _mappedId.find(id);
if (it == _mappedId.end())
@ -443,6 +519,9 @@ namespace nav {
if (index == -1)
return false;
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// get the walker
const dtCrowdAgent *agent = _crowd->getAgent(index);
@ -469,34 +548,48 @@ namespace nav {
trans.rotation.yaw = _yaw_walkers[id] + (shortest_angle * rotation_speed * static_cast<float>(_delta_seconds));
_yaw_walkers[id] = trans.rotation.yaw;
return true;
}
float Navigation::GetWalkerSpeed(ActorId id) {
// check if all is ready
if (!_ready) {
return 0.0f;
}
DEBUG_ASSERT(_crowd != nullptr);
// get the internal index
auto it = _mappedId.find(id);
if (it == _mappedId.end()) {
return false;
return 0.0f;
}
// get the index found
int index = it->second;
if (index == -1) {
return false;
return 0.0f;
}
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
// get the walker
const dtCrowdAgent *agent = _crowd->getAgent(index);
return sqrt(agent->vel[0] * agent->vel[0] + agent->vel[1] * agent->vel[1] + agent->vel[2] *
agent->vel[2]);
}
bool Navigation::GetRandomLocation(carla::geom::Location &location, float maxHeight, dtQueryFilter *filter) {
std::lock_guard<std::mutex> lock(_mutex);
return GetRandomLocationWithoutLock(location, maxHeight, filter);
}
// get a random location for navigation
bool Navigation::GetRandomLocationWithoutLock(carla::geom::Location &location, float maxHeight, dtQueryFilter *filter) {
bool Navigation::GetRandomLocation(carla::geom::Location &location, float maxHeight, dtQueryFilter *filter, bool use_lock) {
// check if all is ready
if (!_ready) {
return false;
}
DEBUG_ASSERT(_navQuery != nullptr);
// filter
@ -507,6 +600,11 @@ namespace nav {
filter = &filter2;
}
if (use_lock) {
// force single thread running this
std::lock_guard<std::mutex> lock(_mutex);
}
// search
dtPolyRef randomRef { 0 };
float point[3] { 0.0f, 0.0f, 0.0f };

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

@ -44,7 +44,7 @@ namespace nav {
bool SetWalkerMaxSpeed(ActorId id, float max_speed);
// set a new target point to go
bool SetWalkerTarget(ActorId id, carla::geom::Location to);
bool SetWalkerTargetIndex(int index, carla::geom::Location to);
bool SetWalkerTargetIndex(int index, carla::geom::Location to, bool use_lock = true);
// get the walker current transform
bool GetWalkerTransform(ActorId id, carla::geom::Transform &trans);
// get the walker current transform
@ -52,10 +52,10 @@ namespace nav {
// update all walkers in crowd
void UpdateCrowd(const client::detail::EpisodeState &state);
// get a random location for navigation
bool GetRandomLocationWithoutLock(carla::geom::Location &location, float maxHeight = -1.0f, dtQueryFilter *filter = nullptr);
bool GetRandomLocation(carla::geom::Location &location, float maxHeight = -1.0f, dtQueryFilter *filter = nullptr);
bool GetRandomLocation(carla::geom::Location &location, float maxHeight = -1.0f, dtQueryFilter *filter = nullptr, bool use_lock = true);
private:
bool _ready { false };
std::vector<uint8_t> _binaryMesh;
double _delta_seconds;
// meshes

2
PythonAPI/examples/spawn_npc.py
View File

@ -124,7 +124,6 @@ def main():
print('spawned %d vehicles, press Ctrl+C to exit.' % len(actor_list))
# Spawn Walkers
print (args.number_of_walkers)
spawn_points = []
for i in range(args.number_of_walkers):
spawn_point = carla.Transform()
@ -163,7 +162,6 @@ def main():
all_id.append(info[i]["con"])
all_id.append(info[i]["id"])
all_actors = world.get_actors(all_id)
# initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
for i in range(0, len(all_id), 2):
# index in the info list