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 : : * @file
10 : : */
11 : :
12 : : #pragma once
13 : :
14 : : #include <ad/map/geometry/PolygonOperation.hpp>
15 : : #include <ad/map/intersection/Intersection.hpp>
16 : : #include <ad/map/route/FullRoute.hpp>
17 : : #include <ad/rss/structured/RssFormulas.hpp>
18 : : #include <ad/rss/world/WorldModel.hpp>
19 : : #include "ad/rss/map/RssObjectConversion.hpp"
20 : : #include "ad/rss/map/RssWorldModelCreation.hpp"
21 : :
22 : : /*!
23 : : * @brief namespace ad
24 : : */
25 : : namespace ad {
26 : : /*!
27 : : * @brief namespace rss
28 : : */
29 : : namespace rss {
30 : : /*!
31 : : * @brief namespace map
32 : : */
33 : : namespace map {
34 : :
35 : : /*!
36 : : * @brief class providing support to create the RSS world model and append RSS Constellations
37 : : */
38 : : class RssWorldModelCreator
39 : : {
40 : : public:
41 : : /*!
42 : : * @brief constructor
43 : : *
44 : : * @param[in] restrict_speed_limit_mode the mode to select the behavior of
45 : : * object_rss_dynamics.max_speed_on_acceleration and
46 : : * egoMaxSpeedOnAcceleration
47 : : * parameter
48 : : * @param[in] greenTrafficLights the list of known green traffic lights.
49 : : * Required to derive the correct priority rule for the ego vehicle when approaching a traffic light intersection.
50 : : * @param[in/out] worldModelCreation the world model creation object managing the world model where the newly
51 : : * created constellations are
52 : : * appended
53 : : */
54 : : RssWorldModelCreator(RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
55 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
56 : : RssWorldModelCreation &worldModelCreation);
57 : :
58 : : /*!
59 : : * @brief overloaded constructor using default values
60 : : *
61 : : * @param[in/out] worldModelCreation the world model creation object managing the world model where the newly
62 : : * created constellations are
63 : : * appended
64 : : */
65 : : explicit RssWorldModelCreator(RssWorldModelCreation &worldModelCreation);
66 : :
67 : : /*!
68 : : * \brief standard destructor
69 : : */
70 : 264 : ~RssWorldModelCreator() = default;
71 : :
72 : : /*!
73 : : * @brief append a not relevant constellation
74 : : *
75 : : * @param[in] route the full route of the ego object (allowed to be empty)
76 : : * @param[in] egoObject the ego object basic information
77 : : * @param[in] otherObject the other object basic information
78 : : *
79 : : * @returns \c true if appending of the constellation succeeded.
80 : : */
81 : : bool appendNotRelevantConstellation(::ad::map::route::FullRoute const &route,
82 : : RssObjectConversion::ConstPtr egoObject,
83 : : RssObjectConversion::ConstPtr otherObject);
84 : :
85 : : /*!
86 : : * @brief append a non intersection constellation
87 : : *
88 : : * @param[in] connectingRoute the connectingRoute route between the ego and the other object
89 : : * @param[in] constellation_type the concrete constellation type (ConstellationType::SameDirection or
90 : : * ConstellationType::OppositeDirection)
91 : : * @param[in] egoObject the ego object basic information
92 : : * @param[in] otherObject the other object basic information
93 : : *
94 : : * @returns \c true if appending of the constellation succeeded.
95 : : */
96 : : bool appendNonIntersectionConstellation(::ad::map::route::ConnectingRoute const &connectingRoute,
97 : : world::ConstellationType const &constellation_type,
98 : : RssObjectConversion::ConstPtr egoObject,
99 : : RssObjectConversion::ConstPtr otherObject);
100 : :
101 : : /*!
102 : : * @brief append a merging constellation
103 : : *
104 : : * @param[in] connectingRoute the merging connectingRoute route of the ego and the other object
105 : : * @param[in] constellation_type the concrete constellation type (usually one of the ConstellationType::Intersection*)
106 : : * @param[in] egoObject the ego object basic information
107 : : * @param[in] otherObject the other object basic information
108 : : *
109 : : * @returns \c true if appending of the constellation succeeded.
110 : : */
111 : : bool appendMergingConstellation(::ad::map::route::ConnectingRoute const &connectingRoute,
112 : : world::ConstellationType const &constellation_type,
113 : : RssObjectConversion::ConstPtr egoObject,
114 : : RssObjectConversion::ConstPtr otherObject);
115 : :
116 : : /*!
117 : : * @brief append an intersection constellation
118 : : *
119 : : * @param[in] intersection the relevant intersection to consider
120 : : * @param[in] egoRoute the object route used to create the intersection object
121 : : * @param[in] objectRoute the object route interacting with the ego route in the intersection
122 : : * @param[in] intersectionOtherRoute the route the intersection was NOT created with (either egoRoute or objectRoute)
123 : : * @param[in] egoObject the ego object basic information
124 : : * @param[in] otherObject the other object basic information
125 : : *
126 : : * @returns \c true if appending of the constellation succeeded.
127 : : */
128 : : bool appendIntersectionConstellation(::ad::map::intersection::IntersectionPtr intersection,
129 : : ::ad::map::route::FullRoute const &egoRoute,
130 : : ::ad::map::route::FullRoute const &objectRoute,
131 : : ::ad::map::route::FullRoute const &intersectionOtherRoute,
132 : : RssObjectConversion::ConstPtr egoObject,
133 : : RssObjectConversion::ConstPtr otherObject);
134 : :
135 : : /*!
136 : : * @brief append a road boundary constellation
137 : : *
138 : : * @param[in] egoRoute the object route used to create the intersection object
139 : : * @param[in] egoObject the ego object basic information
140 : : *
141 : : * @returns \c true if appending of the constellation succeeded.
142 : : */
143 : : bool appendRoadBoundaryConstellations(::ad::map::route::FullRoute const &egoRoute,
144 : : RssObjectConversion::ConstPtr egoObject);
145 : :
146 : : bool appendUnstructuredConstellation(RssObjectConversion::ConstPtr iEgoObject,
147 : : RssObjectConversion::ConstPtr iOtherObject);
148 : :
149 : : private:
150 : : struct Route
151 : : {
152 : 127 : explicit Route(::ad::map::route::FullRoute const &route_in)
153 : 127 : : route(route_in)
154 : 127 : , min_lane_offset(route_in.min_lane_offset)
155 : 127 : , max_lane_offset(route_in.max_lane_offset)
156 : : {
157 : 127 : }
158 : 0 : Route(::ad::map::route::FullRoute const &route_in,
159 : : ::ad::map::point::ENUHeading const &route_heading_start_in,
160 : : ::ad::map::point::ENUHeading const &route_heading_end_in)
161 : 0 : : route(route_in)
162 : 0 : , min_lane_offset(route_in.min_lane_offset)
163 : 0 : , max_lane_offset(route_in.max_lane_offset)
164 : 0 : , route_heading_available(true)
165 : 0 : , route_heading_start(route_heading_start_in)
166 : 0 : , route_heading_end(route_heading_end_in)
167 : : {
168 : 0 : }
169 : 328 : Route(::ad::map::route::FullRoute const &route_in,
170 : : ::ad::map::route::RouteLaneOffset const min_lane_offset_in,
171 : : ::ad::map::route::RouteLaneOffset const max_lane_offset_in,
172 : : ::ad::map::point::ENUHeading const &route_heading_start_in,
173 : : ::ad::map::point::ENUHeading const &route_heading_end_in)
174 : 328 : : route(route_in)
175 : 328 : , min_lane_offset(min_lane_offset_in)
176 : 328 : , max_lane_offset(max_lane_offset_in)
177 : 328 : , route_heading_available(true)
178 : 328 : , route_heading_start(route_heading_start_in)
179 : 328 : , route_heading_end(route_heading_end_in)
180 : : {
181 : 328 : }
182 : : ::ad::map::route::FullRoute const &route;
183 : : ::ad::map::route::RouteLaneOffset const min_lane_offset;
184 : : ::ad::map::route::RouteLaneOffset const max_lane_offset;
185 : : bool route_heading_available{false};
186 : : ::ad::map::point::ENUHeading const route_heading_start;
187 : : ::ad::map::point::ENUHeading const route_heading_end;
188 : : };
189 : :
190 : : /**
191 : : * @brief helper function to actually create the road area
192 : : *
193 : : * @param[in] route The route object the road area is created from
194 : : * @param[in] intersectionLanes The lanes to be marked as intersection (the road area creation stops when intersection
195 : : * is left)
196 : : * @param[in] objects The objects to be updated while creating the route.
197 : : * @param[out] roadArea The road area to create
198 : : *
199 : : * @returns \c true if the operation is successful
200 : : */
201 : : bool createRoadArea(Route const &route,
202 : : ::ad::map::lane::LaneIdSet const &intersectionLanes,
203 : : std::vector<RssObjectConversion::Ptr> objects,
204 : : world::RoadArea &roadArea,
205 : : std::vector<ad::map::route::RoadSegment> *intersectingArea = nullptr);
206 : :
207 : : /**
208 : : * @brief helper function to create a road area for non intersection constellations
209 : : *
210 : : * @param[in] route The route the road area should be created from
211 : : * @param[in] objects The objects to be updated while creating the route.
212 : : * @param[out] roadArea The road area to create
213 : : *
214 : : * @returns \c true if the operation is successful
215 : : */
216 : : bool createNonIntersectionRoadArea(::ad::map::route::FullRoute const &route,
217 : : ::ad::map::point::ENUHeading const &route_heading_start,
218 : : ::ad::map::point::ENUHeading const &route_heading_end,
219 : : std::vector<RssObjectConversion::Ptr> objects,
220 : : world::RoadArea &roadArea);
221 : :
222 : : /**
223 : : * @brief helper function to create a road area for merging constellations
224 : : *
225 : : * @param[in] route The route the road area should be created from
226 : : * @param[in] object The object to be updated while creating the route.
227 : : * @param[out] roadArea The road area to create
228 : : *
229 : : * @returns \c true if the operation is successful
230 : : */
231 : : bool createMergingRoadArea(::ad::map::route::FullRoute const &route,
232 : : ::ad::map::point::ENUHeading const &route_heading_start,
233 : : ::ad::map::point::ENUHeading const &route_heading_end,
234 : : RssObjectConversion::Ptr object,
235 : : world::RoadArea &roadArea);
236 : :
237 : : /**
238 : : * @brief helper function to create a road area for intersection constellations
239 : : *
240 : : * @param[in] route The route the road area should be created from
241 : : * @param[in] intersection The intersection on the route the road area should be created for
242 : : * @param[in] object The object to be updated while creating the route.
243 : : * @param[out] roadArea The road area to create
244 : : *
245 : : * @returns \c true if the operation is successful
246 : : */
247 : : bool createIntersectionRoadArea(::ad::map::route::FullRoute const &route,
248 : : ::ad::map::intersection::IntersectionConstPtr intersection,
249 : : RssObjectConversion::Ptr object,
250 : : world::RoadArea &roadArea,
251 : : std::vector<ad::map::route::RoadSegment> &intersectingArea);
252 : :
253 : : /**
254 : : * @brief helper function to actually append a new constellation to the world model
255 : : *
256 : : * @param[in] constellation_type The type of constellation
257 : : * @param[in] egoObject The ego object of the constellation
258 : : * @param[in] egoRoad The ego road of the constellation
259 : : * @param[in] otherObject The other object of the constellation
260 : : * @param[in] intersecting_road The intersecting road of the constellation
261 : : */
262 : : bool appendConstellation(world::ConstellationType const &constellation_type,
263 : : RssObjectConversion::ConstPtr egoObject,
264 : : world::RoadArea const &egoRoad,
265 : : RssObjectConversion::ConstPtr otherObject,
266 : : world::RoadArea const &intersecting_road);
267 : :
268 : : /**
269 : : * @brief helper function to extract the relevant intersection lane intervals and associated polygons from the
270 : : * intersection road segments
271 : : *
272 : : * @param[in] intersectionArea the road segments being part of the intersecting area to process
273 : : * @param[out] intersectionLaneIntervals the list of lane intervals being part of the actual intersecting area
274 : : * @param[out] polygon The polygon of the actual intersecting area
275 : : */
276 : : void extractLaneIntervalsAndPolygon(std::vector<ad::map::route::RoadSegment> const &intersectionArea,
277 : : std::vector<ad::map::route::LaneInterval> &intersectionLaneIntervals,
278 : : ::ad::geometry::Polygon &polygon);
279 : :
280 : : /**
281 : : * @brief helper function to update the roadArea to restrict the intersection part to the exact intersecting area
282 : : *
283 : : * @param[in/out] roadArea the road area to update and process
284 : : * @param[in] originalLaneIntervals the original lane intervals used as input for the corresponding roadArea segments
285 : : * @param[in] exactIntersectingRegions the LaneOccupiedRegions defining the exact intersecting area
286 : : *
287 : : * @returns \c true if the roadArea contains an actually intersecting area. Otherwhise \c false is returned indicating
288 : : * that no actual intersecting area is present within the roadArea.
289 : : */
290 : : bool updateExactIntersectionArea(world::RoadArea &roadArea,
291 : : std::vector<ad::map::route::LaneInterval> const &originalLaneIntervals,
292 : : ad::map::match::LaneOccupiedRegionList const &exactIntersectingRegions);
293 : :
294 : : /**
295 : : * @brief helper function to update both roadAreas to restrict the intersection part to the exact intersecting area of
296 : : * both
297 : : *
298 : : * @param[in/out] roadAreaA the road area of the vehicle A to update and process
299 : : * @param[in] intersectionAreaA the source road segments being part of the intersecting area corresponding to vehicle
300 : : * A
301 : : * @param[in/out] roadAreaB the road area of the vehicle B to update and process
302 : : * @param[in] intersectionAreaB the source road segments being part of the intersecting area corresponding to vehicle
303 : : * B
304 : : *
305 : : * @returns \c true if both roadAreas contain an actually intersecting area. Otherwhise \c false is returned
306 : : * indicating that no actual intersecting area is present within the two roadAreas.
307 : : */
308 : : bool updateExactIntersectionArea(world::RoadArea &roadAreaA,
309 : : std::vector<ad::map::route::RoadSegment> const &intersectionAreaA,
310 : : world::RoadArea &roadAreaB,
311 : : std::vector<ad::map::route::RoadSegment> const &intersectionAreaB);
312 : :
313 : : double mSpeedLimitFactor;
314 : : ::ad::map::landmark::LandmarkIdSet mGreenTrafficLights;
315 : : RssWorldModelCreation &mWorldModelCreation;
316 : : };
317 : :
318 : : } // namespace map
319 : : } // namespace rss
320 : : } // namespace ad
|
