Merge branch '85-support-for-roadranges' into 'main'

Add support for intepretation of RoadRanges and TrafficAreas

Closes #85

See merge request !173

include/MantleAPI/Execution/i_environment.h

@@ -27,6 +27,7 @@
 #include <MantleAPI/Traffic/default_routing_behavior.h>
 #include <MantleAPI/Traffic/i_controller_repository.h>
 #include <MantleAPI/Traffic/i_entity_repository.h>
+#include <MantleAPI/Traffic/i_traffic_area_service.h>
 #include <MantleAPI/Traffic/i_traffic_swarm_service.h>
 
 #include <cstdint>
@@ -198,6 +199,11 @@ public:
   ///
   /// @return reference to the traffic swarm service interface
   [[nodiscard]] virtual ITrafficSwarmService& GetTrafficSwarmService() = 0;
+
+  /// @brief Gets the traffic area service
+  ///
+  /// @return reference to the traffic area service interface
+  [[nodiscard]] virtual ITrafficAreaService& GetTrafficAreaService() = 0;
 };
 }  // namespace mantle_api
 

include/MantleAPI/Traffic/i_traffic_area_service.h

+/*******************************************************************************
+ * Copyright (c) 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 https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/// @file i_traffic_area_service.h
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_SERVICE_H
+#define MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_SERVICE_H
+
+#include <MantleAPI/Traffic/i_traffic_area_stream.h>
+#include <MantleAPI/Traffic/road_range.h>
+
+#include <memory>
+#include <vector>
+
+namespace mantle_api
+{
+
+/// Collection of traffic area streams belonging to a RoadRange.
+using TrafficArea = std::vector<std::shared_ptr<ITrafficAreaStream>>;
+
+/// Abstraction layer for managing traffic area related queries
+class ITrafficAreaService
+{
+public:
+  virtual ~ITrafficAreaService() = default;
+
+  /// Creates a traffic area object
+  /// @note Ownership of the returned object is shared with the caller
+  /// @param road_range Road range defining the traffic area
+  /// @returns Collection of traffic area streams
+  virtual TrafficArea CreateTrafficArea(const RoadRange& road_range) const = 0;
+};
+
+}  // namespace mantle_api
+
+#endif  // MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_SERVICE_H

include/MantleAPI/Traffic/i_traffic_area_stream.h

+/*******************************************************************************
+ * Copyright (c) 2024-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 https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/// @file i_traffic_area_stream.h
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_STREAM_H
+#define MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_STREAM_H
+
+#include <MantleAPI/Common/pose.h>
+#include <MantleAPI/Traffic/i_entity.h>
+#include <units.h>
+
+#include <memory>
+#include <optional>
+#include <vector>
+
+namespace mantle_api
+{
+
+/// Map agnostic representation of connected lanes
+class ITrafficAreaStream
+{
+public:
+  /// Represents a position on a stream
+  struct StreamPosition
+  {
+    units::length::meter_t s;  ///< Longitudinal position within the stream (relative to the start)
+    units::length::meter_t t;  ///< Lateral position within the stream (relative to the middle of the stream)
+  };
+
+  /// Represents a pose on a stream
+  struct StreamPose : StreamPosition
+  {
+    units::angle::radian_t hdg;  ///< Heading relative to stream direction
+  };
+
+  /// Search direction for finding entities on the stream
+  enum class SearchDirection
+  {
+    kUndefined = 0,
+    kDownstream,
+    kUpstream,
+  };
+
+  /// Gets the length of the stream
+  /// @returns Length of the stream
+  virtual units::length::meter_t GetLength() const = 0;
+
+  /// Checks if a stream position lies on a lane of this stream
+  /// @param stream_position stream position to check
+  /// @returns True if the position is within the stream, false otherwise
+  virtual bool Contains(const StreamPosition& stream_position) const = 0;
+
+  /// Gets the first entity on the stream that overlaps with the given search interval
+  /// @param search_direction Direction to search in
+  /// @param start_distance   Distance from which to start the search
+  /// @param search_distance  Distance to search for in the search_direction
+  /// @returns Entity or empty if no entity was found
+  virtual std::weak_ptr<IEntity> GetEntity(
+      SearchDirection search_direction,
+      units::length::meter_t start_distance,
+      units::length::meter_t search_distance) const = 0;
+
+  /// Converts a stream pose to a global pose
+  /// @param stream_pose Longitudinal, lateral & angular offset along this stream's chain of lane centerlines
+  /// @returns Pose if convertible, empty otherwise
+  virtual std::optional<Pose> Convert(const StreamPose& stream_pose) const = 0;
+
+  /// Converts a global pose to a pose relative to this stream's origin
+  /// @param pose Global position and orientation
+  /// @returns StreamPose if the given point lies on a road part of this stream, otherwise nullopt
+  virtual std::optional<StreamPose> Convert(const Pose& pose) const = 0;
+};
+
+/// @brief almost-equality
+/// @details Compares the values of two stream positions.
+/// @param[in] lhs The left-hand side value for the comparison
+/// @param[in] rhs The right-hand side value for the comparison
+/// @returns true if the values of lhs are almost equal to the values of rhs.
+constexpr bool operator==(const ITrafficAreaStream::StreamPosition& lhs, const ITrafficAreaStream::StreamPosition& rhs)
+{
+  return AlmostEqual(lhs.s, rhs.s) && AlmostEqual(lhs.t, rhs.t);
+}
+
+/// @brief inequality
+/// @details Compares the value of stream positions.
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true if any value of lhs is not almost equal to the value of rhs
+constexpr bool operator!=(const ITrafficAreaStream::StreamPosition& lhs, const ITrafficAreaStream::StreamPosition& rhs)
+{
+  return !(lhs == rhs);
+}
+
+/// @brief almost-equality
+/// @details Compares the values of two stream positions.
+/// @param[in] lhs The left-hand side value for the comparison
+/// @param[in] rhs The right-hand side value for the comparison
+/// @returns true if the values of lhs are almost equal to the values of rhs.
+constexpr bool operator==(const ITrafficAreaStream::StreamPose& lhs, const ITrafficAreaStream::StreamPose& rhs)
+{
+  return static_cast<const ITrafficAreaStream::StreamPosition&>(lhs) == static_cast<const ITrafficAreaStream::StreamPosition&>(rhs) && AlmostEqual(lhs.hdg, rhs.hdg);
+}
+
+/// @brief inequality
+/// @details Compares the value of stream poses.
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true if any value of lhs is not almost equal to the value of rhs
+constexpr bool operator!=(const ITrafficAreaStream::StreamPose& lhs, const ITrafficAreaStream::StreamPose& rhs)
+{
+  return !(lhs == rhs);
+}
+}  // namespace mantle_api
+
+#endif  // MANTLEAPI_TRAFFIC_I_TRAFFIC_AREA_STREAM_H

include/MantleAPI/Traffic/road_range.h

+/*******************************************************************************
+ * Copyright (c) 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 https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/// @file road_range.h
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_TRAFFIC_ROAD_RANGE_H
+#define MANTLEAPI_TRAFFIC_ROAD_RANGE_H
+
+#include <MantleAPI/Map/lane_definition.h>
+#include <units.h>
+
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace mantle_api
+{
+
+/// Defines a cross section on a road at a given s-coordinate
+/// @note aligns with the definition within OpenSCENARIO XML v1.3.0
+struct RoadCursor
+{
+  /// Defines a restriction of the road cursor to specific lanes of a road
+  struct LaneRestriction
+  {
+    LaneId id;  ///< The local ID of the target lane at the specified s coordinate taken from the respective road network definition file.
+  };
+
+  std::optional<units::length::meter_t> s_offset;              ///< The s-coordinate taken along the road's reference line from the start point of the target road. If s is omitted and the road cursor depicts the start of a road range, then s=0 is assumed. If s is omitted and the road cursor depicts the end of a road range, then s=max_length is assumed. Unit: [m]. Range: [0..inf[.
+  std::string road_id;                                         ///< The ID of the target road taken from the respective road network definition file.
+  std::vector<RoadCursor::LaneRestriction> lane_restrictions;  ///< Restriction of the road cursor to specific lanes of a road. If omitted, road cursor is valid for all lanes of the road.
+};
+
+/// Defines an area by a range on a specific road.
+/// @note aligns with the definition within OpenSCENARIO XML v1.3.0
+struct RoadRange
+{
+  std::optional<units::length::meter_t> length;  ///< Limits the length of the road range starting from the first road cursor. If omitted or if length exceeds last road cursor, then last road cursor defines the end of the road range. Unit: [m]. Range: ]0;inf[.
+  std::vector<RoadCursor> road_cursors;          ///< A minimum of 2 road cursors must be provided to specify the start and end of the road range. Intermediate cursors can be used to change the lane "validity".
+};
+
+}  // namespace mantle_api
+
+#endif  // MANTLEAPI_TRAFFIC_ROAD_RANGE_H

test/MantleAPI/Test/test_utils.h

 /*******************************************************************************
- * Copyright (c) 2021-2024, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2021-2025, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  * Copyright (c) 2022 Ansys, Inc.
  * Copyright (c) 2024, Mercedes-Benz Tech Innovation GmbH
  *
@@ -29,6 +29,8 @@
 #include <MantleAPI/Traffic/i_controller_repository.h>
 #include <MantleAPI/Traffic/i_entity.h>
 #include <MantleAPI/Traffic/i_entity_repository.h>
+#include <MantleAPI/Traffic/i_traffic_area_service.h>
+#include <MantleAPI/Traffic/i_traffic_area_stream.h>
 #include <MantleAPI/Traffic/i_traffic_swarm_service.h>
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
@@ -503,6 +505,44 @@ public:
               (override));
 };
 
+class MockTrafficAreaService : public mantle_api::ITrafficAreaService
+{
+public:
+  MOCK_METHOD(std::vector<std::shared_ptr<mantle_api::ITrafficAreaStream>>,
+              CreateTrafficArea,
+              (const mantle_api::RoadRange&),
+              (const, override));
+};
+
+class MockTrafficAreaStream : public mantle_api::ITrafficAreaStream
+{
+public:
+  MOCK_METHOD(units::length::meter_t,
+              GetLength,
+              (),
+              (const, override));
+
+  MOCK_METHOD(bool,
+              Contains,
+              (const mantle_api::ITrafficAreaStream::StreamPosition&),
+              (const, override));
+
+  MOCK_METHOD(std::weak_ptr<mantle_api::IEntity>,
+              GetEntity,
+              (mantle_api::ITrafficAreaStream::SearchDirection, units::length::meter_t, units::length::meter_t),
+              (const, override));
+
+  MOCK_METHOD(std::optional<mantle_api::Pose>,
+              Convert,
+              (const mantle_api::ITrafficAreaStream::StreamPose&),
+              (const, override));
+
+  MOCK_METHOD(std::optional<mantle_api::ITrafficAreaStream::StreamPose>,
+              Convert,
+              (const mantle_api::Pose&),
+              (const, override));
+};
+
 class MockEnvironment : public mantle_api::IEnvironment
 {
 public:
@@ -610,6 +650,8 @@ public:
 
   MockTrafficSwarmService& GetTrafficSwarmService() override { return traffic_swarm_service_; }
 
+  MockTrafficAreaService& GetTrafficAreaService() override { return traffic_area_service_; }
+
 private:
   MockQueryService query_service_{};
   MockEntityRepository entity_repository_{};
@@ -617,6 +659,7 @@ private:
   MockConverter converter_{};
   MockGeometryHelper geometry_helper_{};
   MockTrafficSwarmService traffic_swarm_service_{};
+  MockTrafficAreaService traffic_area_service_{};
 };
 
 }  // namespace mantle_api