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

Adding null checks in LocalizationStage::ExtendAndFindSafeSpace

This commit is contained in:
Praveen Kumar 2020-10-20 18:42:14 +05:30 committed by bernat
parent a90956ad0c
commit 7f6c2b3594
1 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
LibCarla/source/carla/trafficmanager/LocalizationStage.cpp
View File

@ -234,16 +234,23 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
// Extend buffer if safe point not found.
if (!safe_point_found) {
while (!past_junction) {
current_waypoint = current_waypoint->GetNextWaypoint().front();
PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
if (!current_waypoint->CheckJunction()) {
past_junction = true;
junction_end_point = current_waypoint;
bool abort = false;
while (!past_junction && !abort) {
NodeList next_waypoints = current_waypoint->GetNextWaypoint();
if (!next_waypoints.empty()) {
current_waypoint = next_waypoints.front();
PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
if (!current_waypoint->CheckJunction()) {
past_junction = true;
junction_end_point = current_waypoint;
}
} else {
abort = true;
}
}
while (!safe_point_found) {
while (!safe_point_found && !abort) {
std::vector<SimpleWaypointPtr> next_waypoints = current_waypoint->GetNextWaypoint();
if ((junction_end_point->DistanceSquared(current_waypoint) > safe_distance_squared)
|| next_waypoints.size() > 1
@ -252,13 +259,20 @@ void LocalizationStage::ExtendAndFindSafeSpace(const ActorId actor_id,
safe_point_found = true;
safe_point_after_junction = current_waypoint;
} else {
current_waypoint = next_waypoints.front();
PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
if (!next_waypoints.empty()) {
current_waypoint = next_waypoints.front();
PushWaypoint(actor_id, track_traffic, waypoint_buffer, current_waypoint);
} else {
abort = true;
}
}
}
}
if (junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) {
if (junction_end_point != nullptr &&
safe_point_after_junction != nullptr &&
junction_begin_point->DistanceSquared(junction_end_point) < SQUARE(MIN_JUNCTION_LENGTH)) {
junction_end_point = nullptr;
safe_point_after_junction = nullptr;
}