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

Added LaneOffset to GetNearestPoint function + some explanation comments

This commit is contained in:
Marc 2018-11-05 12:49:57 +01:00
parent f2559e0ee2
commit a82b80eb30
2 changed files with 58 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
LibCarla/source/carla/road/element/RoadInfo.h
View File

@ -31,20 +31,37 @@ namespace element {
};
class RoadGeneralInfo : public RoadInfo {
private:
int _junction_id = -1;
std::vector<std::pair<double, double>> _lanes_offset;
private:
public:
void AcceptVisitor(RoadInfoVisitor &v) final {
v.Visit(*this);
}
int _junction_id = -1;
/// A vector of pairs where the first double represents the
/// offset from the begining of the road and the second represents the
/// lateral offest of the lane
std::vector<std::pair<double, double>> _lanes_offset;
void SetJunctionId(int junctionId) { _junction_id = junctionId; }
int GetJunctionId() const { return _junction_id; }
public:
void SetLanesOffset(double offset, double laneOffset) { _lanes_offset.emplace_back(std::pair<double, double>(offset, laneOffset)); }
std::vector<std::pair<double, double>> GetLanesOffset() const { return _lanes_offset; }
void AcceptVisitor(RoadInfoVisitor &v) final {
v.Visit(*this);
}
void SetJunctionId(int junctionId) {
_junction_id = junctionId;
}
int GetJunctionId() const {
return _junction_id;
}
void SetLanesOffset(double offset, double laneOffset) {
_lanes_offset.emplace_back(std::pair<double, double>(offset, laneOffset));
}
/// @returns A vector of pairs where the first double represents the
/// offset from the begining of the road and the second represents the
/// lateral offest of the lane
std::vector<std::pair<double, double>> GetLanesOffset() const {
return _lanes_offset;
}
};
class RoadInfoVelocity : public RoadInfo {
@ -55,8 +72,9 @@ namespace element {
}
RoadInfoVelocity(double vel) : velocity(vel) {}
RoadInfoVelocity(double d, double vel) : RoadInfo(d),
velocity(vel) {}
RoadInfoVelocity(double d, double vel)
: RoadInfo(d),
velocity(vel) {}
double velocity = 0;
};
@ -137,7 +155,7 @@ namespace element {
// NOTE(Andrei): For right lane the IDs are negative,
// so reverse so sort order to haven them going
// from -1 to -n
if(whichLanes == which_lane_e::Right) {
if (whichLanes == which_lane_e::Right) {
std::reverse(lanes_id.begin(), lanes_id.end());
}

37
LibCarla/source/carla/road/element/RoadSegment.h
View File

@ -169,7 +169,7 @@ namespace element {
decltype(_geom)::const_iterator nearest_geom;
std::pair<double, double> last = {0.0, std::numeric_limits<double>::max()};
for (auto g = _geom.begin(); g !=_geom.end(); ++g) {
for (auto g = _geom.begin(); g != _geom.end(); ++g) {
auto d = (*g)->DistanceTo(loc);
if (d.second < last.second) {
last = d;
@ -186,26 +186,38 @@ namespace element {
std::pair<int, geom::Location> GetNearestLane(double dist, const geom::Location &loc) const {
// Because Unreal's coordinates
const geom::Location corrected_loc = geom::Location(loc.x, loc.y, loc.z);
const geom::Location corrected_loc = geom::Location(loc.x, -loc.y, loc.z);
const DirectedPoint dp_center_road = GetDirectedPointIn(dist);
auto info = GetInfo<RoadInfoLane>(0.0);
auto lane_offsets_vec = GetInfo<RoadGeneralInfo>(0.0)->GetLanesOffset();
double lane_offset = 0.0;
// Get lane offset for the current dist
for (auto &&i : lane_offsets_vec) {
if (i.first <= dist) {
lane_offset = i.second;
} else {
break;
}
}
int nearest_lane_id = 0;
geom::Location nearest_loc;
double nearest_dist = std::numeric_limits<double>::max();
double current_width = 0;
double current_width = lane_offset;
// Left lanes
for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left)) {
for (auto &&current_lane_id :
info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left)) {
DirectedPoint dp_center_lane = dp_center_road;
const double half_width = info->getLane(current_lane_id)->_width * 0.5;
current_width += half_width;
if (info->getLane(current_lane_id)->_type == "driving") {
dp_center_lane.ApplyLateralOffset(-current_width);
dp_center_lane.ApplyLateralOffset(current_width);
const double current_dist = geom::Math::Distance2D(dp_center_lane.location, corrected_loc);
if(current_dist < nearest_dist) {
if (current_dist < nearest_dist) {
nearest_dist = current_dist;
nearest_lane_id = current_lane_id;
nearest_loc = dp_center_lane.location;
@ -214,23 +226,25 @@ namespace element {
current_width += half_width;
}
current_width = 0.0;
current_width = lane_offset;
// Right lanes
for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right)) {
for (auto &&current_lane_id :
info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right)) {
DirectedPoint dp_center_lane = dp_center_road;
const double half_width = info->getLane(current_lane_id)->_width * 0.5;
current_width += half_width;
current_width -= half_width;
if (info->getLane(current_lane_id)->_type == "driving") {
dp_center_lane.ApplyLateralOffset(current_width);
const double current_dist = geom::Math::Distance2D(dp_center_lane.location, corrected_loc);
if(current_dist < nearest_dist) {
if (current_dist < nearest_dist) {
nearest_dist = current_dist;
nearest_lane_id = current_lane_id;
nearest_loc = dp_center_lane.location;
}
}
current_width += half_width;
current_width -= half_width;
}
return std::make_pair(nearest_lane_id, nearest_loc);
@ -268,6 +282,7 @@ namespace element {
};
private:
id_type _id;
std::vector<RoadSegment *> _predecessors;
std::vector<RoadSegment *> _successors;