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

Merge branch 'geom_fixes' into fix_prev_next_lane

This commit is contained in:
iFuSiiOnzZ 2018-11-08 12:22:36 +01:00
parent 223ddb94ad 84f72b7565
commit 5667505ec9
6 changed files with 138 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
LibCarla/source/carla/geom/Math.h
View File

@ -75,8 +75,8 @@ namespace geom {
}
/// Returns a pair containing:
/// - @a first: distance from v to p' where p' = p projected on segment (w - v)
/// - @a second: euclidean distance from p to p'
/// - @b first: distance from v to p' where p' = p projected on segment (w - v)
/// - @b second: euclidean distance from p to p'
/// @param p point to calculate distance
/// @param v first point of the segment
/// @param w second point of the segment
@ -102,8 +102,8 @@ namespace geom {
}
/// Returns a pair containing:
/// - @a first: distance across the arc from start_pos to p' where p' = p projected on Arc
/// - @a second: euclidean distance from p to p'
/// - @b first: distance across the arc from start_pos to p' where p' = p projected on Arc
/// - @b second: euclidean distance from p to p'
static std::pair<double, double> DistArcPoint(
Vector3D p,
Vector3D start_pos,

41
LibCarla/source/carla/road/MapBuilder.cpp
View File

@ -5,6 +5,7 @@
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "carla/road/MapBuilder.h"
#include "carla/road/element/RoadInfoVisitor.h"
using namespace carla::road::element;
@ -41,6 +42,46 @@ namespace road {
}
}
// Set the _lane_center_offset of all the lanes
for (auto &&element : _map_data._elements) {
RoadSegment *road_seg = element.second.get();
auto up_bound_g = decltype(road_seg->_info)::reverse_iterator(road_seg->_info.upper_bound(0.0));
auto general_info = MakeRoadInfoIterator<RoadGeneralInfo>(up_bound_g, road_seg->_info.rend());
auto up_bound_l = decltype(road_seg->_info)::reverse_iterator(road_seg->_info.upper_bound(0.0));
auto lane_info = MakeRoadInfoIterator<RoadInfoLane>(up_bound_l, road_seg->_info.rend());
// check that have a RoadGeneralInfo
if (!lane_info.IsAtEnd()) {
double lane_offset = 0.0;
if (!general_info.IsAtEnd()) {
lane_offset = (*general_info)->GetLanesOffset().at(0).second;
}
double current_width = lane_offset;
for (auto &&current_lane_id : (*lane_info)->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left)) {
const double half_width = (*lane_info)->getLane(current_lane_id)->_width * 0.5;
current_width += half_width;
(*lane_info)->_lanes[current_lane_id]._lane_center_offset = current_width;
current_width += half_width;
}
current_width = lane_offset;
for (auto &&current_lane_id : (*lane_info)->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right)) {
const double half_width = (*lane_info)->getLane(current_lane_id)->_width * 0.5;
current_width -= half_width;
(*lane_info)->_lanes[current_lane_id]._lane_center_offset = current_width;
current_width -= half_width;
}
}
}
// _map_data is a memeber of MapBuilder so you must especify if
// you want to keep it (will return copy -> Map(const Map &))
// or move it (will return move -> Map(Map &&))

11
LibCarla/source/carla/road/element/Geometry.h
View File

