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

Added lane types to API

This commit is contained in:
Marc Garcia Puig 2019-03-25 23:52:46 +01:00
parent 5c9e317f04
commit dd4897faa4
19 changed files with 199 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,12 +33,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>
@ -47,7 +48,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,12 +55,13 @@ namespace road {
}
}
/// Return a waypoint for each drivable lane on @a lane_section.
template <typename FuncT>
static void ForEachDrivableLane(RoadId road_id, const LaneSection &lane_section, 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(), lane.GetId(), GetDistanceAtStartOfLane(lane)});
}
}
@ -118,10 +119,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 ?
@ -164,7 +165,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;
@ -174,6 +175,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
@ -187,12 +192,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;
@ -273,7 +285,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);

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

@ -217,7 +217,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
@ -231,41 +232,42 @@ namespace road {
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;
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;
} else {
// elsewhere, we are be moving away
break;
if ((static_cast<uint32_t>(lane.second->GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
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;
} else {
// elsewhere, we are be moving away
break;
}
current_dp.ApplyLateralOffset(half_width);
}
current_dp.ApplyLateralOffset(half_width);
}
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;
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;
} else {
// elsewhere, we are be moving away
break;
if ((static_cast<uint32_t>(lane.second->GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
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;
} else {
// elsewhere, we are be moving away
break;
}
current_dp.ApplyLateralOffset(half_width);
}
current_dp.ApplyLateralOffset(half_width);
}
return result;

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));
}

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

@ -263,7 +263,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) {

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

@ -56,12 +56,38 @@ 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)
.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)
.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")))