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

Fixed lane 0 and rtree issues.

This commit is contained in:
Axel 2020-03-05 11:06:32 +01:00 committed by Marc Garcia Puig
parent 8509511e52
commit 90e342b21e
2 changed files with 96 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
LibCarla/source/carla/road/Map.cpp
View File

@ -69,6 +69,9 @@ namespace road {
FuncT &&func) { FuncT &&func) {
for (const auto &pair : lane_section.GetLanes()) { for (const auto &pair : lane_section.GetLanes()) {
const auto &lane = pair.second; const auto &lane = pair.second;
if (lane.GetId() == 0) {
continue;
}
if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) { if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) {
std::forward<FuncT>(func)(Waypoint{ std::forward<FuncT>(func)(Waypoint{
road_id, road_id,
@ -88,6 +91,9 @@ namespace road {
FuncT &&func) { FuncT &&func) {
for (const auto &pair : lane_section.GetLanes()) { for (const auto &pair : lane_section.GetLanes()) {
const auto &lane = pair.second; const auto &lane = pair.second;
if (lane.GetId() == 0) {
continue;
}
if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(lane_type)) > 0) { if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
std::forward<FuncT>(func)(Waypoint{ std::forward<FuncT>(func)(Waypoint{
road_id, road_id,
@ -110,7 +116,7 @@ namespace road {
} }
} }
/// Return a waypoint for each drivable lane of the specified type on each lane section of @a road. /// Return a waypoint for each lane of the specified type on each lane section of @a road.
template <typename FuncT> template <typename FuncT>
static void ForEachLane(const Road &road, Lane::LaneType lane_type, FuncT &&func) { static void ForEachLane(const Road &road, Lane::LaneType lane_type, FuncT &&func) {
for (const auto &lane_section : road.GetLaneSections()) { for (const auto &lane_section : road.GetLaneSections()) {
@ -706,7 +712,7 @@ namespace road {
return result; return result;
} }
std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology() const { std::vector<std::pair<Waypoint, Waypoint>> Map::GenerateTopology(Lane::LaneType /*lane_type*/) const {
std::vector<std::pair<Waypoint, Waypoint>> result; std::vector<std::pair<Waypoint, Waypoint>> result;
for (const auto &pair : _data.GetRoads()) { for (const auto &pair : _data.GetRoads()) {
const auto &road = pair.second; const auto &road = pair.second;
@ -744,10 +750,18 @@ namespace road {
// =========================================================================== // ===========================================================================
// Checks whether the geometry is straight or not // Checks whether the geometry is straight or not
bool IsLineStraight(const Road &road, const Lane &lane, element::GeometryType geometry_type) { bool IsLineStraight(const Road &road,
const Lane &lane,
const element::RoadInfoGeometry* geometry) {
auto geometry_type = geometry->GetGeometry().GetType();
if (geometry_type != element::GeometryType::LINE) { if (geometry_type != element::GeometryType::LINE) {
return false; return false;
} }
if(lane.GetDistance() < geometry->GetDistance() ||
lane.GetDistance() + lane.GetLength() >
geometry->GetDistance() + geometry->GetGeometry().GetLength()) {
return false;
}
auto lane_offsets = lane.GetInfos<element::RoadInfoLaneOffset>(); auto lane_offsets = lane.GetInfos<element::RoadInfoLaneOffset>();
for (auto *lane_offset : lane_offsets) { for (auto *lane_offset : lane_offsets) {
if (abs(lane_offset->GetPolynomial().GetC()) > 0 || if (abs(lane_offset->GetPolynomial().GetC()) > 0 ||
@ -803,15 +817,11 @@ namespace road {
// returns the remaining length of the geometry depending on the lane // returns the remaining length of the geometry depending on the lane
// direction // direction
double GetRemainingLength( double GetRemainingLength(const Lane &lane, double current_s) {
const Lane &lane,
double geometry_start_s,
double geometry_end_s,
double current_s) {
if (lane.GetId() < 0) { if (lane.GetId() < 0) {
return (geometry_end_s - current_s); return (lane.GetDistance() + lane.GetLength() - current_s);
} else { } else {
return (current_s - geometry_start_s); return (current_s - lane.GetDistance());
} }
} }
@ -823,108 +833,104 @@ namespace road {
// 1.8 degrees, maximum angle in a curve to place a segment // 1.8 degrees, maximum angle in a curve to place a segment
constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0; constexpr double angle_threshold = geom::Math::Pi<double>() / 100.0;
// Generate waypoints at start of every road and for everey lane // Generate waypoints at start of every lane
std::vector<Waypoint> topology = GenerateWaypointsOnRoadEntries(Lane::LaneType::Any); std::vector<Waypoint> topology;
for (const auto &pair : _data.GetRoads()) {
const auto &road = pair.second;
ForEachLane(road, Lane::LaneType::Any, [&](auto &&waypoint) {
if(waypoint.lane_id != 0) {
topology.push_back(waypoint);
}
});
}
std::vector<Rtree::TreeElement> rtree_elements; // container of segments and std::vector<Rtree::TreeElement> rtree_elements; // container of segments and
// waypoints // waypoints
// Loop through all road lanes // Loop through all lanes
for (auto &waypoint : topology) { for (auto &waypoint : topology) {
auto &lane_start_waypoint = waypoint; auto &lane_start_waypoint = waypoint;
auto current_waypoint = lane_start_waypoint; auto current_waypoint = lane_start_waypoint;
const Road &road = _data.GetRoad(current_waypoint.road_id); const Road &road = _data.GetRoad(current_waypoint.road_id);
const Lane &lane = GetLane(current_waypoint);
const auto *geometry = road.GetInfo<element::RoadInfoGeometry>(current_waypoint.s);
// loop through every geometry in the road geom::Transform current_transform = ComputeTransform(current_waypoint);
while (current_waypoint.s <= road.GetLength() && current_waypoint.s >= 0) {
const Lane &lane = GetLane(current_waypoint);
const auto *geometry = road.GetInfo<element::RoadInfoGeometry>(current_waypoint.s);
// start s of this geometry in the road
double geometry_start_s = geometry->GetGeometry().GetStartOffset();
// end s this geometry in the road
double geometry_end_s = geometry_start_s + geometry->GetGeometry().GetLength();
geom::Transform current_transform = ComputeTransform(current_waypoint); // Save computation time in straight lines
if (IsLineStraight(road, lane, geometry)) {
double delta_s = min_delta_s;
double remaining_length =
GetRemainingLength(lane, current_waypoint.s);
remaining_length -= epsilon;
delta_s = remaining_length;
if (delta_s < epsilon) {
continue;
}
auto next = GetNext(current_waypoint, delta_s);
// Save computation time in straight lines RELEASE_ASSERT(next.size() == 1);
if (IsLineStraight(road, lane, geometry->GetGeometry().GetType())) { RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
auto next_waypoint = next.front();
AddElementToRtreeAndUpdateTransforms(
rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
// end of lane
} else {
auto next_waypoint = current_waypoint;
// Loop until the end of the lane
// Advance in small s-increments
while (true) {
double delta_s = min_delta_s; double delta_s = min_delta_s;
double remaining_length = double remaining_length =
GetRemainingLength(lane, geometry_start_s, geometry_end_s, current_waypoint.s); GetRemainingLength(lane, next_waypoint.s);
remaining_length -= epsilon; remaining_length -= epsilon;
delta_s = remaining_length; delta_s = std::min(delta_s, remaining_length);
if (delta_s < epsilon) { if (delta_s < epsilon) {
AddElementToRtreeAndUpdateTransforms(
rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
break; break;
} }
auto next = GetNext(current_waypoint, delta_s);
RELEASE_ASSERT(next.size() == 1);
RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
auto next_waypoint = next.front();
AddElementToRtreeAndUpdateTransforms( auto next = GetNext(next_waypoint, delta_s);
rtree_elements, if (next.size() != 1 ||
current_transform, current_waypoint.section_id != next.front().section_id) {
current_waypoint, AddElementToRtreeAndUpdateTransforms(
next_waypoint); rtree_elements,
} else { current_transform,
auto next_waypoint = current_waypoint; current_waypoint,
// Loop until the end of the geometry next_waypoint);
while (true) { break;
double delta_s = min_delta_s; }
double remaining_length =
GetRemainingLength( next_waypoint = next.front();
lane, geom::Transform next_transform = ComputeTransform(next_waypoint);
geometry_start_s, double angle = geom::Math::GetVectorAngle(
geometry_end_s, current_transform.GetForwardVector(), next_transform.GetForwardVector());
next_waypoint.s);
remaining_length -= epsilon; if (abs(angle) > angle_threshold) {
delta_s = std::min(delta_s, remaining_length); AddElementToRtree(
if (delta_s < epsilon) { rtree_elements,
AddElementToRtreeAndUpdateTransforms( current_transform,
rtree_elements, next_transform,
current_transform, current_waypoint,
current_waypoint, next_waypoint);
next_waypoint); current_waypoint = next_waypoint;
break; current_transform = next_transform;
}
auto next = GetNext(next_waypoint, delta_s);
if (next.size() != 1 ||
current_waypoint.road_id != next.front().road_id) {
AddElementToRtreeAndUpdateTransforms(
rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
break;
}
next_waypoint = next.front();
geom::Transform next_transform = ComputeTransform(next_waypoint);
double angle = geom::Math::GetVectorAngle(
current_transform.GetForwardVector(), next_transform.GetForwardVector());
if (abs(angle) > angle_threshold) {
AddElementToRtree(
rtree_elements,
current_transform,
next_transform,
current_waypoint,
next_waypoint);
current_waypoint = next_waypoint;
current_transform = next_transform;
}
} }
}
auto next = GetNext(current_waypoint, 10.0 * epsilon);
if (next.size() != 1 || next.front().road_id != current_waypoint.road_id) {
break;
} else {
current_waypoint = next.front();
} }
} }
} }
// Add segments to Rtree
_rtree.InsertElements(rtree_elements); _rtree.InsertElements(rtree_elements);
} }

2
LibCarla/source/carla/road/Map.h
View File

@ -128,7 +128,7 @@ namespace road {
/// Generate the minimum set of waypoints that define the topology of @a /// Generate the minimum set of waypoints that define the topology of @a
/// map. The waypoints are placed at the entrance of each lane. /// map. The waypoints are placed at the entrance of each lane.
std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology() const; std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology(Lane::LaneType lane_type = Lane::LaneType::Driving) const;
/// Generate waypoints of the junction /// Generate waypoints of the junction
std::vector<std::pair<Waypoint, Waypoint>> GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const; std::vector<std::pair<Waypoint, Waypoint>> GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const;