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

Merge branch 'marcgpuig/opendrive' into jenkins/opendrive

This commit is contained in:
Marc Garcia Puig 2019-03-26 14:35:19 +01:00
parent 93ef05ab2e 5272abf37b
commit d8d399494e
20 changed files with 189 additions and 444 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
LibCarla/source/carla/client/Map.cpp
View File

@ -39,12 +39,13 @@ namespace client {
SharedPtr<Waypoint> Map::GetWaypoint(
const geom::Location &location,
bool project_to_road) const {
bool project_to_road,
uint32_t lane_type) const {
boost::optional<road::element::Waypoint> waypoint;
if (project_to_road) {
waypoint = _map.GetClosestWaypointOnRoad(location);
waypoint = _map.GetClosestWaypointOnRoad(location, lane_type);
} else {
waypoint = _map.GetWaypoint(location);
waypoint = _map.GetWaypoint(location, lane_type);
}
return waypoint.has_value() ?
SharedPtr<Waypoint>(new Waypoint{shared_from_this(), *waypoint}) :

4
LibCarla/source/carla/client/Map.h
View File

@ -11,6 +11,7 @@
#include "carla/road/Map.h"
#include "carla/road/element/LaneMarking.h"
#include "carla/rpc/MapInfo.h"
#include "carla/road/Lane.h"
#include <string>
@ -49,7 +50,8 @@ namespace client {
SharedPtr<Waypoint> GetWaypoint(
const geom::Location &location,
bool project_to_road = true) const;
bool project_to_road = true,
uint32_t lane_type = static_cast<uint32_t>(road::Lane::LaneType::Driving)) const;
using TopologyList = std::vector<std::pair<SharedPtr<Waypoint>, SharedPtr<Waypoint>>>;

3
LibCarla/source/carla/client/Waypoint.cpp
View File

@ -27,9 +27,10 @@ namespace client {
double Waypoint::GetLaneWidth() const {
return _parent->GetMap().GetLaneWidth(_waypoint);
}
std::string Waypoint::GetType() const {
road::Lane::LaneType Waypoint::GetType() const {
return _parent->GetMap().GetLaneType(_waypoint);
}

3
LibCarla/source/carla/client/Waypoint.h
View File

@ -11,6 +11,7 @@
#include "carla/geom/Transform.h"
#include "carla/road/element/RoadInfoMarkRecord.h"
#include "carla/road/element/Waypoint.h"
#include "carla/road/Lane.h"
namespace carla {
namespace client {
@ -64,7 +65,7 @@ namespace client {
double GetLaneWidth() const;
std::string GetType() const;
road::Lane::LaneType GetType() const;
std::vector<SharedPtr<Waypoint>> Next(double distance) const;

63
LibCarla/source/carla/opendrive/parser/RoadParser.cpp
View File

@ -9,6 +9,7 @@
#include "carla/road/MapBuilder.h"
#include "carla/road/RoadTypes.h"
#include "carla/Logging.h"
#include "carla/StringUtil.h"
#include <deque>
namespace carla {
@ -26,7 +27,7 @@ namespace parser {
struct Lane {
LaneId id;
std::string type;
road::Lane::LaneType type;
bool level;
LaneId predecessor;
LaneId successor;
@ -61,6 +62,52 @@ namespace parser {
std::vector<LaneSection> sections;
};
static road::Lane::LaneType StringToLaneType(const std::string &str) {
if (str == "driving") {
return road::Lane::LaneType::Driving;
} else if (str == "stop") {
return road::Lane::LaneType::Stop;
} else if (str == "shoulder") {
return road::Lane::LaneType::Shoulder;
} else if (str == "biking") {
return road::Lane::LaneType::Biking;
} else if (str == "sidewalk") {
return road::Lane::LaneType::Sidewalk;
} else if (str == "border") {
return road::Lane::LaneType::Border;
} else if (str == "restricted") {
return road::Lane::LaneType::Restricted;
} else if (str == "parking") {
return road::Lane::LaneType::Parking;
} else if (str == "bidirectional") {
return road::Lane::LaneType::Bidirectional;
} else if (str == "median") {
return road::Lane::LaneType::Median;
} else if (str == "special1") {
return road::Lane::LaneType::Special1;
} else if (str == "special2") {
return road::Lane::LaneType::Special2;
} else if (str == "special3") {
return road::Lane::LaneType::Special3;
} else if (str == "roadWorks") {
return road::Lane::LaneType::RoadWorks;
} else if (str == "tram") {
return road::Lane::LaneType::Tram;
} else if (str == "rail") {
return road::Lane::LaneType::Rail;
} else if (str == "entry") {
return road::Lane::LaneType::Entry;
} else if (str == "exit") {
return road::Lane::LaneType::Exit;
} else if (str == "offRamp") {
return road::Lane::LaneType::OffRamp;
} else if (str == "onRamp") {
return road::Lane::LaneType::OnRamp;
} else {
return road::Lane::LaneType::None;
}
}
void RoadParser::Parse(
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder) {
@ -122,10 +169,10 @@ namespace parser {
// left lanes
for (pugi::xml_node node_lane : node_section.child("left").children("lane")) {
Lane lane { 0, "none", false, 0, 0 };
Lane lane { 0, road::Lane::LaneType::None, false, 0, 0 };
lane.id = node_lane.attribute("id").as_int();
lane.type = node_lane.attribute("type").value();
lane.type = StringToLaneType(node_lane.attribute("type").value());
lane.level = node_lane.attribute("level").as_bool();
// link
@ -143,10 +190,10 @@ namespace parser {
// center lane
for (pugi::xml_node node_lane : node_section.child("center").children("lane")) {
Lane lane { 0, "none", false, 0, 0 };
Lane lane { 0, road::Lane::LaneType::None, false, 0, 0 };
lane.id = node_lane.attribute("id").as_int();
lane.type = node_lane.attribute("type").value();
lane.type = StringToLaneType(node_lane.attribute("type").value());
lane.level = node_lane.attribute("level").as_bool();
// link (probably it never exists)
@ -164,10 +211,10 @@ namespace parser {
// right lane
for (pugi::xml_node node_lane : node_section.child("right").children("lane")) {
Lane lane { 0, "none", false, 0, 0 };
Lane lane { 0, road::Lane::LaneType::None, false, 0, 0 };
lane.id = node_lane.attribute("id").as_int();
lane.type = node_lane.attribute("type").value();
lane.type = StringToLaneType(node_lane.attribute("type").value());
lane.level = node_lane.attribute("level").as_bool();
// link
@ -245,7 +292,7 @@ namespace parser {
// lanes
for (auto const l : s.lanes) {
/*carla::road::Lane *lane = */map_builder.AddRoadSectionLane(section, l.id, l.type, l.level, l.predecessor, l.successor);
/*carla::road::Lane *lane = */map_builder.AddRoadSectionLane(section, l.id, static_cast<uint32_t>(l.type), l.level, l.predecessor, l.successor);
}
}
}

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

@ -27,7 +27,7 @@ namespace road {
return _id;
}
std::string Lane::GetType() const {
Lane::LaneType Lane::GetType() const {
return _type;
}

33
LibCarla/source/carla/road/Lane.h
View File

@ -21,6 +21,34 @@ namespace road {
class Road;
class Lane : private MovableNonCopyable {
public:
/// Can be used as flags
enum class LaneType : uint32_t {
None = 0x1,
Driving = 0x1 << 1,
Stop = 0x1 << 2,
Shoulder = 0x1 << 3,
Biking = 0x1 << 4,
Sidewalk = 0x1 << 5,
Border = 0x1 << 6,
Restricted = 0x1 << 7,
Parking = 0x1 << 8,
Bidirectional = 0x1 << 9,
Median = 0x1 << 10,
Special1 = 0x1 << 11,
Special2 = 0x1 << 12,
Special3 = 0x1 << 13,
RoadWorks = 0x1 << 14,
Tram = 0x1 << 15,
Rail = 0x1 << 16,
Entry = 0x1 << 17,
Exit = 0x1 << 18,
OffRamp = 0x1 << 19,
OnRamp = 0x1 << 20,
Any = 0xFFFFFFFF
};
public:
Lane() = default;
@ -42,7 +70,7 @@ namespace road {
LaneId GetId() const;
std::string GetType() const;
LaneType GetType() const;
bool GetLevel() const;
@ -85,8 +113,7 @@ namespace road {
InformationSet _info;
/// @todo: change to enum, see 6.5 of OpenDRIVEFormatSpecRev1.4H.pdf
std::string _type;
LaneType _type;
bool _level;

4
LibCarla/source/carla/road/LaneSection.cpp
View File

@ -37,10 +37,10 @@ namespace road {
return _lanes;
}
std::vector<Lane *> LaneSection::GetLanesOfType(const std::string &type) {
std::vector<Lane *> LaneSection::GetLanesOfType(Lane::LaneType lane_type) {
std::vector<Lane *> drivable_lanes;
for (auto &&lane : _lanes) {
if (lane.second.GetType() == type) {
if ((static_cast<uint32_t>(lane.second.GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
drivable_lanes.emplace_back(&lane.second);
}
}

2
LibCarla/source/carla/road/LaneSection.h
View File

@ -42,7 +42,7 @@ namespace road {
const std::map<LaneId, Lane> &GetLanes() const;
std::vector<Lane *> GetLanesOfType(const std::string &type);
std::vector<Lane *> GetLanesOfType(Lane::LaneType type);
private:

32
LibCarla/source/carla/road/Map.cpp
View File

@ -55,6 +55,7 @@ namespace road {
}
}
/// Return a waypoint for each drivable lane on @a lane_section.
template <typename FuncT>
static void ForEachDrivableLaneImpl(
@ -64,7 +65,7 @@ namespace road {
FuncT &&func) {
for (const auto &pair : lane_section.GetLanes()) {
const auto &lane = pair.second;
if (lane.GetType() == "driving") {
if ((static_cast<uint32_t>(lane.GetType()) & static_cast<uint32_t>(Lane::LaneType::Driving)) > 0) {
std::forward<FuncT>(func)(Waypoint{
road_id,
lane_section.GetId(),
@ -135,10 +136,10 @@ namespace road {
// -- Map: Geometry ----------------------------------------------------------
// ===========================================================================
Waypoint Map::GetClosestWaypointOnRoad(const geom::Location &pos) const {
boost::optional<Waypoint> Map::GetClosestWaypointOnRoad(const geom::Location &pos, uint32_t lane_type) const {
// max_nearests represents the max nearests roads
// where we will search for nearests lanes
constexpr int max_nearests = 10;
constexpr int max_nearests = 15;
// in case that map has less than max_nearests lanes,
// we will use the maximum lanes
const int max_nearest_allowed = _data.GetRoadCount() < max_nearests ?
@ -181,7 +182,7 @@ namespace road {
Waypoint waypoint;
auto nearest_lane_dist = std::numeric_limits<double>::max();
for (int i = 0; i < max_nearest_allowed; ++i) {
auto lane_dist = _data.GetRoad(ids[i]).GetNearestLane(dists[i], pos_inverted_y);
auto lane_dist = _data.GetRoad(ids[i]).GetNearestLane(dists[i], pos_inverted_y, lane_type);
if (lane_dist.second < nearest_lane_dist) {
nearest_lane_dist = lane_dist.second;
@ -191,6 +192,10 @@ namespace road {
}
}
if (nearest_lane_dist == std::numeric_limits<double>::max()) {
return boost::optional<Waypoint>{};
}
const auto &road = _data.GetRoad(waypoint.road_id);
// Make sure 0.0 < waipoint.s < Road's length
@ -206,12 +211,19 @@ namespace road {
return waypoint;
}
boost::optional<Waypoint> Map::GetWaypoint(const geom::Location &pos) const {
Waypoint w = GetClosestWaypointOnRoad(pos);
const auto dist = geom::Math::Distance2D(ComputeTransform(w).location, pos);
const auto lane_width_info = GetLane(w).GetInfo<RoadInfoLaneWidth>(w.s);
boost::optional<Waypoint> Map::GetWaypoint(
const geom::Location &pos,
uint32_t lane_type) const {
boost::optional<Waypoint> w = GetClosestWaypointOnRoad(pos, lane_type);
if (!w.has_value()) {
return w;
}
const auto dist = geom::Math::Distance2D(ComputeTransform(*w).location, pos);
const auto lane_width_info = GetLane(*w).GetInfo<RoadInfoLaneWidth>(w->s);
const auto half_lane_width =
lane_width_info->GetPolynomial().Evaluate(w.s) * 0.5;
lane_width_info->GetPolynomial().Evaluate(w->s) * 0.5;
if (dist < half_lane_width) {
return w;
@ -293,7 +305,7 @@ namespace road {
// -- Map: Road information --------------------------------------------------
// ===========================================================================
std::string Map::GetLaneType(const Waypoint waypoint) const {
Lane::LaneType Map::GetLaneType(const Waypoint waypoint) const {
return GetLane(waypoint).GetType();
}

10
LibCarla/source/carla/road/Map.h
View File

@ -44,9 +44,13 @@ namespace road {
/// -- Geometry ------------------------------------------------------------
/// ========================================================================
element::Waypoint GetClosestWaypointOnRoad(const geom::Location &location) const;
boost::optional<element::Waypoint> GetClosestWaypointOnRoad(
const geom::Location &location,
uint32_t lane_type = static_cast<uint32_t>(Lane::LaneType::Driving)) const;
boost::optional<element::Waypoint> GetWaypoint(const geom::Location &location) const;
boost::optional<element::Waypoint> GetWaypoint(
const geom::Location &location,
uint32_t lane_type = static_cast<uint32_t>(Lane::LaneType::Driving)) const;
geom::Transform ComputeTransform(Waypoint waypoint) const;
@ -54,7 +58,7 @@ namespace road {
/// -- Road information ----------------------------------------------------
/// ========================================================================
std::string GetLaneType(Waypoint waypoint) const;
Lane::LaneType GetLaneType(Waypoint waypoint) const;
double GetLaneWidth(Waypoint waypoint) const;

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

@ -281,7 +281,7 @@ namespace road {
carla::road::Lane *MapBuilder::AddRoadSectionLane(
carla::road::LaneSection *section,
const int32_t lane_id,
const std::string lane_type,
const uint32_t lane_type,
const bool lane_level,
const int32_t predecessor,
const int32_t successor) {
@ -293,7 +293,7 @@ namespace road {
lane->_id = lane_id;
lane->_lane_section = section;
lane->_level = lane_level;
lane->_type = lane_type;
lane->_type = static_cast<carla::road::Lane::LaneType>(lane_type);
lane->_successor = successor;
lane->_predecessor = predecessor;

4
LibCarla/source/carla/road/MapBuilder.h
View File

@ -37,7 +37,7 @@ namespace road {
carla::road::Lane *AddRoadSectionLane(
carla::road::LaneSection *section,
const int32_t lane_id,
const std::string lane_type,
const uint32_t lane_type,
const bool lane_level,
const int32_t predecessor,
const int32_t successor);
@ -190,7 +190,7 @@ namespace road {
const uint32_t road_id,
const int32_t section_index,
const int32_t lane_id,
const std::string lane_type,
const Lane::LaneType lane_type,
const bool lane_level,
const int32_t predecessor,
const int32_t successor);

30
LibCarla/source/carla/road/Road.cpp
View File

@ -226,7 +226,8 @@ namespace road {
const std::pair<const Lane *, double> Road::GetNearestLane(
const double s,
const geom::Location &loc) const {
const geom::Location &loc,
uint32_t lane_type) const {
using namespace carla::road::element;
std::map<LaneId, const Lane *> lanes(GetLanesAt(s));
// negative right lanes
@ -236,23 +237,25 @@ namespace road {
auto left_lanes = MakeListView(
lanes.lower_bound(1), lanes.end());
DirectedPoint dp_lane_zero = GetDirectedPointIn(s);
const DirectedPoint dp_lane_zero = GetDirectedPointIn(s);
std::pair<const Lane *, double> result =
std::make_pair(nullptr, std::numeric_limits<double>::max());
// Unreal's Y axis hack
// dp_lane_zero.location.y *= -1;
DirectedPoint current_dp = dp_lane_zero;
for (const auto &lane : right_lanes) {
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
current_dp.ApplyLateralOffset(half_width);
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
// if the current_dp is near to loc, we are in the right way
if (current_dist <= result.second) {
result.first = &(*lane.second);
result.second = current_dist;
// only consider the lanes that match the type flag for result candidates
if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
result.first = &(*lane.second);
result.second = current_dist;
}
} else {
// elsewhere, we are be moving away
break;
@ -263,13 +266,18 @@ namespace road {
current_dp = dp_lane_zero;
for (const auto &lane : left_lanes) {
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
current_dp.ApplyLateralOffset(half_width);
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
// if the current_dp is near to loc, we are in the right way
if (current_dist <= result.second) {
result.first = &(*lane.second);
result.second = current_dist;
// only consider the lanes that match the type flag for result candidates
if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
result.first = &(*lane.second);
result.second = current_dist;
}
} else {
// elsewhere, we are be moving away
break;

4
LibCarla/source/carla/road/Road.h
View File

@ -105,7 +105,9 @@ namespace road {
/// want to calculate the distance
/// @param loc point to calculate the distance
const std::pair<const Lane *, double> GetNearestLane(
const double s, const geom::Location &loc) const;
const double s,
const geom::Location &loc,
uint32_t type = static_cast<uint32_t>(Lane::LaneType::Any)) const;
template <typename T>
const T *GetInfo (const double s) const {

1
LibCarla/source/carla/road/RoadTypes.h
View File

@ -22,6 +22,7 @@ namespace road {
using ObjId = uint32_t;
using SignId = uint32_t;
using SignRefId = uint32_t;
using ConId = uint32_t;

8
LibCarla/source/carla/road/element/LaneCrossingCalculator.cpp
View File

@ -43,16 +43,16 @@ namespace element {
const geom::Location &destination) {
auto w0 = map.GetClosestWaypointOnRoad(origin);
auto w1 = map.GetClosestWaypointOnRoad(destination);
if (w0.road_id != w1.road_id) {
if (w0->road_id != w1->road_id) {
/// @todo This case should also be handled.
return {};
}
if (map.IsJunction(w0.road_id) || map.IsJunction(w1.road_id)) {
if (map.IsJunction(w0->road_id) || map.IsJunction(w1->road_id)) {
return {};
}
return CrossingAtSameSection(
w0.lane_id,
w1.lane_id,
w0->lane_id,
w1->lane_id,
IsOffRoad(map, origin),
IsOffRoad(map, destination));
}

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

@ -1,387 +0,0 @@
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#pragma once
#include "carla/Debug.h"
#include "carla/Exception.h"
#include "carla/NonCopyable.h"
#include "carla/geom/Location.h"
#include "carla/road/element/RoadElevationInfo.h"
#include "carla/road/element/RoadInfo.h"
#include "carla/road/element/RoadInfoLaneOffset.h"
#include "carla/road/element/RoadInfoLaneWidth.h"
#include "carla/road/element/RoadInfoMarkRecord.h"
#include "carla/road/element/Types.h"
#include <algorithm>
#include <limits>
#include <memory>
#include <set>
#include <stdexcept>
#include <vector>
namespace carla {
namespace road {
class MapBuilder;
namespace element {
class RoadSegment : private NonCopyable {
public:
RoadSegment(id_type id) : _id(id) {}
RoadSegment(RoadSegmentDefinition &&def)
: _id(def.GetId()),
_successors_is_start(std::move(def._successor_is_start)),
_predecessors_is_start(std::move(def._predecessors_is_start)),
_geom(std::move(def._geom)),
_next_lane(std::move(def._next_lane)),
_prev_lane(std::move(def._prev_lane)) {
for (auto &&a : def._info) {
_info.insert(std::move(a));
}
}
id_type GetId() const {
return _id;
}
/// Returns single info given a type and a distance from
/// the start of the road (negative lanes)
template <typename T>
std::shared_ptr<const T> GetInfo(double dist) const {
auto up_bound = decltype(_info)::reverse_iterator(_info.upper_bound(dist));
auto it = MakeRoadInfoIterator<T>(up_bound, _info.rend());
return it.IsAtEnd() ? nullptr : *it;
}
/// Returns single info given a type and a distance from
/// the end of the road (positive lanes)
template <typename T>
std::shared_ptr<const T> GetInfoReverse(double dist) const {
auto lo_bound = _info.lower_bound(dist);
auto it = MakeRoadInfoIterator<T>(lo_bound, _info.end());
return it.IsAtEnd() ? nullptr : *it;
}
/// Returns info vector given a type and a distance from
/// the start of the road (negative lanes)
template <typename T>
std::vector<std::shared_ptr<const T>> GetInfos(double dist) const {
auto up_bound = decltype(_info)::reverse_iterator(_info.upper_bound(dist));
auto it = MakeRoadInfoIterator<T>(up_bound, _info.rend());
std::vector<std::shared_ptr<const T>> result;
for (; !it.IsAtEnd(); ++it) {
result.emplace_back(*it);
}
return result;
}
/// Returns info vector given a type and a distance from
/// the end of the road (positive lanes)
template <typename T>
std::vector<std::shared_ptr<const T>> GetInfosReverse(double dist) const {
auto lo_bound = _info.lower_bound(dist);
auto it = MakeRoadInfoIterator<T>(lo_bound, _info.end());
std::vector<std::shared_ptr<const T>> result;
for (; !it.IsAtEnd(); ++it) {
result.emplace_back(*it);
}
return result;
}
/// Workaround where we must find a specific (RoadInfoMarkRecord) RoadInfo
/// that must have lane_id info. In this case this info is used for selecting
/// only the nearest RoadInfos to the "dist" input.
std::vector<std::shared_ptr<const RoadInfoMarkRecord>> GetRoadInfoMarkRecord(
double dist) const {
auto mark_record_info = GetInfos<RoadInfoMarkRecord>(dist);
std::vector<std::shared_ptr<const RoadInfoMarkRecord>> result;
std::unordered_set<int> inserted_lanes;
for (auto &&mark_record : mark_record_info) {
const int lane_id = mark_record->GetLaneId();
const bool is_new_lane = inserted_lanes.insert(lane_id).second;
if (is_new_lane) {
result.emplace_back(mark_record);
}
}
return result;
}
/// Workaround where we must find a specific (RoadInfoMarkRecord) RoadInfo
/// that must have lane_id info. In this case this info is used for selecting
/// only the nearest RoadInfos to the "dist" input. But reversed!
std::vector<std::shared_ptr<const RoadInfoMarkRecord>> GetRoadInfoMarkRecordReverse(
double dist) const {
auto mark_record_info = GetInfosReverse<RoadInfoMarkRecord>(dist);
std::vector<std::shared_ptr<const RoadInfoMarkRecord>> result;
std::unordered_set<int> inserted_lanes;
for (auto &&mark_record : mark_record_info) {
const int lane_id = mark_record->GetLaneId();
const bool is_new_lane = inserted_lanes.insert(lane_id).second;
if (is_new_lane) {
result.emplace_back(mark_record);
}
}
return result;
}
/// Workaround where we must find a specific (RoadInfoLaneWidth) RoadInfo
/// that must have lane_id info. In this case this info is used for selecting
/// only the nearest RoadInfos to the "dist" input.
std::vector<std::shared_ptr<const RoadInfoLaneWidth>> GetRoadInfoLaneWidth(
double dist) const {
auto lane_offset_info = GetInfos<RoadInfoLaneWidth>(dist);
std::vector<std::shared_ptr<const RoadInfoLaneWidth>> result;
std::unordered_set<int> inserted_lanes;
for (auto &&lane_offset : lane_offset_info) {
const int lane_id = lane_offset->GetLaneId();
const bool is_new_lane = inserted_lanes.insert(lane_id).second;
if (is_new_lane) {
result.emplace_back(lane_offset);
}
}
return result;
}
void PredEmplaceBack(RoadSegment *s) {
_predecessors.emplace_back(s);
}
void SuccEmplaceBack(RoadSegment *s) {
_successors.emplace_back(s);
}
bool HaveSuccessors() const {
return _successors.size() > 0;
}
bool HavePredecessors() const {
return _predecessors.size() > 0;
}
std::vector<id_type> GetSuccessorsIds() const {
std::vector<id_type> s_vec;
for (auto &&succ : _successors) {
s_vec.emplace_back(succ->GetId());
}
return s_vec;
}
std::vector<bool> GetSuccessorsIsSTart() const {
return _successors_is_start;
}
std::vector<id_type> GetPredecessorsIds() const {
std::vector<id_type> p_vec;
for (auto &&pred : _predecessors) {
p_vec.emplace_back(pred->GetId());
}
return p_vec;
}
std::vector<bool> GetPredecessorsIsStart() const {
return _predecessors_is_start;
}
const std::vector<RoadSegment *> GetSuccessors() const {
return _successors;
}
const std::vector<RoadSegment *> GetPredecessors() const {
return _predecessors;
}
/// Given the current lane it gives an std::pair vector with the lane id and road
/// id where you can go. First integer of the pair is the lane id and the second
/// the road id.
///
/// @param 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
/// 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 and road
/// id where you can go. First integer of the pair is the lane id and the second
/// the road id.
///
/// @param 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
/// 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);
return it == _prev_lane.end() ? std::vector<std::pair<int, int>>() : it->second;
}
// Search for the last geometry with less start_offset before 'dist'
DirectedPoint GetDirectedPointIn(double dist) const {
DEBUG_ASSERT(!_geom.empty());
DEBUG_ASSERT(_length > 0.0);
if (dist <= 0.0) {
DEBUG_ASSERT(_geom.front() != nullptr);
return DirectedPointWithElevation(
dist, DirectedPoint(
_geom.front()->GetStartPosition(),
_geom.front()->GetHeading()));
}
if (dist >= _length) {
DEBUG_ASSERT(_geom.back() != nullptr);
return DirectedPointWithElevation(
dist,
_geom.back()->PosFromDist(_length - _geom.back()->GetStartOffset()));
}
for (auto &&g : _geom) {
if ((g->GetStartOffset() < dist) && (dist <= (g->GetStartOffset() + g->GetLength()))) {
return DirectedPointWithElevation(
dist,
g->PosFromDist(dist - g->GetStartOffset()));
}
}
throw_exception(std::runtime_error("failed to find geometry."));
}
/// 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) {
DEBUG_ASSERT(*g != nullptr);
auto d = (*g)->DistanceTo(loc);
if (d.second < last.second) {
last = d;
nearest_geom = g;
}
}
for (auto g = _geom.begin(); g != nearest_geom; ++g) {
DEBUG_ASSERT(*g != nullptr);
last.first += (*g)->GetLength();
}
return last;
}
/// 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
std::pair<int, double> GetNearestLane(double dist, const geom::Location &loc) const {
const DirectedPoint dp_center_road = GetDirectedPointIn(dist);
auto info = GetInfo<RoadInfoLane>(0.0);
int nearest_lane_id = 0;
double nearest_dist = std::numeric_limits<double>::max();
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);
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, loc);
if (current_dist < nearest_dist) {
nearest_dist = current_dist;
nearest_lane_id = current_lane_id;
}
}
}
return std::pair<int, double>(nearest_lane_id, nearest_dist);
}
const double &GetLength() const {
return _length;
}
void SetLength(double d) {
_length = d;
}
private:
DirectedPoint DirectedPointWithElevation(double dist, DirectedPoint dp) const {
const auto elev_info = GetInfo<RoadElevationInfo>(dist);
if (elev_info) {
dp.location.z = elev_info->Evaluate(dist, &dp.pitch);
}
return dp;
}
friend class carla::road::MapBuilder;
struct LessComp {
using is_transparent = void;
bool operator()(
const std::shared_ptr<RoadInfo> &a,
const std::shared_ptr<RoadInfo> &b) const {
return a->d < b->d;
}
bool operator()(
const double &a,
const std::shared_ptr<RoadInfo> &b) const {
return a < b->d;
}
bool operator()(
const std::shared_ptr<RoadInfo> &a,
const double &b) const {
return a->d < b;
}
};
private:
friend class MapBuilder;
id_type _id;
std::vector<RoadSegment *> _predecessors;
std::vector<RoadSegment *> _successors;
std::vector<bool> _successors_is_start;
std::vector<bool> _predecessors_is_start;
std::vector<std::unique_ptr<Geometry>> _geom;
std::multiset<std::shared_ptr<RoadInfo>, LessComp> _info;
double _length = -1.0;
// first int current lane
// second int to which lane
// third int to which road
std::map<int, std::vector<std::pair<int, int>>> _next_lane;
std::map<int, std::vector<std::pair<int, int>>> _prev_lane;
};
} // namespace element
} // namespace road
} // namespace carla

2
LibCarla/source/test/client/test_opendrive.cpp
View File

@ -261,7 +261,7 @@ void print_roads(boost::optional<Map>& map, std::string filename) {
for (auto &section : road.second.GetLaneSections()) {
file << " Section: " << section.GetId() << " " << section.GetDistance() << std::endl;
for (auto &lane : section.GetLanes()) {
file << " Lane: " << lane.second.GetId() << " (" << lane.second.GetType() << ")" << std::endl;
file << " Lane: " << lane.second.GetId() << " (" << static_cast<uint32_t>(lane.second.GetType()) << ")" << std::endl;
file << " Nexts: ";
for (auto link : lane.second.GetNextLanes()) {
if (link != nullptr) {

30
PythonAPI/source/libcarla/Map.cpp
View File

@ -56,13 +56,39 @@ static carla::geom::GeoLocation ToGeolocation(
void export_map() {
using namespace boost::python;
namespace cc = carla::client;
namespace cr = carla::road;
namespace cg = carla::geom;
enum_<cr::Lane::LaneType>("LaneType")
.value("NONE", cr::Lane::LaneType::None) // None is reserved in Python3
.value("Driving", cr::Lane::LaneType::Driving)
.value("Stop", cr::Lane::LaneType::Stop)
.value("Shoulder", cr::Lane::LaneType::Shoulder)
.value("Biking", cr::Lane::LaneType::Biking)
.value("Sidewalk", cr::Lane::LaneType::Sidewalk)
.value("Border", cr::Lane::LaneType::Border)
.value("Restricted", cr::Lane::LaneType::Restricted)
.value("Parking", cr::Lane::LaneType::Parking)
.value("Bidirectional", cr::Lane::LaneType::Bidirectional)
.value("Median", cr::Lane::LaneType::Median)
.value("Special1", cr::Lane::LaneType::Special1)
.value("Special2", cr::Lane::LaneType::Special2)
.value("Special3", cr::Lane::LaneType::Special3)
.value("RoadWorks", cr::Lane::LaneType::RoadWorks)
.value("Tram", cr::Lane::LaneType::Tram)
.value("Rail", cr::Lane::LaneType::Rail)
.value("Entry", cr::Lane::LaneType::Entry)
.value("Exit", cr::Lane::LaneType::Exit)
.value("OffRamp", cr::Lane::LaneType::OffRamp)
.value("OnRamp", cr::Lane::LaneType::OnRamp)
.value("Any", cr::Lane::LaneType::Any)
;
class_<cc::Map, boost::noncopyable, boost::shared_ptr<cc::Map>>("Map", no_init)
.def(init<std::string, std::string>((arg("name"), arg("xodr_content"))))
.add_property("name", CALL_RETURNING_COPY(cc::Map, GetName))
.def("get_spawn_points", CALL_RETURNING_LIST(cc::Map, GetRecommendedSpawnPoints))
.def("get_waypoint", &cc::Map::GetWaypoint, (arg("location"), arg("project_to_road")=true))
.def("get_waypoint", &cc::Map::GetWaypoint, (arg("location"), arg("project_to_road")=true, arg("lane_type")=cr::Lane::LaneType::Driving))
.def("get_topology", &GetTopology)
.def("generate_waypoints", CALL_RETURNING_LIST_1(cc::Map, GenerateWaypoints, double), (args("distance")))
.def("transform_to_geolocation", &ToGeolocation, (arg("location")))
@ -72,7 +98,7 @@ void export_map() {
;
enum_<cc::Waypoint::LaneChange>("LaneChange")
.value("None", cc::Waypoint::LaneChange::None)
.value("NONE", cc::Waypoint::LaneChange::None) // None is reserved in Python3
.value("Right", cc::Waypoint::LaneChange::Right)
.value("Left", cc::Waypoint::LaneChange::Left)
.value("Both", cc::Waypoint::LaneChange::Both)