Merge branch 'release-v2.3.1' into 'main'

osiql v2.3.1

See merge request !16

doc/source/Extensions/10_query.rst

-..
-  *******************************************************************************
-  Copyright (c) 2022-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Localization & coordinate conversion with osiql::Query
-======================================================
-
-The ``osiql::Query`` is the entry-point for any usage of osiql. It should be constructed only once for any ``osi3::GroundTruth`` and provides ways to convert global points to local coordinates and to calculate the intersections of geometry with the GroundTruth's lanes. It also includes more efficient getters (than by using OSI) for various object types given their id and has shortcuts to perform queries on the GroundTruth's host vehicle.
-
-Any global point touching a road can be converted to a set of local points inside lanes using ``Query::FindLocalPoints``. The returned value is a vector in case of overlapping lanes. 
-
-For more information on local points, see :doc:`/Types/10_points`.
-
-For more information on localizing to a specific lane or road, see :doc:`/Wrappers/20_lane` or :doc:`/Extensions/30_road`.
-
-For more information on the Query class, refer to the doxygen documentation.
\ No newline at end of file

doc/source/Extensions/20_stream.rst

-..
-  *******************************************************************************
-  Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Vicinity queries with osiql::Stream
-===================================
-
-Streams can be constructed on-demand from a starting local point and are used to perform vicinity queries. A ``osiql::Stream`` is a tree traversing a local origin point's connected lanes or roads up to a certain distance. A stream can also look back peripherally into lanes merging into it. Once created, a stream can be searched either road-wide or lane-wide, which excludes any lane changes and any findings are returned with a distance from the starting point of the stream.
-
-Streams will be heavily expanded upon and replaced with Nodes in osiql 1.2.0.

doc/source/Types/10_points.rst

-..
-  *******************************************************************************
-  Copyright (c) 2022-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Point Types
-===========
-
-There are four categories of points in osiql:
-
-* **Global point**: A set of cartesian XY(Z)-coordinates
-* **Local point**: A pair of local st-coordinates and an entity they relate to
-* **Pose**: A local point with an added relative angle
-* **Vertex**: A local and global point in one
-
-.. _points:
-.. figure:: ./points.svg
-  
-``osiql::ReferenceLineCoordinates`` follow OSI's definition of a reference-line-based st-coordinate system, whereas a ``osiql::CenterlineCoordinates`` are similar, but their s-coordinate is relative to the start of a lane in its driving direction and their t-coordinate is relative to the respective lane's center line.
-  
\ No newline at end of file

doc/source/Wrappers/10_world.rst

-..
-  *******************************************************************************
-  Copyright (c) 2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-World (osi3::GroundTruth)
-=========================
-
-osiql::World is a wrapper of a osi3::GroundTruth that stores all its wrapper objects in separate containers to allow fetching objects by their id more efficiently than OSI's linear lookup. These are the stored types:
-
-* Lane Boundaries (osi3::LogicalLaneBoundary)
-* Lane Markings (osi3::LaneBoundary)
-* Lanes (osi3::LogicalLane);
-* Reference Lines (osi3::ReferenceLine)
-* Roads (No corresponding OSI object)
-* Moving Objects (osi3::MovingObject)
-* Road Markings (osi3::RoadMarking)
-* Static Objects (osi3::StationaryObject)
-* Light Bulbs (osi3::TrafficLight)
-* Traffic Lights (Set of light bulbs)
-* Traffic Signs (osi3::TrafficSign)
-
-While its host vehicle is read from the GroundTruth by default, it can also be manually replaced using ``SetHostVehicle``.
-
-See the doxygen documentation for all methods that osiql::World provides.
\ No newline at end of file

doc/source/Wrappers/20_lane.rst

