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

Implemented changes to account for corners 

of unregistered vehicles during localization.
This commit is contained in:
Praveen Kumar 2020-04-08 11:25:49 +05:30 committed by Jacopo Bartiromo
parent a25842d924
commit 8c7ac56f8f
3 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -607,17 +607,29 @@ namespace LocalizationConstants {
cg::Location location = it->second->GetLocation();
const auto type = it->second->GetTypeId();
SimpleWaypointPtr nearest_waypoint = nullptr;
std::vector<SimpleWaypointPtr> nearest_waypoints;
if (type[0] == 'v') {
nearest_waypoint = local_map.GetWaypointInVicinity(location);
auto vehicle_ptr = boost::static_pointer_cast<cc::Vehicle>(it->second);
cg::Vector3D extent = vehicle_ptr->GetBoundingBox().extent;
float bx = extent.x;
float by = extent.y;
std::vector<cg::Location> corners = {location + cg::Location(bx, by, 0.0f),
location + cg::Location(-bx, by, 0.0f),
location + cg::Location(bx, -by, 0.0f),
location + cg::Location(-bx, -by, 0.0f)};
for (cg::Location &vertex: corners) {
SimpleWaypointPtr nearest_waypoint = local_map.GetWaypointInVicinity(vertex);
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetPedWaypoint(vertex);};
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);};
nearest_waypoints.push_back(nearest_waypoint);
}
} else if (type[0] == 'w') {
nearest_waypoint = local_map.GetPedWaypoint(location);
}
if (nearest_waypoint == nullptr) {
nearest_waypoint = local_map.GetWaypoint(location);
SimpleWaypointPtr nearest_waypoint = local_map.GetPedWaypoint(location);
if (nearest_waypoint == nullptr) {nearest_waypoint = local_map.GetWaypoint(location);};
nearest_waypoints.push_back(nearest_waypoint);
}
track_traffic.UpdateUnregisteredGridPosition(it->first, nearest_waypoint);
track_traffic.UpdateUnregisteredGridPosition(it->first, nearest_waypoints);
++it;
}

5
LibCarla/source/carla/trafficmanager/LocalizationUtils.cpp
View File

@ -62,7 +62,8 @@ namespace LocalizationConstants {
TrackTraffic::TrackTraffic() {}
void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint) {
void TrackTraffic::UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints) {
// Add actor entry, if not present.
if (actor_to_grids.find(actor_id) == actor_to_grids.end()) {
@ -85,7 +86,7 @@ namespace LocalizationConstants {
current_grids.clear();
// Step through buffer and update grid list for actor and actor list for grids.
if (waypoint != nullptr) {
for (auto &waypoint: waypoints) {
GeoGridId ggid = waypoint->GetGeodesicGridId();
current_grids.insert(ggid);

3
LibCarla/source/carla/trafficmanager/LocalizationUtils.h
View File

@ -50,7 +50,8 @@ namespace traffic_manager {
ActorIdSet GetPassingVehicles(uint64_t waypoint_id);
void UpdateGridPosition(const ActorId actor_id, const Buffer& buffer);
void UpdateUnregisteredGridPosition(const ActorId actor_id, const SimpleWaypointPtr& waypoint);
void UpdateUnregisteredGridPosition(const ActorId actor_id,
const std::vector<SimpleWaypointPtr> waypoints);
/// Method to return the wayPoints from the waypoint Buffer by using target point distance
std::pair<SimpleWaypointPtr,uint64_t> GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance);