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)) {
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) &&

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

@ -197,26 +197,40 @@ namespace MapConstants {
void InMemoryMap::FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint) {
WaypointPtr raw_waypoint = reference_waypoint->GetWaypoint();
uint8_t lane_change = static_cast<uint8_t>(raw_waypoint->GetLaneChange());
uint8_t change_right = static_cast<uint8_t>(carla::road::element::LaneMarking::LaneChange::Right);
uint8_t change_left = static_cast<uint8_t>(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();

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

@ -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<cc::Waypoint>;
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 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!");
}