-..
-  *******************************************************************************
-  Copyright (c) 2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Lane (osi3::LogicalLane)
-========================
-
-A ``osiql::Lane`` is a wrapper of a ``osi3::LogicalLane``, which unlike a ``osi3::Lane`` stores its driving direction, successor lanes and predecessor lanes.
-
-Direction & Side of Road
-************************
-
-The local coordinate system of a lane is dependent on its assigned reference line. The driving direction following the reference line is called ``Direction::Downstream`` in osiql and corresponds to ``osi3::LogicalLane_MoveDirection_MOVE_DIRECTION_INCREASING_S``, while the reversed direction is called ``Direction::Upstream`` in osiql or ``osi3::LogicalLane_MoveDirection_MOVE_DIRECTION_DECREASING_S`` in OSI. More intuitively, osiql provides ``GetSideOfRoad`` functions that return ``Side::Right`` for lanes with driving direction ``Direction::Downstream`` and ``Side::Left`` for ``Direction::Upstream``.
-
-Orientation
-***********
-
-In OSI, a successor of a logical lane is always at the end of the lane which is further along its assigned reference line, meaning a vehicle driving forwards on a lane on the left side of the road (or reference line) is driving from one lane onto its predecessor lane, which is rather unintuitive. In osiql, the method ``Lane::GetConnectedLane<Orientation::Forward>`` returns the successor in that lane's driving direction instead of the reference line's definition direction. Retrieving the OSI successor is still possible:
-
-.. code-block:: cpp
-  :linenos:
-
-  const osiql::Lane& lane{ ... };
-  // Successors in driving direction:
-  const std::vector<osiql::Lane*>& successorLanes{lane.GetConnectedLanes<osiql::Orientation::Forward>()};
-  // Successors in road direction:
-  const std::vector<osiql::Lane*>& osiSuccessorLanes{lane.GetConnectedLanes(osiql::Direction::Downstream)};
-
-Likewise, ``Lane::GetAdjacentLanes<Side, Orientation>`` returns the adjacent lane on the given side relative to the lane's driving direction in the given orientation, meaning ``GetAdjacentLanes<Side::Left, Orientation::Forward>() == GetAdjacentLanes<Side::Right, Orientation::Backward>()``. 
\ No newline at end of file

doc/source/Wrappers/30_lines.rst

-..
-  *******************************************************************************
-  Copyright (c) 2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Reference lines & bounaries
-===========================
-
-The reference line defines the the coordinate system of all lanes that refer to it. While this is not required in OSI, osiql assumes that all adjacent lanes of a lane refer to the same reference line. Queries may fail if this is not the case.
\ No newline at end of file

doc/source/Wrappers/40_objects.rst

-..
-  *******************************************************************************
-  Copyright (c) 2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
-
-  This program and the accompanying materials are made available under the
-  terms of the Eclipse Public License 2.0 which is available at
-  http://www.eclipse.org/legal/epl-2.0.
-
-  SPDX-License-Identifier: EPL-2.0
-  *******************************************************************************
-
-Objects
-=======
-
-Moving objects
-**************
-
-osiql ignores the assigned positions of moving objects. Instead, when constructing a ``osiql::Query``, copies of all moving objects are created and their local positions are calculated. This is because there is a chance that a reference to a ``osi3::MovingObject`` may be invalidated (despawned or reallocated) between timesteps. The original assigned positions can still be accessed directly through the object's handle.
-
-Stationary objects
-******************
-
-Static objects, which includes ``osi3::RoadMarking``, ``osiql::StaticObject``, ``osiql::TrafficLight`` & ``osi3::TrafficSign``, inherit their assigned positions from the OSI GroundTruth.
-
-A ``osi3::TrafficLight`` is an individual light bulb, whereas an ``osiql::TrafficLight`` is a collection of light bulbs that have identical lane assignments to each other.
\ No newline at end of file

include/OsiQueryLibrary/Routing/Node.tpp

