Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #pragma once
10 : :
11 : : #include <ad/map/lane/LaneOperation.hpp>
12 : : #include <ad/map/match/MapMatchedOperation.hpp>
13 : : #include <ad/map/route/Planning.hpp>
14 : : #include <ad/rss/map/RssObjectAdapter.hpp>
15 : : #include <ad/rss/map/RssRouteCheckerControl.hpp>
16 : : #include <tuple>
17 : :
18 : : namespace ad {
19 : : namespace rss {
20 : : namespace map {
21 : : namespace test_support {
22 : :
23 : : class CheckerControlTest : public ::ad::rss::map::RssRouteCheckerControl<void *>
24 : : {
25 : : public:
26 : : /*!
27 : : * \brief Smart pointer on RssObjectAdapter
28 : : */
29 : : typedef std::shared_ptr<CheckerControlTest> Ptr;
30 : :
31 : : /*!
32 : : * \brief Smart pointer on constant RssObjectAdapter
33 : : */
34 : : typedef std::shared_ptr<CheckerControlTest const> ConstPtr;
35 : :
36 : : static const ::ad::physics::Duration cResponseTimeEgoVehicle;
37 : : static const ::ad::physics::Duration cResponseTimeOtherVehicles;
38 : :
39 : : static const ::ad::physics::Acceleration cMaximumLongitudinalAcceleration;
40 : : static const ::ad::physics::Acceleration cMinimumLongitudinalBrakingDeceleleration;
41 : : static const ::ad::physics::Acceleration cMaximumLongitudinalBrakingDeceleleration;
42 : : static const ::ad::physics::Acceleration cMinimumLongitudinalBrakingDecelelerationCorrect;
43 : :
44 : : static const ::ad::physics::Acceleration cMaximumLateralAcceleration;
45 : : static const ::ad::physics::Acceleration cMinimumLateralBrakingDeceleleration;
46 : :
47 : : CheckerControlTest();
48 : :
49 : : ::ad::rss::world::UnstructuredSettings getUnstructuredSettings();
50 : : ::ad::rss::world::RssDynamics getObjectVehicleDynamics(::ad::physics::Speed const max_speed_on_acceleration
51 : : = ::ad::physics::Speed(100.));
52 : : ::ad::rss::world::RssDynamics getEgoVehicleDynamics(::ad::physics::Speed const max_speed_on_acceleration
53 : : = ::ad::physics::Speed(100.));
54 : :
55 : 1 : ::ad::rss::world::RssDynamics defaultPedestrianRssDynamicsCallback(void *pedestrian)
56 : : {
57 : : (void)pedestrian;
58 [ + - ]: 1 : return getObjectVehicleDynamics();
59 : : }
60 : :
61 : 223 : ::ad::rss::world::RssDynamics defaultOtherVehicleRssDynamicsCallback(void *otherVehicle)
62 : : {
63 : : (void)otherVehicle;
64 [ + - ]: 223 : return getObjectVehicleDynamics();
65 : : }
66 : :
67 : 231 : ::ad::rss::world::RssDynamics defaultEgoVehicleRssDynamicsCallback(void *egoVehicle)
68 : : {
69 : : (void)egoVehicle;
70 [ + - ]: 231 : return getEgoVehicleDynamics();
71 : : }
72 : :
73 : : ::ad::rss::map::RssConstellationConfiguration
74 : 44 : vehicleConstellationCallback(::ad::rss::map::RssRoute const &route,
75 : : ::ad::rss::map::RssObjectData const &egoVehicleObject,
76 : : void *egoVehicle,
77 : : ::ad::rss::map::RssObjectData const &otherVehicleObject,
78 : : void *otherVehicle)
79 : : {
80 : : (void)route;
81 : : (void)egoVehicle;
82 : : (void)otherVehicle;
83 : 44 : ::ad::rss::map::RssConstellationConfiguration result;
84 : 44 : result.ego_vehicle_dynamics = egoVehicleObject.rss_dynamics;
85 : 44 : result.object_dynamics = otherVehicleObject.rss_dynamics;
86 : 44 : result.restrict_speed_limit_mode = mRestrictSpeedLimitMode;
87 : 44 : result.constellation_creation_mode = mVehicleWorldModelCreationMode;
88 : 44 : return result;
89 : : }
90 : :
91 : : ::ad::rss::map::RssConstellationConfiguration
92 : 0 : pedestrianConstellationCallback(::ad::rss::map::RssRoute const &route,
93 : : ::ad::rss::map::RssObjectData const &egoVehicleObject,
94 : : void *egoVehicle,
95 : : ::ad::rss::map::RssObjectData const &pedestrianObject,
96 : : void *pedestrian)
97 : : {
98 : : (void)route;
99 : : (void)egoVehicle;
100 : : (void)pedestrian;
101 : 0 : ::ad::rss::map::RssConstellationConfiguration result;
102 : 0 : result.ego_vehicle_dynamics = egoVehicleObject.rss_dynamics;
103 : 0 : result.object_dynamics = pedestrianObject.rss_dynamics;
104 : 0 : result.restrict_speed_limit_mode = mRestrictSpeedLimitMode;
105 : 0 : result.constellation_creation_mode = mPedestrianWorldModelCreationMode;
106 : 0 : return result;
107 : : }
108 : :
109 : : ::ad::rss::map::RssConstellationConfiguration
110 : 4 : artificialObjectConstellationCallback(::ad::rss::map::RssRoute const &route,
111 : : ::ad::rss::map::RssObjectData const &egoVehicleObject,
112 : : void *egoVehicle,
113 : : ::ad::rss::map::RssObjectData const &artificialObject)
114 : : {
115 : : (void)route;
116 : : (void)egoVehicle;
117 : 4 : ::ad::rss::map::RssConstellationConfiguration result;
118 : 4 : result.ego_vehicle_dynamics = egoVehicleObject.rss_dynamics;
119 : 4 : result.object_dynamics = artificialObject.rss_dynamics;
120 : 4 : result.restrict_speed_limit_mode = mRestrictSpeedLimitMode;
121 [ + + ]: 4 : if (artificialObject.type == ::ad::rss::world::ObjectType::ArtificialVehicle)
122 : : {
123 : 2 : result.constellation_creation_mode = mArtificialVehicleWorldModelCreationMode;
124 : : }
125 [ + - ]: 2 : else if (artificialObject.type == ::ad::rss::world::ObjectType::ArtificialPedestrian)
126 : : {
127 : 2 : result.constellation_creation_mode = mArtificialPedestrianWorldModelCreationMode;
128 : : }
129 : : else
130 : : {
131 : 0 : result.constellation_creation_mode = mArtificialObjectWorldModelCreationMode;
132 : : }
133 : 4 : return result;
134 : : }
135 : :
136 : : ::ad::rss::map::RssRestrictSpeedLimitMode mRestrictSpeedLimitMode;
137 : : ::ad::rss::map::RssConstellationCreationMode mPedestrianWorldModelCreationMode;
138 : : ::ad::rss::map::RssConstellationCreationMode mVehicleWorldModelCreationMode;
139 : : ::ad::rss::map::RssConstellationCreationMode mArtificialObjectWorldModelCreationMode;
140 : : ::ad::rss::map::RssConstellationCreationMode mArtificialPedestrianWorldModelCreationMode;
141 : : ::ad::rss::map::RssConstellationCreationMode mArtificialVehicleWorldModelCreationMode;
142 : : };
143 : :
144 : : class RssObjectInstanceTest : public RssObjectInstance<void *>
145 : : {
146 : : public:
147 : : /*!
148 : : * \brief Smart pointer on RssObjectAdapter
149 : : */
150 : : typedef std::shared_ptr<RssObjectInstanceTest> Ptr;
151 : :
152 : : /*!
153 : : * \brief Smart pointer on constant RssObjectAdapter
154 : : */
155 : : typedef std::shared_ptr<RssObjectInstanceTest const> ConstPtr;
156 : :
157 : 439 : RssObjectInstanceTest(::ad::rss::world::ObjectId const &objectId,
158 : : ::ad::rss::world::ObjectType const &objectType,
159 : : CheckerControlTest::Ptr checkerControl)
160 : 439 : : RssObjectInstance<void *>(nullptr, checkerControl)
161 : 439 : , mObjectId(objectId)
162 : 439 : , mObjectType(objectType)
163 : : {
164 : 439 : }
165 : :
166 : 439 : virtual ~RssObjectInstanceTest() = default;
167 : :
168 : 439 : ::ad::rss::world::ObjectType getObjectType() const override
169 : : {
170 : 439 : return mObjectType;
171 : : }
172 : :
173 : : ::ad::rss::world::ObjectId mObjectId;
174 : : ::ad::rss::world::ObjectType mObjectType;
175 : : };
176 : :
177 : : class RssObjectAdapterTest : virtual public ::ad::rss::map::RssObjectAdapter<void *>
178 : : {
179 : : public:
180 : : /*!
181 : : * \brief Smart pointer on RssObjectAdapterTest
182 : : */
183 : : typedef std::shared_ptr<RssObjectAdapterTest> Ptr;
184 : :
185 : : /*!
186 : : * \brief Smart pointer on constant RssObjectAdapterTest
187 : : */
188 : : typedef std::shared_ptr<RssObjectAdapterTest const> ConstPtr;
189 : :
190 : : typedef std::tuple<::ad::map::point::Longitude, ::ad::map::point::Latitude, double> ObjectGeoLocationTuple;
191 : :
192 : 671 : static ::ad::map::point::GeoPoint getGeoPoint(ObjectGeoLocationTuple const &objectLocation)
193 : : {
194 : 671 : auto const lon = std::get<0>(objectLocation);
195 : 671 : auto const lat = std::get<1>(objectLocation);
196 [ + + + - : 671 : if (lon.isValid() && lat.isValid())
+ + ]
197 : : {
198 : 662 : return ::ad::map::point::createGeoPoint(lon, lat, ::ad::map::point::AltitudeUnknown);
199 : : }
200 : : else
201 : : {
202 : 9 : return ::ad::map::point::GeoPoint();
203 : : }
204 : : }
205 : :
206 : 823 : static ::ad::map::point::ENUPoint getENUPoint(ad::map::point::GeoPoint const &geo_point)
207 : : {
208 [ + + ]: 823 : if (isValid(geo_point, false))
209 : : {
210 [ + + ]: 814 : if (geo_point.altitude == ::ad::map::point::AltitudeUnknown)
211 : : {
212 [ + - ]: 813 : ::ad::map::match::AdMapMatching mapMatching;
213 [ + - ]: 813 : auto matchResults = mapMatching.findLanes(geo_point, ad::physics::Distance(0.1));
214 [ + - ]: 813 : if (!matchResults.empty())
215 : : {
216 [ + - ]: 813 : return ::ad::map::point::toENU(matchResults[0].matched_point);
217 : : }
218 : 0 : return ::ad::map::point::ENUPoint();
219 : 813 : }
220 : 1 : return ::ad::map::point::toENU(geo_point);
221 : : }
222 : : else
223 : : {
224 : 9 : return ::ad::map::point::ENUPoint();
225 : : }
226 : : }
227 : :
228 : 671 : static ::ad::map::point::ENUPoint getENUPoint(ObjectGeoLocationTuple const &objectLocation)
229 : : {
230 : 671 : auto const geo_point = getGeoPoint(objectLocation);
231 [ + - ]: 1342 : return getENUPoint(geo_point);
232 : : }
233 : :
234 : 661 : static ::ad::map::point::ENUHeading getENUHeading(ObjectGeoLocationTuple const &objectLocation)
235 : : {
236 : 661 : auto const heading = ::ad::physics::Angle(std::get<2>(objectLocation));
237 [ + - - + ]: 661 : if (std::fabs(heading) > ::ad::physics::c2PI)
238 : : {
239 [ # # ]: 0 : ::ad::map::match::AdMapMatching mapMatching;
240 : : auto const matchPositionRouteStart = mapMatching.getMapMatchedPositions(
241 [ # # ]: 0 : getGeoPoint(objectLocation), ::ad::physics::Distance(1), ::ad::physics::Probability(0.5));
242 [ # # ]: 0 : if (!matchPositionRouteStart.empty())
243 : : {
244 [ # # ]: 0 : return ::ad::map::lane::getLaneENUHeading(matchPositionRouteStart.front());
245 : : }
246 : : else
247 : : {
248 : 0 : return ::ad::map::point::ENUHeading();
249 : : }
250 : 0 : }
251 [ + - ]: 661 : return ::ad::map::point::createENUHeading(heading);
252 : : }
253 : :
254 : 427 : void prepareMatchPosition()
255 : : {
256 [ + - ]: 427 : ::ad::map::match::AdMapMatching mapMatching;
257 [ + - ]: 427 : mTestMatchObject = getMatchedObject(mapMatching, ::ad::physics::Distance(2.));
258 : 427 : }
259 : :
260 : 427 : ::ad::rss::map::RssObjectData getTestRssObjecData()
261 : : {
262 : 427 : ::ad::rss::map::RssObjectData rssObjectData;
263 : 427 : rssObjectData.id = getObjectId();
264 : 427 : rssObjectData.type = getObjectType();
265 : 427 : rssObjectData.last_update = std::chrono::system_clock::now();
266 [ + + ]: 427 : if (mTestMatchObject.map_matched_bounding_box.reference_point_positions.empty())
267 : : {
268 [ + - ]: 275 : prepareMatchPosition();
269 : : }
270 [ + - ]: 427 : rssObjectData.match_object = mTestMatchObject;
271 : 427 : rssObjectData.speed_range = getSpeedRange();
272 : 427 : rssObjectData.yaw_rate = getYawRate();
273 [ + - ]: 427 : rssObjectData.rss_dynamics = getDefaultRssDynamics();
274 : 427 : rssObjectData.steering_angle = getSteeringAngle();
275 : 427 : return rssObjectData;
276 : 0 : }
277 : :
278 : 439 : RssObjectInstanceTest::Ptr getObjectInstance()
279 : : {
280 : 439 : return std::dynamic_pointer_cast<RssObjectInstanceTest>(mObjectInstance);
281 : : }
282 : :
283 : 439 : explicit RssObjectAdapterTest(RssObjectInstanceTest::Ptr objectInstance, ObjectGeoLocationTuple const &objectLocation)
284 : 439 : : RssObjectAdapter<void *>(objectInstance)
285 : : {
286 : 439 : id = getObjectInstance()->mObjectId;
287 : 439 : yaw_rate = ad::physics::AngularVelocity(0.);
288 [ + - ]: 439 : updatePosition(objectLocation);
289 : 439 : dimension.length = ::ad::physics::Distance(4.5);
290 : 439 : dimension.width = ::ad::physics::Distance(2.);
291 : 439 : dimension.height = ::ad::physics::Distance(1.5);
292 : 439 : speed_range.minimum = ::ad::physics::Speed(0.);
293 : 439 : speed_range.maximum = ::ad::physics::Speed(0.);
294 : 439 : steering_angle = ::ad::physics::Angle(0.);
295 : 439 : }
296 : :
297 : 670 : void updatePosition(ObjectGeoLocationTuple const &objectLocation)
298 : : {
299 [ + - ]: 670 : auto enuPoint = getENUPoint(objectLocation);
300 [ + - + + ]: 670 : if (withinValidInputRange(enuPoint, false))
301 : : {
302 [ + - ]: 661 : heading = getENUHeading(objectLocation);
303 : 661 : center_point = enuPoint;
304 : : }
305 : 670 : }
306 : :
307 : 439 : virtual ~RssObjectAdapterTest() = default;
308 : :
309 : : ::ad::rss::world::RssDynamics mDefaultRssDynamics;
310 : :
311 : : ::ad::map::match::Object mTestMatchObject;
312 : : };
313 : :
314 : : class RssPedestrianAdapterTest : public ::ad::rss::map::RssPedestrianAdapter<void *>, public RssObjectAdapterTest
315 : : {
316 : : public:
317 : : /*!
318 : : * \brief Smart pointer on RssPedestrianAdapterTest
319 : : */
320 : : typedef std::shared_ptr<RssPedestrianAdapterTest> Ptr;
321 : :
322 : : /*!
323 : : * \brief Smart pointer on constant RssPedestrianAdapterTest
324 : : */
325 : : typedef std::shared_ptr<RssPedestrianAdapterTest const> ConstPtr;
326 : :
327 : : ::ad::rss::map::RssObjectData getTestRssObjecData()
328 : : {
329 : : return RssObjectAdapterTest::getTestRssObjecData();
330 : : }
331 : :
332 : 1 : RssPedestrianAdapterTest(RssObjectInstanceTest::Ptr objectInstance, ObjectGeoLocationTuple const &objectLocation)
333 : 1 : : ::ad::rss::map::RssObjectAdapter<void *>(objectInstance)
334 : : , ::ad::rss::map::RssPedestrianAdapter<void *>(objectInstance)
335 [ + - + - : 1 : , RssObjectAdapterTest(objectInstance, objectLocation)
+ - ]
336 : : {
337 : 1 : }
338 : : };
339 : :
340 : : class RssVehicleAdapterTest : public virtual ::ad::rss::map::RssVehicleAdapter<void *>, public RssObjectAdapterTest
341 : : {
342 : : public:
343 : : /*!
344 : : * \brief Smart pointer on RssVehicleAdapterTest
345 : : */
346 : : typedef std::shared_ptr<RssVehicleAdapterTest> Ptr;
347 : :
348 : : /*!
349 : : * \brief Smart pointer on constant RssVehicleAdapterTest
350 : : */
351 : : typedef std::shared_ptr<RssVehicleAdapterTest const> ConstPtr;
352 : :
353 : 213 : ::ad::rss::map::RssObjectData getTestRssObjecData()
354 : : {
355 : 213 : return RssObjectAdapterTest::getTestRssObjecData();
356 : : }
357 : :
358 : 152 : void prepareRoute()
359 : : {
360 [ + - + - ]: 152 : auto const routingTargetOperation = getCheckerControl()->getCurrentRoutingTargetOperation(getObjectId());
361 [ + - ]: 152 : if (routingTargetOperation.command != RssRoutingTargetCommand::None)
362 : : {
363 : 152 : ::ad::map::point::ParaPoint startPoint;
364 [ - + ]: 152 : if (!mTestMatchObject.map_matched_bounding_box.reference_point_positions.empty())
365 : : {
366 : : startPoint
367 : 0 : = mTestMatchObject.map_matched_bounding_box
368 : 0 : .reference_point_positions[static_cast<uint64_t>(::ad::map::match::ObjectReferencePoints::Center)][0]
369 : : .lane_point.para_point;
370 : : }
371 : : else
372 : : {
373 [ + - + - ]: 152 : startPoint = ::ad::map::lane::uniqueParaPoint(::ad::map::point::toGeo(getCenterPoint()));
374 : : }
375 [ + - ]: 304 : mTestRoute = ::ad::map::route::planning::planRoute(
376 [ + - ]: 304 : ::ad::map::route::planning::createRoutingPoint(startPoint, getHeading()),
377 : 152 : routingTargetOperation.routingTargets);
378 : : }
379 : 152 : }
380 : :
381 : 152 : void shortenRoute()
382 : : {
383 [ + - ]: 152 : if (!mTestMatchObject.map_matched_bounding_box.reference_point_positions.empty())
384 : : {
385 : 152 : ad::map::point::ParaPointList allLaneMatches;
386 : 456 : for (auto referencePoint :
387 [ + + ]: 608 : {::ad::map::match::ObjectReferencePoints::RearRight, ::ad::map::match::ObjectReferencePoints::RearLeft})
388 : : {
389 : : auto const &referencePosition
390 : 304 : = mTestMatchObject.map_matched_bounding_box.reference_point_positions[size_t(referencePoint)];
391 [ + - ]: 304 : auto const paraPoints = ::ad::map::match::getParaPoints(referencePosition);
392 [ + - ]: 304 : allLaneMatches.insert(allLaneMatches.end(), paraPoints.begin(), paraPoints.end());
393 : 304 : }
394 : :
395 : 152 : ::ad::map::route::shortenRoute(
396 : : allLaneMatches,
397 [ + - ]: 152 : mTestRoute,
398 : : ::ad::map::route::ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute);
399 : 152 : }
400 : 152 : }
401 : :
402 : 438 : RssVehicleAdapterTest(RssObjectInstanceTest::Ptr objectInstance, ObjectGeoLocationTuple const &objectLocation)
403 : 217 : : ::ad::rss::map::RssObjectAdapter<void *>(objectInstance)
404 : : , ::ad::rss::map::RssVehicleAdapter<void *>(objectInstance)
405 [ + - + - : 438 : , RssObjectAdapterTest(objectInstance, objectLocation)
+ - ]
406 : : {
407 : 438 : }
408 : :
409 : : ::ad::map::route::FullRoute mTestRoute;
410 : : };
411 : :
412 : : class RssEgoVehicleAdapterTest : public ::ad::rss::map::RssEgoVehicleAdapter<void *>, public RssVehicleAdapterTest
413 : : {
414 : : public:
415 : : /*!
416 : : * \brief Smart pointer on RssEgoVehicleAdapterTest
417 : : */
418 : : typedef std::shared_ptr<RssEgoVehicleAdapterTest> Ptr;
419 : :
420 : : /*!
421 : : * \brief Smart pointer on constant RssEgoVehicleAdapterTest
422 : : */
423 : : typedef std::shared_ptr<RssEgoVehicleAdapterTest const> ConstPtr;
424 : :
425 : 214 : ::ad::rss::map::RssObjectData getTestRssObjecData()
426 : : {
427 : 214 : return RssObjectAdapterTest::getTestRssObjecData();
428 : : }
429 : :
430 : 221 : RssEgoVehicleAdapterTest(RssObjectInstanceTest::Ptr objectInstance, ObjectGeoLocationTuple const &objectLocation)
431 : 221 : : ::ad::rss::map::RssObjectAdapter<void *>(objectInstance)
432 : : , ::ad::rss::map::RssVehicleAdapter<void *>(objectInstance)
433 : : , ::ad::rss::map::RssEgoVehicleAdapter<void *>(objectInstance)
434 [ + - + - : 221 : , RssVehicleAdapterTest(objectInstance, objectLocation)
+ - + - ]
435 : : {
436 : 221 : }
437 : : };
438 : :
439 : : } // namespace test_support
440 : : } // namespace map
441 : : } // namespace rss
442 : : } // namespace ad
|
