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

Fixed GetCornerPositions

This commit is contained in:
Marc Garcia Puig 2020-04-13 12:29:25 +02:00 committed by bernat
parent d23e3d5f82
commit 3443a64db2
2 changed files with 70 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
LibCarla/source/carla/road/Lane.cpp
View File

@ -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
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<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
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<float>(dp.pitch)),
geom::Math::ToDegrees(-static_cast<float>(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<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;
auto loc_l = static_cast<geom::Vector3D>(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

11
PythonAPI/util/lane_explorer.py
View File

@ -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):