@@ -235,8 +235,7 @@ decltype(auto) Node<D, Scope>::GetPlacements(Predicate &&pred) const
     else
     {
         std::vector<Placement<Type>> result;
-        decltype(auto) placements{GetPlacements<Type>()};
-        for (const auto &entry : placements)
+        for (decltype(auto) entry : GetPlacements<Type>())
         {
             if (pred(get<Placement<Type>>(entry)))
             {
@@ -251,7 +250,7 @@ template <LaneDirection D, typename Scope>
 template <typename Type, typename Predicate>
 std::optional<Placement<Type>> Node<D, Scope>::GetPlacement(Predicate &&pred) const
 {
-    for (const auto &entry : GetPlacements<Type>())
+    for (decltype(auto) entry : GetPlacements<Type>())
     {
         if (pred(get<Placement<Type>>(entry)))
         {
@@ -341,22 +340,14 @@ template <LaneDirection D, typename Scope>
 template <typename Type, typename Predicate, typename Transform, typename OutputIt>
 OutputIt Node<D, Scope>::InsertAllWithin(OutputIt output, Predicate &&pred, Transform &&transform) const
 {
-    decltype(auto) placements{GetPlacements<Type>()};
-    if constexpr (std::is_same_v<Predicate, Return<true>>)
-    {
-        return std::transform(placements.begin(), placements.end(), output, ToLocation{*this});
-    }
-    else
+    for (decltype(auto) entry : GetPlacements<Type>())
     {
-        for (const auto &placement : placements)
+        if (Location<Type> candidate{ToLocation{*this}(get<Placement<Type>>(entry))}; pred(candidate))
         {
-            if (Location<Type> candidate{ToLocation{*this}(placement)}; pred(candidate))
-            {
-                *(output++) = transform(std::move(candidate));
-            }
+            *(output++) = transform(std::move(candidate));
         }
-        return output;
     }
+    return output;
 }
 
 template <LaneDirection D, typename Scope>

include/OsiQueryLibrary/Street/Lane.h

 /********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
@@ -48,6 +48,8 @@ struct Lane : public detail::BoundaryEnclosure<Lane>, public Overlapable<Lane>
 
     using detail::BoundaryEnclosure<Lane>::BoundaryEnclosure;
 
+    using Direction = LaneDirection;
+
     //! \brief The type of a lane
     enum class Type : std::underlying_type_t<osi3::LogicalLane_Type>
     {

include/OsiQueryLibrary/Street/Road.h

 /********************************************************************************
- * Copyright (c) 2022-2024 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2022-2025 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
@@ -27,6 +27,8 @@ struct Road : RoadBase, Overlapable<Road>
 {
     static constexpr const std::string_view name = "Road";
 
+    using Direction = RoadDirection;
+
     Road(const ReferenceLine &);
 
     //! Returns the id of this road's rightmost lane

tests/unitTests/NodeTests.cpp

@@ -573,3 +573,31 @@ TEST_F(RoadNetwork, FindAll_GivenObjectsOnEveryRoad_WhenSearchingForwardAndBackw
     std::sort(objects.begin(), objects.end(), Less<Distance>{});
     EXPECT_THAT(objects, SizeIs(19));
 }
+
+TEST(Node, FindAllWithin)
+{
+    test::GroundTruth groundTruth;
+    auto &logicalLane{groundTruth.AddLane(
+        groundTruth.AddReferenceLine(XY{0, 0}, XY{10, 0}),
+        groundTruth.AddBoundary(XY{0, 0}, XY{10, 0}),
+        groundTruth.AddBoundary(XY{0, -10}, XY{10, -10})
+    )};
+
+    World world{groundTruth.handle};
+    const Lane *lane{world.GetLane(get<Id>(logicalLane))};
+    auto roadNode{Node<>::Create(lane)};
+    auto laneNode{Node<LaneDirection::Forward, Lane>::Create(lane)};
+
+    EXPECT_THAT(roadNode->FindAllWithin<MovingObject>(), IsEmpty());
+    EXPECT_THAT(laneNode->FindAllWithin<MovingObject>(), IsEmpty());
+
+    groundTruth.Add<MovingObject>(XY{5, -5});
+    world.UpdateAll<MovingObject>(groundTruth.handle.moving_object());
+    EXPECT_THAT(roadNode->FindAllWithin<MovingObject>(), SizeIs(1));
+    EXPECT_THAT(laneNode->FindAllWithin<MovingObject>(), SizeIs(1));
+
+    groundTruth.Add<MovingObject>(XY{0, -5});
+    world.UpdateAll<MovingObject>(groundTruth.handle.moving_object());
+    EXPECT_THAT(roadNode->FindAllWithin<MovingObject>(), SizeIs(2));
+    EXPECT_THAT(laneNode->FindAllWithin<MovingObject>(), SizeIs(2));
+}