From 9eacba9e04dfc6fd1a63c5f21c747408d690780c Mon Sep 17 00:00:00 2001
From: Arun Das <arun.das@bmw.de>
Date: Fri, 18 Jun 2021 12:13:42 +0200
Subject: [PATCH] Update and check control strategies for entity and not for
controller; * and / operator for vectors
Signed-off-by: Arun Das <arun.das@bmw.de>
---
MantleAPI/include/MantleAPI/Common/spline.h | 10 ++++++++++
MantleAPI/include/MantleAPI/Common/vector.h | 10 ++++++++++
.../include/MantleAPI/Execution/i_environment.h | 14 +++++++-------
.../MantleAPI/Map/i_lane_location_query_service.h | 4 +++-
MantleAPI/test/MantleAPI/Test/test_utils.h | 11 +++++++----
5 files changed, 37 insertions(+), 12 deletions(-)
diff --git a/MantleAPI/include/MantleAPI/Common/spline.h b/MantleAPI/include/MantleAPI/Common/spline.h
index d12d4ddc..e498e222 100644
--- a/MantleAPI/include/MantleAPI/Common/spline.h
+++ b/MantleAPI/include/MantleAPI/Common/spline.h
@@ -1,3 +1,4 @@
+
/*******************************************************************************
* Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
*
@@ -17,11 +18,20 @@
#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/time_utils.h>
+#include <MantleAPI/Common/vector.h>
#include <array>
namespace mantle_api
{
+struct SplineSegment
+{
+ mantle_api::Vec3d a;
+ mantle_api::Vec3d b;
+ mantle_api::Vec3d c;
+ mantle_api::Vec3d d;
+};
+
struct SplineSection
{
mantle_api::Time start_time{0};
diff --git a/MantleAPI/include/MantleAPI/Common/vector.h b/MantleAPI/include/MantleAPI/Common/vector.h
index 5127e5e0..86cff86b 100644
--- a/MantleAPI/include/MantleAPI/Common/vector.h
+++ b/MantleAPI/include/MantleAPI/Common/vector.h
@@ -62,6 +62,16 @@ inline Vec3d operator*(const Vec3d& lhs, double d) noexcept
return {lhs.x * d, lhs.y * d, lhs.z * d};
}
+inline Vec3d operator*(double d, const Vec3d& rhs) noexcept
+{
+ return rhs * d;
+}
+
+inline Vec3d operator/(const Vec3d& lhs, double d) noexcept
+{
+ return {lhs.x / d, lhs.y / d, lhs.z / d};
+}
+
} // namespace mantle_api
#endif // MANTLEAPI_COMMON_VECTOR_H
diff --git a/MantleAPI/include/MantleAPI/Execution/i_environment.h b/MantleAPI/include/MantleAPI/Execution/i_environment.h
index 5fbbcbf9..c5e0876f 100644
--- a/MantleAPI/include/MantleAPI/Execution/i_environment.h
+++ b/MantleAPI/include/MantleAPI/Execution/i_environment.h
@@ -54,19 +54,19 @@ class IEnvironment
virtual void RemoveControllerFromEntity(std::uint64_t entity_id) = 0;
- /// Updates the control strategies in the composite controller.
+ /// Updates the control strategies for an entity.
///
- /// @param controller_id Specifies the controller to be updated
+ /// @param entity_id Specifies the entity to be updated
/// @param control_strategies Specifies the desired movement behaviour for the entity
virtual void UpdateControlStrategies(
- std::uint64_t controller_id,
+ std::uint64_t entity_id,
std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
- /// Checks, if a control strategy of a certain type in a specific controller is fulfilled
+ /// Checks, if a control strategy of a certain type for a specific entity has been fulfilled
///
- /// @param controller_id The controller to check
- /// @param type The control strategy type
- virtual bool HasControlStrategyGoalBeenReached(std::uint64_t controller_id,
+ /// @param entity_id The entity to check
+ /// @param type The control strategy type
+ virtual bool HasControlStrategyGoalBeenReached(std::uint64_t entity_id,
mantle_api::ControlStrategyType type) const = 0;
virtual const ILaneLocationQueryService& GetQueryService() const = 0;
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 0fd41bde..5cf1884b 100644
--- a/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h
+++ b/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h
@@ -36,7 +36,9 @@ class ILaneLocationQueryService
// virtual const IIdentifiable& GetMapObjectById(UniqueId id) = 0;
virtual Orientation3d GetLaneOrientation(const Vec3d& position) const = 0;
- virtual Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position, double upwards_shift) const = 0;
+ virtual Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
+ double upwards_shift,
+ bool allow_invalid_positions = false) const = 0;
virtual bool IsPositionOnLane(const Vec3d& position) const = 0;
};
} // namespace mantle_api
diff --git a/MantleAPI/test/MantleAPI/Test/test_utils.h b/MantleAPI/test/MantleAPI/Test/test_utils.h
index dc03ff8e..a0692893 100644
--- a/MantleAPI/test/MantleAPI/Test/test_utils.h
+++ b/MantleAPI/test/MantleAPI/Test/test_utils.h
@@ -46,7 +46,7 @@ class MockConverter : public mantle_api::ICoordConverter
class MockVehicle : public mantle_api::IVehicle
{
public:
- mantle_api::UniqueId GetUniqueId() const override { return 0; }
+ MOCK_METHOD(mantle_api::UniqueId, GetUniqueId, (), (const, override));
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
@@ -98,10 +98,13 @@ class MockQueryService : public mantle_api::ILaneLocationQueryService
return {};
}
- Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position, double upwards_shift) const override
+ Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
+ double upwards_shift,
+ bool allowed_to_leave_lane = false) const override
{
std::ignore = position;
std::ignore = upwards_shift;
+ std::ignore = allowed_to_leave_lane;
return mantle_api::Vec3d();
}
bool IsPositionOnLane(const Vec3d& position) const override
@@ -304,13 +307,13 @@ class MockEnvironment : public mantle_api::IEnvironment
MOCK_METHOD(void,
UpdateControlStrategies,
- (std::uint64_t controller_id,
+ (std::uint64_t entity_id,
std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies),
(override));
MOCK_METHOD(bool,
HasControlStrategyGoalBeenReached,
- (std::uint64_t controller_id, mantle_api::ControlStrategyType type),
+ (std::uint64_t entity_id, mantle_api::ControlStrategyType type),
(const, override));
const mantle_api::ILaneLocationQueryService& GetQueryService() const override { return query_service_; }
--
GitLab