@ -129,6 +129,11 @@ namespace element {
return p;
}
/// Returns a pair containing:
/// - @b first: distance to the nearest point in this line from the
/// begining of the shape.
/// - @b second: euclidean distance from the nearest point in this line to p.
/// @param p point to calculate the distance
std::pair<double, double> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistSegmentPoint(
p,
@ -164,6 +169,11 @@ namespace element {
return p;
}
/// Returns a pair containing:
/// - @b first: distance to the nearest point in this arc from the
/// begining of the shape.
/// - @b second: euclidean distance from the nearest point in this arc to p.
/// @param p point to calculate the distance
std::pair<double, double> DistanceTo(const geom::Location &p) const override {
return geom::Math::DistArcPoint(
p,
@ -227,6 +237,7 @@ namespace element {
return p;
}
/// @todo
std::pair<double, double> DistanceTo(const geom::Location &) const override {
DEBUG_ERROR;
return {0.0, 0.0};

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

@ -13,6 +13,7 @@
namespace carla {
namespace road {
class MapBuilder;
namespace element {
class RoadInfo {
@ -31,20 +32,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 +73,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;
};
@ -66,6 +85,7 @@ namespace element {
int _id;
double _width;
double _lane_center_offset;
std::string _type;
std::vector<int> _successor;
@ -73,17 +93,21 @@ namespace element {
LaneInfo()
: _id(0),
_width(0.0) {}
_width(0.0),
_lane_center_offset(0.0) {}
LaneInfo(int id, double width, const std::string &type)
: _id(id),
_width(width),
_lane_center_offset(0.0),
_type(type) {}
};
class RoadInfoLane : public RoadInfo {
private:
friend MapBuilder;
using lane_t = std::map<int, LaneInfo>;
lane_t _lanes;
@ -137,7 +161,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());
}

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

@ -62,7 +62,7 @@ namespace element {
// returns info vector given a type and a distance
std::vector<std::shared_ptr<const RoadInfo>> GetInfos(double dist) const {
// @todo
(void)dist;
(void) dist;
return std::vector<std::shared_ptr<const RoadInfo>>();
}
@ -114,30 +114,38 @@ namespace element {
return _predecessors;
}
// Given the current lane it gives an std::pair of the lane id the and road id
// where you can go. First integer of the pair is the lane id and the second the road id.
// Given the current lane it gives an std::pair of the lane id the and road
// id
// where you can go. First integer of the pair is the lane id and the
// second the road id.
//
// INPUT:
// int current_lane_id for which lane do you want the next
//
// OUTPUT:
// std::vector<std::pair<int, int>> return a pair with lane id (first int) and the road id (second int),
// if no lane has been found the given pair it will be (0, 0) as lane id
// std::vector<std::pair<int, int>> return a pair with lane id (first
// int) and the road id (second int),
// if no lane has been found the given
// pair it will be (0, 0) as lane id
// zero used for the reference line
std::vector<std::pair<int, int>> GetNextLane(int current_lane_id) const {
std::map<int, std::vector<std::pair<int, int>>>::const_iterator it = _next_lane.find(current_lane_id);
return it == _next_lane.end() ? std::vector<std::pair<int, int>>() : it->second;
}
// Given the current lane it gives an std::pair vector with the lane id the and road id
// where you can go. First integer of the pair is the lane id and the second the road id.
// Given the current lane it gives an std::pair vector with the lane id the
// and road id
// where you can go. First integer of the pair is the lane id and the second
// the road id.
//
// INPUT:
// int current_lane_id for which lane do you want the next
//
// OUTPUT:
// std::vector<std::pair<int, int>> return a pair with lane id (first int) and the road id (second int),
// if no lane has been found the given pair it will be (0, 0) as lane id
// std::vector<std::pair<int, int>> return a pair with lane id (first
// int) and the road id (second int),
// if no lane has been found the given
// pair it will be (0, 0) as lane id
// zero used for the reference line
std::vector<std::pair<int, int>> GetPrevLane(int current_lane_id) const {
std::map<int, std::vector<std::pair<int, int>>>::const_iterator it = _prev_lane.find(current_lane_id);
@ -165,11 +173,17 @@ namespace element {
return DirectedPoint::Invalid();
}
/// Returns a pair containing:
/// - @b first: distance to the nearest point on the center in
/// this road segment from the begining of it.
/// - @b second: euclidean distance from the nearest point in
/// this road segment to p.
/// @param loc point to calculate the distance
std::pair<double, double> GetNearestPoint(const geom::Location &loc) const {
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;
@ -184,56 +198,36 @@ namespace element {
return last;
}
std::pair<int, geom::Location> GetNearestLane(double dist, const geom::Location &loc) const {
/// Returns a the nearest lane id.
/// @param dist distance from the begining of the road to the point you
/// want to calculate the distance
/// @param loc point to calculate the distance
int 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);
int nearest_lane_id = 0;
geom::Location nearest_loc;
double nearest_dist = std::numeric_limits<double>::max();
double current_width = 0;
// Left lanes
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;
for (auto &&current_lane_id :
info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Both)) {
const auto current_lane_info = info->getLane(current_lane_id);
current_width += half_width;
if (info->getLane(current_lane_id)->_type == "driving") {
dp_center_lane.ApplyLateralOffset(-current_width);
if (current_lane_info->_type == "driving") {
DirectedPoint dp_center_lane = dp_center_road;
dp_center_lane.ApplyLateralOffset(current_lane_info->_lane_center_offset);
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 = 0.0;
// Right lanes
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;
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) {
nearest_dist = current_dist;
nearest_lane_id = current_lane_id;
nearest_loc = dp_center_lane.location;
}
}
current_width += half_width;
}
return std::make_pair(nearest_lane_id, nearest_loc);
return nearest_lane_id;
}
const double &GetLength() const {
@ -268,6 +262,9 @@ namespace element {
};
private:
friend class MapBuilder;
id_type _id;
std::vector<RoadSegment *> _predecessors;
std::vector<RoadSegment *> _successors;

27
LibCarla/source/carla/road/element/Waypoint.cpp
View File

@ -53,7 +53,7 @@ namespace element {
}
DEBUG_ASSERT(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
const auto nearest_lane = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc);
_lane_id = nearest_lane.first;
_lane_id = nearest_lane;
}
Waypoint::Waypoint(
@ -77,34 +77,11 @@ namespace element {
if (_lane_id > 0) {
rot.yaw += 180.0;
}
double current_width = 0.0;
auto *info = GetRoad(*_map, _road_id).GetInfo<RoadInfoLane>(0.0);
DEBUG_ASSERT(info != nullptr);
if (_lane_id > 0) {
// Left lanes
for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Left)) {
const double half_width = info->getLane(current_lane_id)->_width * 0.5;
current_width -= half_width;
if (current_lane_id == _lane_id) {
break;
}
current_width -= half_width;
}
} else {
// Right lanes
for (auto &&current_lane_id : info->getLanesIDs(carla::road::element::RoadInfoLane::which_lane_e::Right)) {
const double half_width = info->getLane(current_lane_id)->_width * 0.5;
current_width += half_width;
if (current_lane_id == _lane_id) {
break;
}
current_width += half_width;
}
}
dp.ApplyLateralOffset(-current_width);
dp.ApplyLateralOffset(info->getLane(_lane_id)->_lane_center_offset);
return geom::Transform(dp.location, rot);
}