From ad2f6f1d52dca8f87e5055386c28ae40fa395a4d Mon Sep 17 00:00:00 2001 From: Marc Date: Thu, 25 Oct 2018 14:45:03 +0200 Subject: [PATCH 1/2] Sorted includes --- LibCarla/source/carla/road/Map.h | 5 +++-- LibCarla/source/carla/road/MapBuilder.cpp | 5 ----- LibCarla/source/carla/road/MapBuilder.h | 4 +--- LibCarla/source/carla/road/element/Geometry.h | 10 +++++++++- LibCarla/source/carla/road/element/RoadSegment.h | 4 ++-- LibCarla/source/carla/road/element/Waypoint.h | 10 +++++++--- 6 files changed, 22 insertions(+), 16 deletions(-) diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 3601524f8..61c5f1bf5 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -6,11 +6,12 @@ #pragma once +#include "carla/geom/Math.h" #include "carla/Memory.h" #include "carla/NonCopyable.h" #include "carla/Optional.h" -#include "carla/road/MapData.h" #include "carla/road/element/Waypoint.h" +#include "carla/road/MapData.h" namespace carla { namespace road { @@ -33,7 +34,7 @@ namespace road { } Map(MapData m) - : _data(std::move(m)) {} + : _data(std::move(m)) {} private: diff --git a/LibCarla/source/carla/road/MapBuilder.cpp b/LibCarla/source/carla/road/MapBuilder.cpp index 6313cd2a4..cc06f1ffd 100644 --- a/LibCarla/source/carla/road/MapBuilder.cpp +++ b/LibCarla/source/carla/road/MapBuilder.cpp @@ -47,10 +47,5 @@ namespace road { return SharedPtr(new Map{std::move(_map_data)}); } - bool MapBuilder::InterpretRoadFlow() { - // todo - return false; - } - } // namespace road } // namespace carla diff --git a/LibCarla/source/carla/road/MapBuilder.h b/LibCarla/source/carla/road/MapBuilder.h index d62601f77..98b4b02b7 100644 --- a/LibCarla/source/carla/road/MapBuilder.h +++ b/LibCarla/source/carla/road/MapBuilder.h @@ -6,8 +6,8 @@ #pragma once -#include "carla/road/Map.h" #include "carla/geom/Location.h" +#include "carla/road/Map.h" #include @@ -31,8 +31,6 @@ namespace road { return r; } - bool InterpretRoadFlow(); - private: MapData _map_data; diff --git a/LibCarla/source/carla/road/element/Geometry.h b/LibCarla/source/carla/road/element/Geometry.h index 1e05c0ccb..dfa0d03ab 100644 --- a/LibCarla/source/carla/road/element/Geometry.h +++ b/LibCarla/source/carla/road/element/Geometry.h @@ -6,8 +6,8 @@ #pragma once -#include "carla/geom/Math.h" #include "carla/geom/Location.h" +#include "carla/geom/Math.h" #include "carla/opendrive/logic/cephes/fresnel.h" #include @@ -44,6 +44,14 @@ namespace element { return d; } + void ApplyLateralOffset(double lateral_offset) { + /// @todo Z axis?? + auto normal_x = std::sin(tangent); + auto normal_y = -std::cos(tangent); + location.x += lateral_offset * normal_x; + location.y += lateral_offset * normal_y; + } + friend bool operator==(const DirectedPoint &lhs, const DirectedPoint &rhs) { return (lhs.location == rhs.location) && (lhs.tangent == rhs.tangent); } diff --git a/LibCarla/source/carla/road/element/RoadSegment.h b/LibCarla/source/carla/road/element/RoadSegment.h index 6c7dfdf14..88fb85f15 100644 --- a/LibCarla/source/carla/road/element/RoadSegment.h +++ b/LibCarla/source/carla/road/element/RoadSegment.h @@ -6,9 +6,9 @@ #pragma once -#include "carla/road/element/Types.h" -#include "carla/road/element/RoadInfo.h" #include "carla/geom/Location.h" +#include "carla/road/element/RoadInfo.h" +#include "carla/road/element/Types.h" #include #include diff --git a/LibCarla/source/carla/road/element/Waypoint.h b/LibCarla/source/carla/road/element/Waypoint.h index 62ad814c1..e20cab942 100644 --- a/LibCarla/source/carla/road/element/Waypoint.h +++ b/LibCarla/source/carla/road/element/Waypoint.h @@ -6,8 +6,8 @@ #pragma once -#include "carla/Memory.h" #include "carla/geom/Transform.h" +#include "carla/Memory.h" #include "carla/road/element/RoadInfoList.h" #include "carla/road/element/Types.h" @@ -21,8 +21,6 @@ namespace element { class Waypoint { public: - Waypoint() = default; - ~Waypoint(); const geom::Transform &GetTransform() const { @@ -42,12 +40,18 @@ namespace element { private: + friend carla::road::Map; + + Waypoint() = default; + SharedPtr _map; geom::Transform _transform; id_type _road_id = 0; + int _lane_id = 0; + double _dist = 0.0; }; From ee1eebe0cef854089f3f86db891ce91e25881842 Mon Sep 17 00:00:00 2001 From: Marc Date: Fri, 26 Oct 2018 19:03:32 +0200 Subject: [PATCH 2/2] Improved Waypoint interface --- LibCarla/source/carla/geom/Location.h | 2 +- LibCarla/source/carla/geom/Math.h | 48 ++++++++++++++----- LibCarla/source/carla/road/Map.cpp | 32 +++++++++++++ LibCarla/source/carla/road/Map.h | 20 +++----- LibCarla/source/carla/road/MapData.h | 4 ++ LibCarla/source/carla/road/element/Geometry.h | 30 ++++++++++++ LibCarla/source/carla/road/element/RoadInfo.h | 8 ++-- .../source/carla/road/element/RoadSegment.h | 40 +++++++++++++++- .../source/carla/road/element/Waypoint.cpp | 32 +++++++++++++ LibCarla/source/carla/road/element/Waypoint.h | 7 ++- LibCarla/source/test/test_geom.cpp | 26 +++++----- 11 files changed, 205 insertions(+), 44 deletions(-) create mode 100644 LibCarla/source/carla/road/Map.cpp diff --git a/LibCarla/source/carla/geom/Location.h b/LibCarla/source/carla/geom/Location.h index 9a7e3a438..eb9114e30 100644 --- a/LibCarla/source/carla/geom/Location.h +++ b/LibCarla/source/carla/geom/Location.h @@ -15,7 +15,7 @@ namespace carla { namespace geom { - class Location : private Vector3D { + class Location : public Vector3D { public: Location() = default; diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index 68183b58e..7cc9a5f2a 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -6,9 +6,10 @@ #pragma once -#include "carla/geom/Vector3D.h" #include "carla/Debug.h" +#include "carla/geom/Vector3D.h" +#include #include namespace carla { @@ -29,8 +30,19 @@ namespace geom { return 0.5 * pi(); } + static constexpr auto to_radiants() { + return 180.0 / pi(); + } + + static double clamp( + const double &a, + const double &min, + const double &max) { + return std::min(std::max(a, min), max); + } + static double clamp01(double a) { - return std::min(std::max(a, 0.0), 1.0); + return clamp(a, 0.0, 1.0); } template @@ -62,17 +74,25 @@ namespace geom { return std::sqrt(DistanceSquared2D(a, b)); } - static double DistanceSegmentPoint( + /// 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' + /// @param p point to calculate distance + /// @param v first point of the segment + /// @param w second point of the segment + static std::pair DistSegmentPoint( const Vector3D &p, const Vector3D &v, const Vector3D &w) { const double l2 = DistanceSquared2D(v, w); + const double l = std::sqrt(l2); if (l2 == 0.0) { - return Distance2D(p, v); + return std::make_pair(0.0, Distance2D(v, p)); } - const double t = clamp01(Dot2D(p - v, w - v) / l2); + const double dot_p_w = Dot2D(p - v, w - v); + const double t = clamp01(dot_p_w / l2); const Vector3D projection = v + t * (w - v); - return Distance2D(p, projection); + return std::make_pair(t * l, Distance2D(projection, p)); } static Vector3D RotatePointOnOrigin2D(Vector3D p, double angle) { @@ -81,7 +101,10 @@ namespace geom { return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0); } - static double DistanceArcPoint( + /// 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' + static std::pair DistArcPoint( const Vector3D &p, const Vector3D &start_pos, const double length, @@ -99,7 +122,7 @@ namespace geom { // check if the point is in the center of the circle, so we know p // is in the same distance of every possible point in the arc if (rotated_p == circ_center) { - return radius; + return std::make_pair(0.0, radius); } // use the arc length to calculate the angle in the last point of it @@ -107,7 +130,6 @@ namespace geom { const double circumf = 2 * pi() * radius; const double last_point_angle = (length / circumf) * pi_double(); - // move the point relative to the center of the circle and find // the angle between the point and the center of coords in rad const double angle = std::atan2(intersection.y - radius, intersection.x) + pi_half(); @@ -115,7 +137,9 @@ namespace geom { // see if the angle is between 0 and last_point_angle DEBUG_ASSERT(angle >= 0); if (angle <= last_point_angle) { - return Distance2D(intersection, rotated_p); + return std::make_pair( + angle * radius, + Distance2D(intersection, rotated_p)); } // find the nearest point, start or end to intersection @@ -124,7 +148,9 @@ namespace geom { (radius * std::cos(last_point_angle - pi_half())) + circ_center.x, (radius * std::sin(last_point_angle - pi_half())) + circ_center.y, 0.0); const double end_dist = Distance2D(end_pos, rotated_p); - return (start_dist < end_dist) ? start_dist : end_dist; + return (start_dist < end_dist) ? + std::make_pair(0.0, start_dist) : + std::make_pair(length, end_dist); } static bool PointInRectangle( diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp new file mode 100644 index 000000000..8d226307d --- /dev/null +++ b/LibCarla/source/carla/road/Map.cpp @@ -0,0 +1,32 @@ +// 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 . + +#include "carla/geom/Math.h" +#include "carla/road/Map.h" +#include "carla/road/element/Types.h" + +#include + + +namespace carla { +namespace road { + + using namespace element; + + Waypoint Map::GetClosestWaypointOnRoad(const geom::Location &loc) const { + return Waypoint(shared_from_this(), loc); + } + + Optional Map::GetWaypoint(const geom::Location &) const { + return Optional(); + } + + const MapData &Map::GetData() const { + return _data; + } + +} // namespace road +} // namespace carla diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 61c5f1bf5..ae8ff920e 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -6,7 +6,6 @@ #pragma once -#include "carla/geom/Math.h" #include "carla/Memory.h" #include "carla/NonCopyable.h" #include "carla/Optional.h" @@ -19,23 +18,18 @@ namespace road { class Map : public EnableSharedFromThis, private MovableNonCopyable { + public: - element::Waypoint GetClosestWaypointOnRoad(const geom::Location &) const { - return element::Waypoint(); - } - - Optional GetWaypoint(const geom::Location &) const { - return Optional(); - } - - const MapData &GetData() { - return _data; - } - Map(MapData m) : _data(std::move(m)) {} + element::Waypoint GetClosestWaypointOnRoad(const geom::Location &) const; + + Optional GetWaypoint(const geom::Location &) const; + + const MapData &GetData() const; + private: MapData _data; diff --git a/LibCarla/source/carla/road/MapData.h b/LibCarla/source/carla/road/MapData.h index 1c625e8e3..fd2128320 100644 --- a/LibCarla/source/carla/road/MapData.h +++ b/LibCarla/source/carla/road/MapData.h @@ -13,6 +13,9 @@ namespace carla { namespace road { +namespace element { + class Waypoint; +} class MapData : private MovableNonCopyable { @@ -32,6 +35,7 @@ namespace road { friend class MapBuilder; friend class Map; + friend element::Waypoint; MapData() = default; diff --git a/LibCarla/source/carla/road/element/Geometry.h b/LibCarla/source/carla/road/element/Geometry.h index dfa0d03ab..bef68d568 100644 --- a/LibCarla/source/carla/road/element/Geometry.h +++ b/LibCarla/source/carla/road/element/Geometry.h @@ -9,6 +9,7 @@ #include "carla/geom/Location.h" #include "carla/geom/Math.h" #include "carla/opendrive/logic/cephes/fresnel.h" +#include "carla/Debug.h" #include @@ -81,6 +82,8 @@ namespace element { virtual const DirectedPoint PosFromDist(double dist) const = 0; + virtual std::pair DistanceTo(const geom::Location &p) const = 0; + protected: Geometry( @@ -125,6 +128,14 @@ namespace element { p.location.y += dist * std::sin(p.tangent); return p; } + + std::pair DistanceTo(const geom::Location &p) const override { + return geom::Math::DistSegmentPoint( + p, + _start_position, + PosFromDist(_length).location); + } + }; class GeometryArc : public Geometry { @@ -153,6 +164,20 @@ namespace element { return p; } + std::pair DistanceTo(const geom::Location &p) const override { + /*const Vector3D &p, + const Vector3D &start_pos, + const double length, + const double heading, // [radians] + const double curvature*/ + return geom::Math::DistArcPoint( + p, + _start_position, + _length, + _heading, + _curvature); + } + double GetCurvature() { return _curvature; } @@ -207,6 +232,11 @@ namespace element { return p; } + std::pair DistanceTo(const geom::Location &) const override { + DEBUG_ERROR; + return {0.0, 0.0}; + } + private: double _curve_start; diff --git a/LibCarla/source/carla/road/element/RoadInfo.h b/LibCarla/source/carla/road/element/RoadInfo.h index d7c9d3901..60d041a34 100644 --- a/LibCarla/source/carla/road/element/RoadInfo.h +++ b/LibCarla/source/carla/road/element/RoadInfo.h @@ -90,10 +90,10 @@ namespace element { return (int) _lanes.size(); } - std::vector getLanesIDs(which_lane_e whichLanes = which_lane_e::Both) { + std::vector getLanesIDs(which_lane_e whichLanes = which_lane_e::Both) const { std::vector lanes_id; - for (lane_t::iterator it = _lanes.begin(); it != _lanes.end(); ++it) { + for (lane_t::const_iterator it = _lanes.begin(); it != _lanes.end(); ++it) { switch (whichLanes) { case which_lane_e::Both: { lanes_id.push_back(it->first); @@ -115,8 +115,8 @@ namespace element { return lanes_id; } - const LaneInfo *getLane(int id) { - lane_t::iterator it = _lanes.find(id); + const LaneInfo *getLane(int id) const { + lane_t::const_iterator it = _lanes.find(id); return it == _lanes.end() ? nullptr : &it->second; } }; diff --git a/LibCarla/source/carla/road/element/RoadSegment.h b/LibCarla/source/carla/road/element/RoadSegment.h index 88fb85f15..63f3d7d7c 100644 --- a/LibCarla/source/carla/road/element/RoadSegment.h +++ b/LibCarla/source/carla/road/element/RoadSegment.h @@ -10,8 +10,9 @@ #include "carla/road/element/RoadInfo.h" #include "carla/road/element/Types.h" -#include +#include #include +#include #include namespace carla { @@ -132,6 +133,43 @@ namespace element { return DirectedPoint::Invalid(); } + std::pair GetNearestPoint(const geom::Location &loc) const { + double min = std::numeric_limits::max(); + std::pair last = {0.0, 0.0}; + + decltype(_geom)::const_iterator nearest_geom; + + for (auto g = _geom.begin(); g !=_geom.end(); ++g) { + auto d = (*g)->DistanceTo(loc); + if (d.second < min) { + last = d; + min = d.second; + nearest_geom = g; + } + } + + for (auto g = _geom.begin(); g != nearest_geom; ++g) { + last.first += (*g)->GetLength(); + } + + return last; + } + + int GetNearestLane(double, const geom::Location &) const { + /* + DirectedPoint dp = GetDirectedPointIn(dist); + + double dist_dp_loc = geom::Math::Distance2D(dp.location, loc); + + const RoadInfoLane *road_info_lane = GetInfo(dist); + for (auto &&lane_id : road_info_lane->getLanesIDs()) { + const LaneInfo *info = road_info_lane->getLane(lane_id); + // search for info width + } + */ + return 0; + } + const double &GetLength() const { return _length; } diff --git a/LibCarla/source/carla/road/element/Waypoint.cpp b/LibCarla/source/carla/road/element/Waypoint.cpp index e0d228dd8..55b4f6aa4 100644 --- a/LibCarla/source/carla/road/element/Waypoint.cpp +++ b/LibCarla/source/carla/road/element/Waypoint.cpp @@ -12,6 +12,38 @@ namespace carla { namespace road { namespace element { + Waypoint::Waypoint() {} + + Waypoint::Waypoint(SharedPtr m, const geom::Location &loc) + : _map(m) { + + _road_id = 0; + _dist = 0; + + double nearest_dist = std::numeric_limits::max(); + for (auto &&r : _map->GetData()._elements) { + auto current_dist = r.second->GetNearestPoint(loc); + if (current_dist.second < nearest_dist) { + nearest_dist = current_dist.second; + _road_id = r.first; + _dist = current_dist.first; + } + } + + assert(_dist <= _map->GetData().GetRoad(_road_id)->GetLength()); + + // + const geom::Location loc_in_geom = + _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist).location; + const double heading = + _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist).tangent; + + const geom::Rotation rot(0.0, heading * geom::Math::to_radiants(), 0.0); + _transform = geom::Transform(loc_in_geom, rot); + + _lane_id = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc); + } + RoadInfoList Waypoint::GetRoadInfo() const { return RoadInfoList(_map->GetData().GetRoad(_road_id)->GetInfos(_dist)); } diff --git a/LibCarla/source/carla/road/element/Waypoint.h b/LibCarla/source/carla/road/element/Waypoint.h index e20cab942..d8d326081 100644 --- a/LibCarla/source/carla/road/element/Waypoint.h +++ b/LibCarla/source/carla/road/element/Waypoint.h @@ -31,6 +31,7 @@ namespace element { return _road_id; } + std::vector Next(double distance) const { (void) distance; return std::vector(); @@ -42,9 +43,11 @@ namespace element { friend carla::road::Map; - Waypoint() = default; + Waypoint(); - SharedPtr _map; + Waypoint(SharedPtr, const geom::Location &); + + SharedPtr _map; geom::Transform _transform; diff --git a/LibCarla/source/test/test_geom.cpp b/LibCarla/source/test/test_geom.cpp index 4d014708c..dc522ccc4 100644 --- a/LibCarla/source/test/test_geom.cpp +++ b/LibCarla/source/test/test_geom.cpp @@ -27,7 +27,7 @@ TEST(geom, distance) { } TEST(geom, nearest_point_segment) { - const float pos[] = { + const float segment[] = { 0, 0, 10, 0, 2, 5, 10, 8, -6, 8, 8, -2, @@ -61,14 +61,16 @@ TEST(geom, nearest_point_segment) { double min_dist = std::numeric_limits::max(); int id = -1; for (int j = 0; j < 40; j += 4) { - const double dist = Math::DistanceSegmentPoint(point[i], - {pos[j], pos[j + 1], 0}, {pos[j + 2], pos[j + 3], 0}); + const double dist = Math::DistSegmentPoint( + point[i], + {segment[j + 0], segment[j + 1], 0}, + {segment[j + 2], segment[j + 3], 0}).second; if (dist < min_dist) { min_dist = dist; id = j / 4; } } - ASSERT_EQ(id, results[i]) << "Fails point number " << i; + ASSERT_EQ(id, results[i]) << "Fails point number: " << i; } } @@ -112,12 +114,12 @@ TEST(geom, point_in_rectangle) { } TEST(geom, nearest_point_arc) { - ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(1,0,0), - Vector3D(0,0,0), 1.57, 0, 1), 0.414214, 0.01); - ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(2,1,0), - Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01); - ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(0,1,0), - Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01); - ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(1,2,0), - Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01); + ASSERT_NEAR(Math::DistArcPoint(Vector3D(1,0,0), + Vector3D(0,0,0), 1.57, 0, 1).second, 0.414214, 0.01); + ASSERT_NEAR(Math::DistArcPoint(Vector3D(2,1,0), + Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01); + ASSERT_NEAR(Math::DistArcPoint(Vector3D(0,1,0), + Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01); + ASSERT_NEAR(Math::DistArcPoint(Vector3D(1,2,0), + Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01); }