Added lane types to API
This commit is contained in:
parent
5c9e317f04
commit
dd4897faa4
|
@ -33,12 +33,13 @@ namespace client {
|
||||||
|
|
||||||
SharedPtr<Waypoint> Map::GetWaypoint(
|
SharedPtr<Waypoint> Map::GetWaypoint(
|
||||||
const geom::Location &location,
|
const geom::Location &location,
|
||||||
bool project_to_road) const {
|
bool project_to_road,
|
||||||
|
uint32_t lane_type) const {
|
||||||
boost::optional<road::element::Waypoint> waypoint;
|
boost::optional<road::element::Waypoint> waypoint;
|
||||||
if (project_to_road) {
|
if (project_to_road) {
|
||||||
waypoint = _map.GetClosestWaypointOnRoad(location);
|
waypoint = _map.GetClosestWaypointOnRoad(location, lane_type);
|
||||||
} else {
|
} else {
|
||||||
waypoint = _map.GetWaypoint(location);
|
waypoint = _map.GetWaypoint(location, lane_type);
|
||||||
}
|
}
|
||||||
return waypoint.has_value() ?
|
return waypoint.has_value() ?
|
||||||
SharedPtr<Waypoint>(new Waypoint{shared_from_this(), *waypoint}) :
|
SharedPtr<Waypoint>(new Waypoint{shared_from_this(), *waypoint}) :
|
||||||
|
|
|
@ -11,6 +11,7 @@
|
||||||
#include "carla/road/Map.h"
|
#include "carla/road/Map.h"
|
||||||
#include "carla/road/element/LaneMarking.h"
|
#include "carla/road/element/LaneMarking.h"
|
||||||
#include "carla/rpc/MapInfo.h"
|
#include "carla/rpc/MapInfo.h"
|
||||||
|
#include "carla/road/Lane.h"
|
||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
@ -47,7 +48,8 @@ namespace client {
|
||||||
|
|
||||||
SharedPtr<Waypoint> GetWaypoint(
|
SharedPtr<Waypoint> GetWaypoint(
|
||||||
const geom::Location &location,
|
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>>>;
|
using TopologyList = std::vector<std::pair<SharedPtr<Waypoint>, SharedPtr<Waypoint>>>;
|
||||||
|
|
||||||
|
|
|
@ -27,9 +27,10 @@ namespace client {
|
||||||
|
|
||||||
double Waypoint::GetLaneWidth() const {
|
double Waypoint::GetLaneWidth() const {
|
||||||
return _parent->GetMap().GetLaneWidth(_waypoint);
|
return _parent->GetMap().GetLaneWidth(_waypoint);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string Waypoint::GetType() const {
|
road::Lane::LaneType Waypoint::GetType() const {
|
||||||
return _parent->GetMap().GetLaneType(_waypoint);
|
return _parent->GetMap().GetLaneType(_waypoint);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -11,6 +11,7 @@
|
||||||
#include "carla/geom/Transform.h"
|
#include "carla/geom/Transform.h"
|
||||||
#include "carla/road/element/RoadInfoMarkRecord.h"
|
#include "carla/road/element/RoadInfoMarkRecord.h"
|
||||||
#include "carla/road/element/Waypoint.h"
|
#include "carla/road/element/Waypoint.h"
|
||||||
|
#include "carla/road/Lane.h"
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace client {
|
namespace client {
|
||||||
|
@ -64,7 +65,7 @@ namespace client {
|
||||||
|
|
||||||
double GetLaneWidth() const;
|
double GetLaneWidth() const;
|
||||||
|
|
||||||
std::string GetType() const;
|
road::Lane::LaneType GetType() const;
|
||||||
|
|
||||||
std::vector<SharedPtr<Waypoint>> Next(double distance) const;
|
std::vector<SharedPtr<Waypoint>> Next(double distance) const;
|
||||||
|
|
||||||
|
|
|
@ -9,6 +9,7 @@
|
||||||
#include "carla/road/MapBuilder.h"
|
#include "carla/road/MapBuilder.h"
|
||||||
#include "carla/road/RoadTypes.h"
|
#include "carla/road/RoadTypes.h"
|
||||||
#include "carla/Logging.h"
|
#include "carla/Logging.h"
|
||||||
|
#include "carla/StringUtil.h"
|
||||||
#include <deque>
|
#include <deque>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
|
@ -26,7 +27,7 @@ namespace parser {
|
||||||
|
|
||||||
struct Lane {
|
struct Lane {
|
||||||
LaneId id;
|
LaneId id;
|
||||||
std::string type;
|
road::Lane::LaneType type;
|
||||||
bool level;
|
bool level;
|
||||||
LaneId predecessor;
|
LaneId predecessor;
|
||||||
LaneId successor;
|
LaneId successor;
|
||||||
|
@ -61,6 +62,52 @@ namespace parser {
|
||||||
std::vector<LaneSection> sections;
|
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(
|
void RoadParser::Parse(
|
||||||
const pugi::xml_document &xml,
|
const pugi::xml_document &xml,
|
||||||
carla::road::MapBuilder &map_builder) {
|
carla::road::MapBuilder &map_builder) {
|
||||||
|
@ -122,10 +169,10 @@ namespace parser {
|
||||||
|
|
||||||
// left lanes
|
// left lanes
|
||||||
for (pugi::xml_node node_lane : node_section.child("left").children("lane")) {
|
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.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();
|
lane.level = node_lane.attribute("level").as_bool();
|
||||||
|
|
||||||
// link
|
// link
|
||||||
|
@ -143,10 +190,10 @@ namespace parser {
|
||||||
|
|
||||||
// center lane
|
// center lane
|
||||||
for (pugi::xml_node node_lane : node_section.child("center").children("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.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();
|
lane.level = node_lane.attribute("level").as_bool();
|
||||||
|
|
||||||
// link (probably it never exists)
|
// link (probably it never exists)
|
||||||
|
@ -164,10 +211,10 @@ namespace parser {
|
||||||
|
|
||||||
// right lane
|
// right lane
|
||||||
for (pugi::xml_node node_lane : node_section.child("right").children("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.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();
|
lane.level = node_lane.attribute("level").as_bool();
|
||||||
|
|
||||||
// link
|
// link
|
||||||
|
@ -245,7 +292,7 @@ namespace parser {
|
||||||
|
|
||||||
// lanes
|
// lanes
|
||||||
for (auto const l : s.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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -27,7 +27,7 @@ namespace road {
|
||||||
return _id;
|
return _id;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string Lane::GetType() const {
|
Lane::LaneType Lane::GetType() const {
|
||||||
return _type;
|
return _type;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -21,6 +21,34 @@ namespace road {
|
||||||
class Road;
|
class Road;
|
||||||
|
|
||||||
class Lane : private MovableNonCopyable {
|
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:
|
public:
|
||||||
|
|
||||||
Lane() = default;
|
Lane() = default;
|
||||||
|
@ -42,7 +70,7 @@ namespace road {
|
||||||
|
|
||||||
LaneId GetId() const;
|
LaneId GetId() const;
|
||||||
|
|
||||||
std::string GetType() const;
|
LaneType GetType() const;
|
||||||
|
|
||||||
bool GetLevel() const;
|
bool GetLevel() const;
|
||||||
|
|
||||||
|
@ -85,8 +113,7 @@ namespace road {
|
||||||
|
|
||||||
InformationSet _info;
|
InformationSet _info;
|
||||||
|
|
||||||
/// @todo: change to enum, see 6.5 of OpenDRIVEFormatSpecRev1.4H.pdf
|
LaneType _type;
|
||||||
std::string _type;
|
|
||||||
|
|
||||||
bool _level;
|
bool _level;
|
||||||
|
|
||||||
|
|
|
@ -37,10 +37,10 @@ namespace road {
|
||||||
return _lanes;
|
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;
|
std::vector<Lane *> drivable_lanes;
|
||||||
for (auto &&lane : _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);
|
drivable_lanes.emplace_back(&lane.second);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -42,7 +42,7 @@ namespace road {
|
||||||
|
|
||||||
const std::map<LaneId, Lane> &GetLanes() const;
|
const std::map<LaneId, Lane> &GetLanes() const;
|
||||||
|
|
||||||
std::vector<Lane *> GetLanesOfType(const std::string &type);
|
std::vector<Lane *> GetLanesOfType(Lane::LaneType type);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
|
@ -55,12 +55,13 @@ namespace road {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/// Return a waypoint for each drivable lane on @a lane_section.
|
/// Return a waypoint for each drivable lane on @a lane_section.
|
||||||
template <typename FuncT>
|
template <typename FuncT>
|
||||||
static void ForEachDrivableLane(RoadId road_id, const LaneSection &lane_section, FuncT &&func) {
|
static void ForEachDrivableLane(RoadId road_id, const LaneSection &lane_section, FuncT &&func) {
|
||||||
for (const auto &pair : lane_section.GetLanes()) {
|
for (const auto &pair : lane_section.GetLanes()) {
|
||||||
const auto &lane = pair.second;
|
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)});
|
std::forward<FuncT>(func)(Waypoint{road_id, lane_section.GetId(), lane.GetId(), GetDistanceAtStartOfLane(lane)});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -118,10 +119,10 @@ namespace road {
|
||||||
// -- Map: Geometry ----------------------------------------------------------
|
// -- 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
|
// max_nearests represents the max nearests roads
|
||||||
// where we will search for nearests lanes
|
// 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,
|
// in case that map has less than max_nearests lanes,
|
||||||
// we will use the maximum lanes
|
// we will use the maximum lanes
|
||||||
const int max_nearest_allowed = _data.GetRoadCount() < max_nearests ?
|
const int max_nearest_allowed = _data.GetRoadCount() < max_nearests ?
|
||||||
|
@ -164,7 +165,7 @@ namespace road {
|
||||||
Waypoint waypoint;
|
Waypoint waypoint;
|
||||||
auto nearest_lane_dist = std::numeric_limits<double>::max();
|
auto nearest_lane_dist = std::numeric_limits<double>::max();
|
||||||
for (int i = 0; i < max_nearest_allowed; ++i) {
|
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) {
|
if (lane_dist.second < nearest_lane_dist) {
|
||||||
nearest_lane_dist = lane_dist.second;
|
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);
|
const auto &road = _data.GetRoad(waypoint.road_id);
|
||||||
|
|
||||||
// Make sure 0.0 < waipoint.s < Road's length
|
// Make sure 0.0 < waipoint.s < Road's length
|
||||||
|
@ -187,12 +192,19 @@ namespace road {
|
||||||
return waypoint;
|
return waypoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
boost::optional<Waypoint> Map::GetWaypoint(const geom::Location &pos) const {
|
boost::optional<Waypoint> Map::GetWaypoint(
|
||||||
Waypoint w = GetClosestWaypointOnRoad(pos);
|
const geom::Location &pos,
|
||||||
const auto dist = geom::Math::Distance2D(ComputeTransform(w).location, pos);
|
uint32_t lane_type) const {
|
||||||
const auto lane_width_info = GetLane(w).GetInfo<RoadInfoLaneWidth>(w.s);
|
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 =
|
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) {
|
if (dist < half_lane_width) {
|
||||||
return w;
|
return w;
|
||||||
|
@ -273,7 +285,7 @@ namespace road {
|
||||||
// -- Map: Road information --------------------------------------------------
|
// -- Map: Road information --------------------------------------------------
|
||||||
// ===========================================================================
|
// ===========================================================================
|
||||||
|
|
||||||
std::string Map::GetLaneType(const Waypoint waypoint) const {
|
Lane::LaneType Map::GetLaneType(const Waypoint waypoint) const {
|
||||||
return GetLane(waypoint).GetType();
|
return GetLane(waypoint).GetType();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -44,9 +44,13 @@ namespace road {
|
||||||
/// -- Geometry ------------------------------------------------------------
|
/// -- 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;
|
geom::Transform ComputeTransform(Waypoint waypoint) const;
|
||||||
|
|
||||||
|
@ -54,7 +58,7 @@ namespace road {
|
||||||
/// -- Road information ----------------------------------------------------
|
/// -- Road information ----------------------------------------------------
|
||||||
/// ========================================================================
|
/// ========================================================================
|
||||||
|
|
||||||
std::string GetLaneType(Waypoint waypoint) const;
|
Lane::LaneType GetLaneType(Waypoint waypoint) const;
|
||||||
|
|
||||||
double GetLaneWidth(Waypoint waypoint) const;
|
double GetLaneWidth(Waypoint waypoint) const;
|
||||||
|
|
||||||
|
|
|
@ -281,7 +281,7 @@ namespace road {
|
||||||
carla::road::Lane *MapBuilder::AddRoadSectionLane(
|
carla::road::Lane *MapBuilder::AddRoadSectionLane(
|
||||||
carla::road::LaneSection *section,
|
carla::road::LaneSection *section,
|
||||||
const int32_t lane_id,
|
const int32_t lane_id,
|
||||||
const std::string lane_type,
|
const uint32_t lane_type,
|
||||||
const bool lane_level,
|
const bool lane_level,
|
||||||
const int32_t predecessor,
|
const int32_t predecessor,
|
||||||
const int32_t successor) {
|
const int32_t successor) {
|
||||||
|
@ -293,7 +293,7 @@ namespace road {
|
||||||
lane->_id = lane_id;
|
lane->_id = lane_id;
|
||||||
lane->_lane_section = section;
|
lane->_lane_section = section;
|
||||||
lane->_level = lane_level;
|
lane->_level = lane_level;
|
||||||
lane->_type = lane_type;
|
lane->_type = static_cast<carla::road::Lane::LaneType>(lane_type);
|
||||||
lane->_successor = successor;
|
lane->_successor = successor;
|
||||||
lane->_predecessor = predecessor;
|
lane->_predecessor = predecessor;
|
||||||
|
|
||||||
|
|
|
@ -37,7 +37,7 @@ namespace road {
|
||||||
carla::road::Lane *AddRoadSectionLane(
|
carla::road::Lane *AddRoadSectionLane(
|
||||||
carla::road::LaneSection *section,
|
carla::road::LaneSection *section,
|
||||||
const int32_t lane_id,
|
const int32_t lane_id,
|
||||||
const std::string lane_type,
|
const uint32_t lane_type,
|
||||||
const bool lane_level,
|
const bool lane_level,
|
||||||
const int32_t predecessor,
|
const int32_t predecessor,
|
||||||
const int32_t successor);
|
const int32_t successor);
|
||||||
|
@ -190,7 +190,7 @@ namespace road {
|
||||||
const uint32_t road_id,
|
const uint32_t road_id,
|
||||||
const int32_t section_index,
|
const int32_t section_index,
|
||||||
const int32_t lane_id,
|
const int32_t lane_id,
|
||||||
const std::string lane_type,
|
const Lane::LaneType lane_type,
|
||||||
const bool lane_level,
|
const bool lane_level,
|
||||||
const int32_t predecessor,
|
const int32_t predecessor,
|
||||||
const int32_t successor);
|
const int32_t successor);
|
||||||
|
|
|
@ -217,7 +217,8 @@ namespace road {
|
||||||
|
|
||||||
const std::pair<const Lane *, double> Road::GetNearestLane(
|
const std::pair<const Lane *, double> Road::GetNearestLane(
|
||||||
const double s,
|
const double s,
|
||||||
const geom::Location &loc) const {
|
const geom::Location &loc,
|
||||||
|
uint32_t lane_type) const {
|
||||||
using namespace carla::road::element;
|
using namespace carla::road::element;
|
||||||
std::map<LaneId, const Lane *> lanes(GetLanesAt(s));
|
std::map<LaneId, const Lane *> lanes(GetLanesAt(s));
|
||||||
// negative right lanes
|
// negative right lanes
|
||||||
|
@ -231,41 +232,42 @@ namespace road {
|
||||||
std::pair<const Lane *, double> result =
|
std::pair<const Lane *, double> result =
|
||||||
std::make_pair(nullptr, std::numeric_limits<double>::max());
|
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;
|
DirectedPoint current_dp = dp_lane_zero;
|
||||||
for (const auto &lane : right_lanes) {
|
for (const auto &lane : right_lanes) {
|
||||||
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
if ((static_cast<uint32_t>(lane.second->GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
|
||||||
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
|
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
|
||||||
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
// if the current_dp is near to loc, we are in the right way
|
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
||||||
if (current_dist <= result.second) {
|
// if the current_dp is near to loc, we are in the right way
|
||||||
result.first = &(*lane.second);
|
if (current_dist <= result.second) {
|
||||||
result.second = current_dist;
|
result.first = &(*lane.second);
|
||||||
} else {
|
result.second = current_dist;
|
||||||
// elsewhere, we are be moving away
|
} else {
|
||||||
break;
|
// elsewhere, we are be moving away
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
}
|
}
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
current_dp = dp_lane_zero;
|
current_dp = dp_lane_zero;
|
||||||
for (const auto &lane : left_lanes) {
|
for (const auto &lane : left_lanes) {
|
||||||
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
if ((static_cast<uint32_t>(lane.second->GetType()) & static_cast<uint32_t>(lane_type)) > 0) {
|
||||||
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
|
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) / 2.0;
|
||||||
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
// if the current_dp is near to loc, we are in the right way
|
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
||||||
if (current_dist <= result.second) {
|
// if the current_dp is near to loc, we are in the right way
|
||||||
result.first = &(*lane.second);
|
if (current_dist <= result.second) {
|
||||||
result.second = current_dist;
|
result.first = &(*lane.second);
|
||||||
} else {
|
result.second = current_dist;
|
||||||
// elsewhere, we are be moving away
|
} else {
|
||||||
break;
|
// elsewhere, we are be moving away
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
}
|
}
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return result;
|
return result;
|
||||||
|
|
|
@ -105,7 +105,9 @@ namespace road {
|
||||||
/// want to calculate the distance
|
/// want to calculate the distance
|
||||||
/// @param loc point to calculate the distance
|
/// @param loc point to calculate the distance
|
||||||
const std::pair<const Lane *, double> GetNearestLane(
|
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>
|
template <typename T>
|
||||||
const T *GetInfo (const double s) const {
|
const T *GetInfo (const double s) const {
|
||||||
|
|
|
@ -22,6 +22,7 @@ namespace road {
|
||||||
using ObjId = uint32_t;
|
using ObjId = uint32_t;
|
||||||
|
|
||||||
using SignId = uint32_t;
|
using SignId = uint32_t;
|
||||||
|
|
||||||
using SignRefId = uint32_t;
|
using SignRefId = uint32_t;
|
||||||
|
|
||||||
using ConId = uint32_t;
|
using ConId = uint32_t;
|
||||||
|
|
|
@ -43,16 +43,16 @@ namespace element {
|
||||||
const geom::Location &destination) {
|
const geom::Location &destination) {
|
||||||
auto w0 = map.GetClosestWaypointOnRoad(origin);
|
auto w0 = map.GetClosestWaypointOnRoad(origin);
|
||||||
auto w1 = map.GetClosestWaypointOnRoad(destination);
|
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.
|
/// @todo This case should also be handled.
|
||||||
return {};
|
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 {};
|
||||||
}
|
}
|
||||||
return CrossingAtSameSection(
|
return CrossingAtSameSection(
|
||||||
w0.lane_id,
|
w0->lane_id,
|
||||||
w1.lane_id,
|
w1->lane_id,
|
||||||
IsOffRoad(map, origin),
|
IsOffRoad(map, origin),
|
||||||
IsOffRoad(map, destination));
|
IsOffRoad(map, destination));
|
||||||
}
|
}
|
||||||
|
|
|
@ -263,7 +263,7 @@ void print_roads(boost::optional<Map>& map, std::string filename) {
|
||||||
for (auto §ion : road.second.GetLaneSections()) {
|
for (auto §ion : road.second.GetLaneSections()) {
|
||||||
file << " Section: " << section.GetId() << " " << section.GetDistance() << std::endl;
|
file << " Section: " << section.GetId() << " " << section.GetDistance() << std::endl;
|
||||||
for (auto &lane : section.GetLanes()) {
|
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: ";
|
file << " Nexts: ";
|
||||||
for (auto link : lane.second.GetNextLanes()) {
|
for (auto link : lane.second.GetNextLanes()) {
|
||||||
if (link != nullptr) {
|
if (link != nullptr) {
|
||||||
|
|
|
@ -56,12 +56,38 @@ static carla::geom::GeoLocation ToGeolocation(
|
||||||
void export_map() {
|
void export_map() {
|
||||||
using namespace boost::python;
|
using namespace boost::python;
|
||||||
namespace cc = carla::client;
|
namespace cc = carla::client;
|
||||||
|
namespace cr = carla::road;
|
||||||
namespace cg = carla::geom;
|
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)
|
class_<cc::Map, boost::noncopyable, boost::shared_ptr<cc::Map>>("Map", no_init)
|
||||||
.add_property("name", CALL_RETURNING_COPY(cc::Map, GetName))
|
.add_property("name", CALL_RETURNING_COPY(cc::Map, GetName))
|
||||||
.def("get_spawn_points", CALL_RETURNING_LIST(cc::Map, GetRecommendedSpawnPoints))
|
.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("get_topology", &GetTopology)
|
||||||
.def("generate_waypoints", CALL_RETURNING_LIST_1(cc::Map, GenerateWaypoints, double), (args("distance")))
|
.def("generate_waypoints", CALL_RETURNING_LIST_1(cc::Map, GenerateWaypoints, double), (args("distance")))
|
||||||
.def("transform_to_geolocation", &ToGeolocation, (arg("location")))
|
.def("transform_to_geolocation", &ToGeolocation, (arg("location")))
|
||||||
|
|
Loading…
Reference in New Issue