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

Debug buffer oscillation

This commit is contained in:
Praveen Kumar 2020-03-03 16:33:47 +05:30 committed by Marc Garcia Puig
parent 5616dd005c
commit 8ce51d73e3
1 changed files with 17 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -179,19 +179,6 @@ namespace LocalizationConstants {
PushWaypoint(waypoint_buffer, actor_id, next_wp);
}
///////////////////////////////// DEBUG //////////////////////////////////
uint b_size = static_cast<uint32_t>(waypoint_buffer.size());
uint b_step = 5;
uint b_step_size = b_size/b_step;
for (uint j = 0u; j < b_step; ++j) {
auto first = waypoint_buffer.at(j * b_step_size);
auto second = waypoint_buffer.at(std::min((j+1) * b_step_size, b_size-1));
debug_helper.DrawLine(first->GetLocation() + cg::Location(0, 0, 1),
second->GetLocation() + cg::Location(0, 0, 1),
0.2f, {255u, 255u, 0u}, 0.1f);
}
//////////////////////////////////////////////////////////////////////////
// Updating geodesic grid position for actor.
track_traffic.UpdateGridPosition(actor_id, waypoint_buffer);
@ -214,6 +201,23 @@ namespace LocalizationConstants {
dot_product *= -1.0f;
}
///////////////////////////////// DEBUG //////////////////////////////////
debug_helper.DrawArrow(vehicle_location + cg::Location(0, 0, 1),
target_location + cg::Location(0, 0, 1),
0.2f, 0.2f, {0u, 255u, 255u}, 0.1f);
uint b_size = static_cast<uint32_t>(waypoint_buffer.size());
uint b_step = 5;
uint b_step_size = b_size/b_step;
for (uint j = 0u; j < b_step; ++j) {
auto first = waypoint_buffer.at(j * b_step_size);
auto second = waypoint_buffer.at(std::min((j+1) * b_step_size, b_size-1));
debug_helper.DrawLine(first->GetLocation() + cg::Location(0, 0, 1),
second->GetLocation() + cg::Location(0, 0, 1),
0.2f, {255u, 255u, 0u}, 0.1f);
}
//////////////////////////////////////////////////////////////////////////
float distance = 0.0f; // TODO: use in PID
// Filtering out false junctions on highways: