Fixed GetCornerPositions

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<float>(computed_width.first);
+      lane_t_offset = static_cast<float>(computed_width.first);
       lane_tangent = static_cast<float>(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<float>(computed_width.first);
+      lane_t_offset = static_cast<float>(computed_width.first);
       lane_tangent = static_cast<float>(computed_width.second);
     }
 
-    // get a directed point in s and apply the computed lateral offet
+    // Compute the tangent of the road's (lane 0) "laneOffset" on the current s
-    element::DirectedPoint dp = road->GetDirectedPointIn(s);
-
-    // compute the tangent of the laneOffset
     const auto lane_offset_info = road->GetInfo<element::RoadInfoLaneOffset>(s);
     const auto lane_offset_tangent =
         static_cast<float>(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
+    // Get a directed point on the center of the current lane given an s
-    lane_tangent *= -1;
+    element::DirectedPoint dp = road->GetDirectedPointIn(s);
 
-    geom::Rotation rot(
+    // Transform from the center of the road to the center of the lane
-        geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
+    dp.ApplyLateralOffset(lane_t_offset);
-        geom::Math::ToDegrees(-static_cast<float>(dp.tangent)), // Unreal's Y axis hack
-        0.0f);
 
-    dp.ApplyLateralOffset(lane_width);
+    // Update the lane tangent with the road "laneOffset" at current s
-
+    dp.tangent -= lane_tangent;
-    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);
-    }
 
     // Unreal's Y axis hack
     dp.location.y *= -1;
+    dp.tangent    *= -1;
+
+    geom::Rotation rot(
+        geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
+        geom::Math::ToDegrees(static_cast<float>(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<geom::Vector3D, geom::Vector3D> 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<LaneId, Lane> &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<float>(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<float>(computed_width.first);
+    }
 
     float lane_width = static_cast<float>(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<geom::Vector3D>(wp_trnsf.location) + wp_right_distance;
+    // Unreal's Y axis hack
-    auto loc_l = static_cast<geom::Vector3D>(wp_trnsf.location) - wp_right_distance;
+    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;
+      dp_r.location.z += 0.1524f;
-      loc_l.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

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)
+    debug.draw_arrow(
-    pitch_in_rad = math.radians(trans.rotation.pitch)
+        trans.location, trans.location + trans.get_forward_vector(),
-    p1 = carla.Location(
+        thickness=0.05, arrow_size=0.1, color=col, life_time=lt)
-        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)
 
 
 def draw_waypoint_union(debug, w0, w1, color=carla.Color(255, 0, 0), lt=5):