From 3443a64db24e66e452c572a5831a65bf3581b7f8 Mon Sep 17 00:00:00 2001 From: Marc Garcia Puig Date: Mon, 13 Apr 2020 12:29:25 +0200 Subject: [PATCH] Fixed GetCornerPositions --- LibCarla/source/carla/road/Lane.cpp | 98 ++++++++++++++++++++--------- PythonAPI/util/lane_explorer.py | 11 +--- 2 files changed, 70 insertions(+), 39 deletions(-) diff --git a/LibCarla/source/carla/road/Lane.cpp b/LibCarla/source/carla/road/Lane.cpp index 5ccab9b21..f90095f19 100644 --- a/LibCarla/source/carla/road/Lane.cpp +++ b/LibCarla/source/carla/road/Lane.cpp @@ -143,7 +143,10 @@ namespace road { RELEASE_ASSERT(GetId() >= lanes.begin()->first); RELEASE_ASSERT(GetId() <= lanes.rbegin()->first); - float lane_width = 0.0f; + // These will accumulate the lateral offset (t) and lane heading of all + // the lanes in between the current lane and lane 0, where the main road + // geometry is described + float lane_t_offset = 0.0f; float lane_tangent = 0.0f; if (GetId() < 0) { @@ -152,77 +155,110 @@ namespace road { std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend()); const auto computed_width = ComputeTotalLaneWidth(side_lanes, s, GetId()); - lane_width = static_cast(computed_width.first); + lane_t_offset = static_cast(computed_width.first); lane_tangent = static_cast(computed_width.second); } else if (GetId() > 0) { // left lane const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end()); const auto computed_width = ComputeTotalLaneWidth(side_lanes, s, GetId()); - lane_width = static_cast(computed_width.first); + lane_t_offset = static_cast(computed_width.first); lane_tangent = static_cast(computed_width.second); } - // get a directed point in s and apply the computed lateral offet - element::DirectedPoint dp = road->GetDirectedPointIn(s); - - // compute the tangent of the laneOffset + // Compute the tangent of the road's (lane 0) "laneOffset" on the current s const auto lane_offset_info = road->GetInfo(s); const auto lane_offset_tangent = static_cast(lane_offset_info->GetPolynomial().Tangent(s)); + // Update the road tangent with the "laneOffset" information at current s lane_tangent -= lane_offset_tangent; - // Unreal's Y axis hack - lane_tangent *= -1; + // Get a directed point on the center of the current lane given an s + element::DirectedPoint dp = road->GetDirectedPointIn(s); - geom::Rotation rot( - geom::Math::ToDegrees(static_cast(dp.pitch)), - geom::Math::ToDegrees(-static_cast(dp.tangent)), // Unreal's Y axis hack - 0.0f); + // Transform from the center of the road to the center of the lane + dp.ApplyLateralOffset(lane_t_offset); - dp.ApplyLateralOffset(lane_width); - - if (GetId() > 0) { - rot.yaw += 180.0f - geom::Math::ToDegrees(lane_tangent); - rot.pitch = 360.0f - rot.pitch; - } else { - rot.yaw -= geom::Math::ToDegrees(lane_tangent); - } + // Update the lane tangent with the road "laneOffset" at current s + dp.tangent -= lane_tangent; // Unreal's Y axis hack dp.location.y *= -1; + dp.tangent *= -1; + + geom::Rotation rot( + geom::Math::ToDegrees(static_cast(dp.pitch)), + geom::Math::ToDegrees(static_cast(dp.tangent)), + 0.0f); + + // Fix the direction of the possitive lanes + if (GetId() > 0) { + rot.yaw += 180.0f; + rot.pitch = 360.0f - rot.pitch; + } return geom::Transform(dp.location, rot); } std::pair Lane::GetCornerPositions( const double s, const float extra_width) const { - DEBUG_ASSERT(GetRoad() != nullptr); + const Road *road = GetRoad(); + DEBUG_ASSERT(road != nullptr); + + const auto *lane_section = GetLaneSection(); + DEBUG_ASSERT(lane_section != nullptr); + const std::map &lanes = lane_section->GetLanes(); + + // check that lane_id exists on the current s + RELEASE_ASSERT(!lanes.empty()); + RELEASE_ASSERT(GetId() >= lanes.begin()->first); + RELEASE_ASSERT(GetId() <= lanes.rbegin()->first); + + float lane_t_offset = 0.0f; + + if (GetId() < 0) { + // right lane + const auto side_lanes = MakeListView( + std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend()); + const auto computed_width = + ComputeTotalLaneWidth(side_lanes, s, GetId()); + lane_t_offset = static_cast(computed_width.first); + } else if (GetId() > 0) { + // left lane + const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end()); + const auto computed_width = + ComputeTotalLaneWidth(side_lanes, s, GetId()); + lane_t_offset = static_cast(computed_width.first); + } float lane_width = static_cast(GetWidth(s)) / 2.0f; - if (extra_width != 0.f && GetRoad()->IsJunction() && GetType() == Lane::LaneType::Driving) { + if (extra_width != 0.f && road->IsJunction() && GetType() == Lane::LaneType::Driving) { lane_width += extra_width; } - lane_width = GetId() > 0 ? -lane_width : lane_width; - const geom::Transform wp_trnsf = ComputeTransform(s); + // Get two points on the center of the road on given s + element::DirectedPoint dp_r, dp_l; + dp_r = dp_l = road->GetDirectedPointIn(s); - const auto wp_right_distance = wp_trnsf.GetRightVector() * lane_width; + // Transform from the center of the road to each of lane corners + dp_r.ApplyLateralOffset(lane_t_offset + lane_width); + dp_l.ApplyLateralOffset(lane_t_offset - lane_width); - auto loc_r = static_cast(wp_trnsf.location) + wp_right_distance; - auto loc_l = static_cast(wp_trnsf.location) - wp_right_distance; + // Unreal's Y axis hack + dp_r.location.y *= -1; + dp_l.location.y *= -1; // Apply an offset to the Sidewalks if (GetType() == LaneType::Sidewalk) { // RoadRunner doesn't export it right now and as a workarround where 15.24 cm // is the exact height that match with most of the RoadRunner sidewalks - loc_r.z += 0.1524f; - loc_l.z += 0.1524f; + dp_r.location.z += 0.1524f; + dp_l.location.z += 0.1524f; /// TODO: use the OpenDRIVE 5.3.7.2.1.1.9 Lane Height Record } - return std::make_pair(loc_r, loc_l); + return std::make_pair(dp_r.location, dp_l.location); } } // road diff --git a/PythonAPI/util/lane_explorer.py b/PythonAPI/util/lane_explorer.py index 7872f67c3..584143c2b 100755 --- a/PythonAPI/util/lane_explorer.py +++ b/PythonAPI/util/lane_explorer.py @@ -21,7 +21,6 @@ except IndexError: import carla import argparse -import math import random import time @@ -38,13 +37,9 @@ waypoint_separation = 4 def draw_transform(debug, trans, col=carla.Color(255, 0, 0), lt=-1): - yaw_in_rad = math.radians(trans.rotation.yaw) - pitch_in_rad = math.radians(trans.rotation.pitch) - p1 = carla.Location( - x=trans.location.x + math.cos(pitch_in_rad) * math.cos(yaw_in_rad), - y=trans.location.y + math.cos(pitch_in_rad) * math.sin(yaw_in_rad), - z=trans.location.z + math.sin(pitch_in_rad)) - debug.draw_arrow(trans.location, p1, thickness=0.05, arrow_size=0.1, color=col, life_time=lt) + debug.draw_arrow( + trans.location, trans.location + trans.get_forward_vector(), + thickness=0.05, arrow_size=0.1, color=col, life_time=lt) def draw_waypoint_union(debug, w0, w1, color=carla.Color(255, 0, 0), lt=5):