From b0b33f850d8eae9951ba833d82db4bfa0236acd9 Mon Sep 17 00:00:00 2001 From: Praveen Kumar Date: Thu, 28 Nov 2019 10:40:38 +0530 Subject: [PATCH] Fixed lane change map sampling --- .../carla/trafficmanager/CollisionStage.cpp | 6 +-- .../carla/trafficmanager/InMemoryMap.cpp | 40 +++++++++++++------ .../source/carla/trafficmanager/InMemoryMap.h | 2 + .../carla/trafficmanager/SimpleWaypoint.cpp | 2 +- 4 files changed, 33 insertions(+), 17 deletions(-) diff --git a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp index b0a546c96..bfa2526b1 100644 --- a/LibCarla/source/carla/trafficmanager/CollisionStage.cpp +++ b/LibCarla/source/carla/trafficmanager/CollisionStage.cpp @@ -121,9 +121,9 @@ namespace CollisionStageConstants { (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { debug_helper.DrawLine( - ego_actor->GetLocation() + cg::Location(0, 0, 2), - actor->GetLocation() + cg::Location(0, 0, 2), 0.2, - {255u, 0u, 0u}, 0.1 + ego_location + cg::Location(0, 0, 2), + other_location + cg::Location(0, 0, 2), + 0.2f, {255u, 0u, 0u}, 0.1f ); if (parameters.GetCollisionDetection(ego_actor, actor) && diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp index b523e25ba..43c11bc6a 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.cpp @@ -197,26 +197,40 @@ namespace MapConstants { void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) { WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint(); - uint8_t lane_change = static_cast(raw_waypoint->GetLaneChange()); - uint8_t change_right = static_cast(carla::road::element::LaneMarking::LaneChange::Right); - uint8_t change_left = static_cast(carla::road::element::LaneMarking::LaneChange::Left); + crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange(); + auto change_right = crd::element::LaneMarking::LaneChange::Right; + auto change_left = crd::element::LaneMarking::LaneChange::Left; + auto change_both = crd::element::LaneMarking::LaneChange::Both; try { - if ((lane_change & change_right) > 0) { + if (lane_change == change_right || lane_change == change_both) { + WaypointPtr right_waypoint = raw_waypoint->GetRight(); - SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(right_waypoint->GetTransform().location); - if (closest_simple_waypoint == nullptr) { - closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location); + if (right_waypoint != nullptr && + right_waypoint->GetType() == crd::Lane::LaneType::Driving && + (right_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { + + SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(right_waypoint->GetTransform().location); + if (closest_simple_waypoint == nullptr) { + closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location); + } + reference_waypoint->SetRightWaypoint(closest_simple_waypoint); } - reference_waypoint->SetRightWaypoint(closest_simple_waypoint); } - if ((lane_change & change_left) > 0) { + + if (lane_change == change_left || lane_change == change_both) { + WaypointPtr left_waypoint = raw_waypoint->GetLeft(); - SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(left_waypoint->GetTransform().location); - if (closest_simple_waypoint == nullptr) { - closest_simple_waypoint = GetWaypoint(left_waypoint->GetTransform().location); + if (left_waypoint != nullptr && + left_waypoint->GetType() == crd::Lane::LaneType::Driving && + (left_waypoint->GetLaneId() * raw_waypoint->GetLaneId() > 0)) { + + SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(left_waypoint->GetTransform().location); + if (closest_simple_waypoint == nullptr) { + closest_simple_waypoint = GetWaypoint(left_waypoint->GetTransform().location); + } + reference_waypoint->SetLeftWaypoint(closest_simple_waypoint); } - reference_waypoint->SetLeftWaypoint(closest_simple_waypoint); } } catch (const std::invalid_argument &e) { cg::Location loc = reference_waypoint->GetLocation(); diff --git a/LibCarla/source/carla/trafficmanager/InMemoryMap.h b/LibCarla/source/carla/trafficmanager/InMemoryMap.h index 4a9978bee..80a397e23 100644 --- a/LibCarla/source/carla/trafficmanager/InMemoryMap.h +++ b/LibCarla/source/carla/trafficmanager/InMemoryMap.h @@ -11,6 +11,7 @@ #include "carla/geom/Location.h" #include "carla/geom/Math.h" #include "carla/Memory.h" +#include "carla/road/Lane.h" #include "carla/trafficmanager/SimpleWaypoint.h" @@ -18,6 +19,7 @@ namespace traffic_manager { namespace cg = carla::geom; namespace cc = carla::client; +namespace crd = carla::road; using WaypointPtr = carla::SharedPtr; using TopologyList = std::vector>; diff --git a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp index 91002cc99..2e3e1eb57 100644 --- a/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp +++ b/LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp @@ -58,7 +58,7 @@ namespace traffic_manager { cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0) { - next_left_waypoint = _waypoint; + next_right_waypoint = _waypoint; } else { throw std::invalid_argument("Argument not on the right side!"); }