Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "TestSupport.hpp"
10 : : #include "ad/rss/structured/RssIntersectionConstellationChecker.hpp"
11 : :
12 : : namespace ad {
13 : : namespace rss {
14 : : namespace structured {
15 : :
16 : : class RssIntersectionPriorityConstellationCheckerTests : public testing::Test
17 : : {
18 : : protected:
19 : 3 : virtual void SetUp()
20 : : {
21 : 3 : constellation.constellation_type = world::ConstellationType::IntersectionEgoHasPriority;
22 : 3 : }
23 : :
24 : 3 : virtual void TearDown()
25 : : {
26 : 3 : }
27 : : core::RelativeObjectState leadingVehicle;
28 : : core::RelativeObjectState followingVehicle;
29 : : core::RelativeConstellation constellation;
30 : : state::RssState rssState;
31 : : world::TimeIndex time_index{1u};
32 : : };
33 : :
34 : 4 : TEST_F(RssIntersectionPriorityConstellationCheckerTests, 50kmh_safe_distance_ego_leading)
35 : : {
36 : 1 : RssIntersectionConstellationChecker checker;
37 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(120);
38 : 1 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(2.);
39 : 1 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(2.);
40 : 1 : leadingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
41 : 1 : leadingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
42 : 1 : leadingVehicle.structured_object_state.has_priority = true;
43 : :
44 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(30);
45 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
46 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
47 : 1 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(12.);
48 : 1 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(12.);
49 : :
50 : 1 : constellation.other_state = followingVehicle;
51 : 1 : constellation.ego_state = leadingVehicle;
52 : : constellation.relative_position
53 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::InFront, Distance(10.));
54 : :
55 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
56 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
57 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
58 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
59 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
60 [ + - ]: 1 : }
61 : :
62 : 4 : TEST_F(RssIntersectionPriorityConstellationCheckerTests, 50kmh_safe_distance_ego_following)
63 : : {
64 : 1 : RssIntersectionConstellationChecker checker;
65 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
66 : 1 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(10.);
67 : 1 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(10.);
68 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
69 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
70 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
71 : 1 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(70.);
72 : 1 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(70.);
73 : :
74 : 1 : constellation.other_state = leadingVehicle;
75 : 1 : constellation.ego_state = followingVehicle;
76 : 1 : constellation.ego_state.structured_object_state.has_priority = true;
77 : : constellation.relative_position
78 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(60.));
79 : :
80 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
81 [ - + - - : 1 : ASSERT_TRUE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
82 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
83 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
84 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
85 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
86 [ + - ]: 1 : }
87 : :
88 : 4 : TEST_F(RssIntersectionPriorityConstellationCheckerTests, 50km_h_stop_before_intersection)
89 : : {
90 : 1 : RssIntersectionConstellationChecker checker;
91 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
92 : 1 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(80.);
93 : 1 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(80.);
94 : 1 : leadingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
95 : 1 : leadingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
96 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
97 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
98 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
99 : 1 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(110.);
100 : 1 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(110.);
101 : :
102 : 1 : constellation.other_state = leadingVehicle;
103 : 1 : constellation.ego_state = followingVehicle;
104 : 1 : constellation.ego_state.structured_object_state.has_priority = true;
105 : : constellation.relative_position
106 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(30.));
107 : :
108 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
109 [ - + - - : 1 : ASSERT_TRUE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
110 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
111 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
112 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
113 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
114 : :
115 : 1 : constellation.other_state.structured_object_state.distance_to_enter_intersection = Distance(70.);
116 : 1 : constellation.other_state.structured_object_state.distance_to_leave_intersection = Distance(70.);
117 : 1 : constellation.ego_state.structured_object_state.distance_to_enter_intersection = Distance(100.);
118 : 1 : constellation.ego_state.structured_object_state.distance_to_leave_intersection = Distance(100.);
119 : : constellation.relative_position
120 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::Overlap, Distance(0.));
121 : :
122 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
123 [ - + - - : 1 : ASSERT_FALSE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
124 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
125 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
126 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
127 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
128 [ + - ]: 1 : }
129 : :
130 : : } // namespace structured
131 : : } // namespace rss
132 : : } // namespace ad
|
