diff --git a/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h b/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h index d79fdeef483cf12726e5c16a42265663dbaf384e..15795f473f8d931fc261c64cb16d22b0d6f086dc 100644 --- a/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h +++ b/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2021-2022, 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 @@ -17,12 +17,21 @@ #include <MantleAPI/Common/i_identifiable.h> #include <MantleAPI/Common/orientation.h> +#include <MantleAPI/Common/pose.h> #include <MantleAPI/Common/vector.h> #include <units.h> + +#include <optional> #include <vector> namespace mantle_api { +enum class Direction +{ + kForward = 0, + kBackwards = 1 +}; + /// Abstraction layer for all map related functions. class ILaneLocationQueryService { @@ -43,8 +52,15 @@ public: bool allow_invalid_positions = false) const = 0; virtual bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const = 0; virtual std::vector<UniqueId> GetLaneIdsAtPosition(const Vec3<units::length::meter_t>& position) const = 0; + + /// @brief Calculate the new pose which is at certain distance from the reference_pose_on_lane following a direction. + /// + ///@param reference_pose_on_lane Starting position. Must be on a lane. + ///@param direction Direction to go along the lane from the reference_pose_on_lane. The orientation of the reference pose is used to determine the forward direction. "Forward" means aligned with the positive x direction of the local coordinate system defined by the pose. + ///@return Pose that is at the given distance from reference_pose_on_lane in the specified direction. The new orientation is parallel to the lane orientation at the target position. + virtual std::optional<Pose> FindLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, units::length::meter_t distance, Direction direction) const = 0; }; } // namespace mantle_api -#endif // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H +#endif // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H \ No newline at end of file diff --git a/MantleAPI/test/MantleAPI/Test/test_utils.h b/MantleAPI/test/MantleAPI/Test/test_utils.h index ebdd4df5141fa92771f2551701a21565e75885ef..1623acf9bfab3d2144b30037fc225669eb99774b 100644 --- a/MantleAPI/test/MantleAPI/Test/test_utils.h +++ b/MantleAPI/test/MantleAPI/Test/test_utils.h @@ -1,5 +1,5 @@ /******************************************************************************* - * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * Copyright (c) 2021-2022, 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 @@ -89,7 +89,7 @@ public: private: std::string name_{}; - std::unique_ptr<mantle_api::EntityProperties> properties_{nullptr}; + std::unique_ptr<mantle_api::EntityProperties> properties_{std::make_unique<mantle_api::VehicleProperties>()}; }; class MockQueryService : public mantle_api::ILaneLocationQueryService @@ -98,8 +98,8 @@ public: MOCK_METHOD(Orientation3<units::angle::radian_t>, GetLaneOrientation, (const Vec3<units::length::meter_t>& position), (const override)); Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position, - double upwards_shift, - bool allowed_to_leave_lane = false) const override + double upwards_shift, + bool allowed_to_leave_lane = false) const override { std::ignore = position; std::ignore = upwards_shift; @@ -118,8 +118,17 @@ public: return {}; } - private: - MockVehicle test_vehicle_{}; + std::optional<Pose> FindLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, double distance, Direction direction) const override + { + std::ignore = reference_pose_on_lane; + std::ignore = distance; + std::ignore = direction; + Pose pose{}; + return pose; + } + +private: + MockVehicle test_vehicle_{}; }; class MockPedestrian : public mantle_api::IPedestrian @@ -405,4 +414,4 @@ private: MockConverter converter_{}; }; -} // namespace mantle_api +} // namespace mantle_api \ No newline at end of file