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

Fixed lane change map sampling

This commit is contained in:
Praveen Kumar 2019-11-28 10:40:38 +05:30 committed by bernat
parent d1e0613d8f
commit b0b33f850d
4 changed files with 33 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
LibCarla/source/carla/trafficmanager/CollisionStage.cpp
View File

@ -121,9 +121,9 @@ namespace CollisionStageConstants {
(std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) { (std::abs(ego_location.z - other_location.z) < VERTICAL_OVERLAP_THRESHOLD)) {
debug_helper.DrawLine( debug_helper.DrawLine(
ego_actor->GetLocation() + cg::Location(0, 0, 2), ego_location + cg::Location(0, 0, 2),
actor->GetLocation() + cg::Location(0, 0, 2), 0.2, other_location + cg::Location(0, 0, 2),
{255u, 0u, 0u}, 0.1 0.2f, {255u, 0u, 0u}, 0.1f
); );
if (parameters.GetCollisionDetection(ego_actor, actor) && if (parameters.GetCollisionDetection(ego_actor, actor) &&

40
LibCarla/source/carla/trafficmanager/InMemoryMap.cpp
View File

@ -197,26 +197,40 @@ namespace MapConstants {
void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) { void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) {
WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint(); WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
uint8_t lane_change = static_cast<uint8_t>(raw_waypoint->GetLaneChange()); crd::element::LaneMarking::LaneChange lane_change = raw_waypoint->GetLaneChange();
uint8_t change_right = static_cast<uint8_t>(carla::road::element::LaneMarking::LaneChange::Right); auto change_right = crd::element::LaneMarking::LaneChange::Right;
uint8_t change_left = static_cast<uint8_t>(carla::road::element::LaneMarking::LaneChange::Left); auto change_left = crd::element::LaneMarking::LaneChange::Left;
auto change_both = crd::element::LaneMarking::LaneChange::Both;
try { try {
if ((lane_change & change_right) > 0) { if (lane_change == change_right || lane_change == change_both) {
WaypointPtr right_waypoint = raw_waypoint->GetRight(); WaypointPtr right_waypoint = raw_waypoint->GetRight();
SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(right_waypoint->GetTransform().location); if (right_waypoint != nullptr &&
if (closest_simple_waypoint == nullptr) { right_waypoint->GetType() == crd::Lane::LaneType::Driving &&
closest_simple_waypoint = GetWaypoint(right_waypoint->GetTransform().location); (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(); WaypointPtr left_waypoint = raw_waypoint->GetLeft();
SimpleWaypointPtr closest_simple_waypoint = GetWaypointInVicinity(left_waypoint->GetTransform().location); if (left_waypoint != nullptr &&
if (closest_simple_waypoint == nullptr) { left_waypoint->GetType() == crd::Lane::LaneType::Driving &&
closest_simple_waypoint = GetWaypoint(left_waypoint->GetTransform().location); (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) { } catch (const std::invalid_argument &e) {
cg::Location loc = reference_waypoint->GetLocation(); cg::Location loc = reference_waypoint->GetLocation();

2
LibCarla/source/carla/trafficmanager/InMemoryMap.h
View File

@ -11,6 +11,7 @@
#include "carla/geom/Location.h" #include "carla/geom/Location.h"
#include "carla/geom/Math.h" #include "carla/geom/Math.h"
#include "carla/Memory.h" #include "carla/Memory.h"
#include "carla/road/Lane.h"
#include "carla/trafficmanager/SimpleWaypoint.h" #include "carla/trafficmanager/SimpleWaypoint.h"
@ -18,6 +19,7 @@ namespace traffic_manager {
namespace cg = carla::geom; namespace cg = carla::geom;
namespace cc = carla::client; namespace cc = carla::client;
namespace crd = carla::road;
using WaypointPtr = carla::SharedPtr<cc::Waypoint>; using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>; using TopologyList = std::vector<std::pair<WaypointPtr, WaypointPtr>>;

2
LibCarla/source/carla/trafficmanager/SimpleWaypoint.cpp
View File

@ -58,7 +58,7 @@ namespace traffic_manager {
cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector(); cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation(); cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0) { if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0) {
next_left_waypoint = _waypoint; next_right_waypoint = _waypoint;
} else { } else {
throw std::invalid_argument("Argument not on the right side!"); throw std::invalid_argument("Argument not on the right side!");
} }