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 "ad/rss/map/RssObjectConversion.hpp"
10 : :
11 : : #include <ad/map/point/HeadingOperation.hpp>
12 : : #include <ad/map/route/RouteOperation.hpp>
13 : : #include <ad/physics/Operation.hpp>
14 : : #include <ad/rss/structured/RssFormulas.hpp>
15 : : #include <algorithm>
16 : : #include "ad/rss/map/Logging.hpp"
17 : : #include "ad/rss/map/RssWorldModelCreator.hpp"
18 : :
19 : : namespace ad {
20 : : namespace rss {
21 : : namespace map {
22 : :
23 : 526 : RssObjectConversion::RssObjectConversion(RssObjectData const &object_data)
24 : 526 : : mObjectMapMatchedPosition(&object_data.match_object)
25 : 526 : , mMaxSpeedOnAcceleration(0.)
26 : 526 : , mOriginalObjectSpeed(object_data.speed_range)
27 : 526 : , mRssDynamics(object_data.rss_dynamics)
28 : : {
29 : 526 : initializeRssObject(object_data.id,
30 [ + + ]: 526 : object_data.type,
31 : 527 : ::ad::rss::world::OccupiedRegionVector(),
32 : 526 : object_data.match_object.enu_position,
33 : 526 : object_data.speed_range,
34 : 526 : object_data.yaw_rate,
35 : 526 : object_data.steering_angle);
36 : 526 : }
37 : :
38 : 6 : RssObjectConversion::RssObjectConversion(RssObjectData const &object_data,
39 : 6 : ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions)
40 : 6 : : mObjectMapMatchedPosition(nullptr)
41 : 6 : , mMaxSpeedOnAcceleration(0.)
42 : 6 : , mOriginalObjectSpeed(object_data.speed_range)
43 : 6 : , mRssDynamics(object_data.rss_dynamics)
44 : : {
45 : 6 : initializeRssObject(object_data.id,
46 : 6 : object_data.type,
47 : : objectOccupiedRegions,
48 : 6 : object_data.match_object.enu_position,
49 : 6 : object_data.speed_range,
50 : 6 : object_data.yaw_rate,
51 [ + - ]: 6 : object_data.steering_angle);
52 : 6 : }
53 : :
54 : 532 : void RssObjectConversion::initializeRssObject(::ad::rss::world::ObjectId const &object_id,
55 : : ::ad::rss::world::ObjectType const &object_type,
56 : : ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions,
57 : : ::ad::map::match::ENUObjectPosition const &objectEnuPosition,
58 : : ::ad::physics::SpeedRange const &objectSpeed,
59 : : ::ad::physics::AngularVelocity const &objectYawRate,
60 : : ::ad::physics::Angle const &objectSteeringAngle)
61 : : {
62 : 532 : mRssObject.object_id = object_id;
63 : 532 : mRssObject.object_type = object_type;
64 : 532 : mRssObject.velocity.speed_lon_min = ::ad::physics::Speed(0.);
65 : 532 : mRssObject.velocity.speed_lon_max = ::ad::physics::Speed(0.);
66 : 532 : mRssObject.velocity.speed_lat_min = ::ad::physics::Speed(0.);
67 : 532 : mRssObject.velocity.speed_lat_max = ::ad::physics::Speed(0.);
68 : 532 : mRssObject.occupied_regions = objectOccupiedRegions;
69 : :
70 : 532 : mRssObject.state.yaw = ::ad::physics::Angle(objectEnuPosition.heading.mENUHeading);
71 : 532 : mRssObject.state.center_point.x = ::ad::physics::Distance(objectEnuPosition.center_point.x.mENUCoordinate);
72 : 532 : mRssObject.state.center_point.y = ::ad::physics::Distance(objectEnuPosition.center_point.y.mENUCoordinate);
73 : 532 : mRssObject.state.dimension.width = objectEnuPosition.dimension.width;
74 : 532 : mRssObject.state.dimension.length = objectEnuPosition.dimension.length;
75 : 532 : mRssObject.state.steering_angle = objectSteeringAngle;
76 : :
77 : : // restrict to positive speeds
78 [ + + ]: 532 : mRssObject.state.speed_range.minimum = std::max(::ad::physics::Speed(0.), objectSpeed.minimum);
79 [ + - ]: 531 : mRssObject.state.speed_range.maximum = std::max(::ad::physics::Speed(0.), objectSpeed.maximum);
80 [ + - + + ]: 531 : if (mRssObject.state.speed_range.maximum == ::ad::physics::Speed(0.))
81 : : {
82 : : // ensure angular speed is also zero if speed is zero
83 : 6 : mRssObject.state.yaw_rate = ::ad::physics::AngularVelocity(0.);
84 : : }
85 : : else
86 : : {
87 : 525 : mRssObject.state.yaw_rate = objectYawRate;
88 : : }
89 : 531 : }
90 : :
91 : 734 : bool RssObjectConversion::isOriginalSpeedAcceptable(::ad::physics::Speed const acceptableNegativeSpeed) const
92 : : {
93 [ + - ]: 734 : if (mRssObject.state.speed_range == mOriginalObjectSpeed)
94 : : {
95 : 734 : return true;
96 : : }
97 [ # # ]: 0 : if (mOriginalObjectSpeed.minimum >= acceptableNegativeSpeed)
98 : : {
99 : 0 : return true;
100 : : }
101 : 0 : return false;
102 : : }
103 : :
104 : 2350 : ::ad::rss::world::Object const &RssObjectConversion::getRssObject() const
105 : : {
106 : 2350 : return mRssObject;
107 : : }
108 : :
109 : 6816 : ::ad::rss::world::ObjectId RssObjectConversion::getId() const
110 : : {
111 : 6816 : return mRssObject.object_id;
112 : : }
113 : :
114 : 870 : ::ad::rss::world::RssDynamics RssObjectConversion::getRssDynamics() const
115 : : {
116 : 870 : ::ad::rss::world::RssDynamics resultDynamics(mRssDynamics);
117 [ + - + + ]: 870 : if (mMaxSpeedOnAcceleration != ::ad::physics::Speed(0.))
118 : : {
119 : 481 : resultDynamics.max_speed_on_acceleration = mMaxSpeedOnAcceleration;
120 : : }
121 : 870 : return resultDynamics;
122 : : }
123 : :
124 : 548 : bool RssObjectConversion::calculateConservativeMinStoppingDistance(
125 : : ::ad::rss::world::ObjectId const &object_id,
126 : : ::ad::physics::Speed const ¤t_max_speed,
127 : : ::ad::rss::world::RssDynamics const &rss_dynamics,
128 : : ::ad::physics::Distance &conservativeMinStoppingDistance)
129 : : {
130 : : auto const result
131 [ + - ]: 548 : = structured::calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(current_max_speed,
132 : 0 : physics::Speed::getMax(),
133 : 548 : rss_dynamics.response_time,
134 : 548 : rss_dynamics.alpha_lon.accel_max,
135 : 548 : rss_dynamics.alpha_lon.brake_min_correct,
136 : : conservativeMinStoppingDistance);
137 : :
138 [ - + ]: 548 : if (!result)
139 : : {
140 [ # # ]: 0 : getLogger()->error(
141 : : "RssObjectConversion::calculateConservativeMinStoppingDistance[ {} ]>> error calculating longitudinal "
142 : : "distance offset after stated braking pattern {} {}",
143 : : object_id,
144 : : current_max_speed,
145 : : rss_dynamics);
146 : : }
147 : 548 : return result;
148 : : }
149 : :
150 : 522 : bool RssObjectConversion::calculateConservativeMinStoppingDistance(
151 : : ::ad::physics::Distance &conservativeMinStoppingDistance) const
152 : : {
153 : 1044 : return calculateConservativeMinStoppingDistance(
154 : 522 : mRssObject.object_id, mRssObject.state.speed_range.maximum, mRssDynamics, conservativeMinStoppingDistance);
155 : : }
156 : :
157 : 1997 : void RssObjectConversion::updateSpeedLimit(::ad::physics::Speed const &max_speed_on_acceleration)
158 : : {
159 [ + - ]: 1997 : if (withinValidInputRange(max_speed_on_acceleration, false))
160 : : {
161 : 1997 : mMaxSpeedOnAcceleration = std::max(mMaxSpeedOnAcceleration, max_speed_on_acceleration);
162 : : }
163 : 1997 : }
164 : :
165 : 897 : void RssObjectConversion::addRestrictedOccupiedRegion(::ad::map::match::LaneOccupiedRegion const &laneOccupiedRegion,
166 : : ::ad::map::route::LaneInterval const &lane_interval)
167 : : {
168 [ + - ]: 897 : ::ad::rss::world::OccupiedRegion occupiedRegion;
169 : 897 : occupiedRegion.segment_id = ::ad::rss::world::LaneSegmentId(laneOccupiedRegion.lane_id.mLaneId);
170 : :
171 : 897 : ::ad::physics::ParametricValue cutAtStart;
172 [ + - + + ]: 897 : if (::ad::map::route::isRouteDirectionNegative(lane_interval))
173 : : {
174 : : occupiedRegion.lon_range.maximum
175 [ + - ]: 354 : = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.longitudinal_range.minimum;
176 : : occupiedRegion.lon_range.minimum
177 [ + - ]: 354 : = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.longitudinal_range.maximum;
178 : :
179 [ + - ]: 354 : occupiedRegion.lat_range.maximum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.lateral_range.minimum;
180 [ + - ]: 354 : occupiedRegion.lat_range.minimum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.lateral_range.maximum;
181 [ + - ]: 354 : cutAtStart = ::ad::physics::ParametricValue(1.) - lane_interval.start;
182 : : }
183 : : else
184 : : {
185 : 543 : occupiedRegion.lon_range = laneOccupiedRegion.longitudinal_range;
186 : 543 : occupiedRegion.lat_range = laneOccupiedRegion.lateral_range;
187 : 543 : cutAtStart = lane_interval.start;
188 : : }
189 : :
190 : : // scale region to the lane interval sizes
191 [ + - ]: 897 : auto const intervalLength = calcParametricLength(lane_interval);
192 [ + - + + ]: 897 : if (intervalLength == ::ad::physics::ParametricValue(0.))
193 : : {
194 : 228 : occupiedRegion.lon_range.minimum = ::ad::physics::ParametricValue(0.);
195 : 228 : occupiedRegion.lon_range.maximum = ::ad::physics::ParametricValue(1.);
196 : : }
197 : : else
198 : : {
199 : : // move region
200 [ + - ]: 669 : occupiedRegion.lon_range.minimum -= cutAtStart;
201 [ + - ]: 669 : occupiedRegion.lon_range.maximum -= cutAtStart;
202 : : // scale region
203 [ + - ]: 669 : occupiedRegion.lon_range.minimum = occupiedRegion.lon_range.minimum / intervalLength.mParametricValue;
204 [ + - ]: 669 : occupiedRegion.lon_range.maximum = occupiedRegion.lon_range.maximum / intervalLength.mParametricValue;
205 : : // restrict to segment
206 : : occupiedRegion.lon_range.minimum
207 [ + - + - ]: 669 : = std::min(std::max(occupiedRegion.lon_range.minimum, ::ad::physics::ParametricValue(0.)),
208 : 669 : ::ad::physics::ParametricValue(1.));
209 : : occupiedRegion.lon_range.maximum
210 [ + - + - ]: 669 : = std::min(std::max(occupiedRegion.lon_range.maximum, ::ad::physics::ParametricValue(0.)),
211 : 1338 : ::ad::physics::ParametricValue(1.));
212 : : }
213 [ + - + - ]: 1794 : getLogger()->trace(
214 : : "RssObjectConversion::addRestrictedOccupiedRegion[ {} ]>>laneOccupiedRegion {} lane_interval {} -> "
215 : : "occupiedRegion {}",
216 [ + - + - ]: 1794 : getId(),
217 : : laneOccupiedRegion,
218 : : lane_interval,
219 : : occupiedRegion);
220 : :
221 [ + - ]: 897 : mRssObject.occupied_regions.push_back(occupiedRegion);
222 : 897 : }
223 : :
224 : 1997 : void RssObjectConversion::laneIntervalAdded(::ad::map::route::LaneInterval const &lane_interval)
225 : : {
226 [ + - ]: 1997 : if (mObjectMapMatchedPosition != nullptr)
227 : : {
228 : : auto findLaneIntervalResult
229 [ + - ]: 1997 : = std::find_if(mObjectMapMatchedPosition->map_matched_bounding_box.lane_occupied_regions.begin(),
230 : 1997 : mObjectMapMatchedPosition->map_matched_bounding_box.lane_occupied_regions.end(),
231 : 3354 : [lane_interval](::ad::map::match::LaneOccupiedRegion const &occupiedRegion) {
232 : 3354 : return lane_interval.lane_id == occupiedRegion.lane_id;
233 : : });
234 [ + + ]: 1997 : if (findLaneIntervalResult != mObjectMapMatchedPosition->map_matched_bounding_box.lane_occupied_regions.end())
235 : : {
236 [ + - ]: 897 : addRestrictedOccupiedRegion(*findLaneIntervalResult, lane_interval);
237 : : }
238 : : }
239 : 1997 : }
240 : :
241 : 783 : void RssObjectConversion::updateVelocityOnRoute(::ad::map::point::ENUHeading const &route_heading)
242 : : {
243 [ + + ]: 783 : if (mRssObject.object_type == ::ad::rss::world::ObjectType::ArtificialVehicle)
244 : : {
245 : : // artificial vehicles are expected to drive along the respective route
246 : : // otherwhise in intersection areas they get -- depending on the route constellation --
247 : : // non reasonalbe lateral speeds
248 : : // @todo: if this is required to become possible in future, each route would desire its own
249 : : // artificial vehicle placed with the appropriate route heading
250 : 1 : mRssObject.velocity.speed_lon_min = mRssObject.state.speed_range.minimum;
251 : 1 : mRssObject.velocity.speed_lon_max = mRssObject.state.speed_range.maximum;
252 : 1 : mRssObject.velocity.speed_lat_min = ::ad::physics::Speed(0.);
253 : 1 : mRssObject.velocity.speed_lat_max = ::ad::physics::Speed(0.);
254 : : }
255 : : else
256 : : {
257 : : double headingDiff
258 [ + - ]: 782 : = physics::normalizeAngleSigned(
259 : 782 : physics::Angle(route_heading.mENUHeading - mObjectMapMatchedPosition->enu_position.heading.mENUHeading))
260 : 782 : .mAngle;
261 : :
262 : 782 : auto const headingDiffLonFactor = std::fabs(std::cos(headingDiff));
263 [ + - ]: 782 : mRssObject.velocity.speed_lon_min = headingDiffLonFactor * mRssObject.state.speed_range.minimum;
264 [ + - ]: 782 : mRssObject.velocity.speed_lon_max = headingDiffLonFactor * mRssObject.state.speed_range.maximum;
265 : :
266 : 782 : auto const headingDiffLatFactor = std::sin(headingDiff);
267 [ + + ]: 782 : if (headingDiffLatFactor < 0.)
268 : : {
269 [ + - ]: 256 : mRssObject.velocity.speed_lat_min = headingDiffLatFactor * mRssObject.state.speed_range.maximum;
270 [ + - ]: 256 : mRssObject.velocity.speed_lat_max = headingDiffLatFactor * mRssObject.state.speed_range.minimum;
271 : : }
272 : : else
273 : : {
274 [ + - ]: 526 : mRssObject.velocity.speed_lat_min = headingDiffLatFactor * mRssObject.state.speed_range.minimum;
275 [ + - ]: 526 : mRssObject.velocity.speed_lat_max = headingDiffLatFactor * mRssObject.state.speed_range.maximum;
276 : : }
277 : : }
278 : 783 : }
279 : :
280 : 127 : bool RssObjectConversion::updateVelocityOnRoute(::ad::map::route::FullRoute const &route)
281 : : {
282 [ - + ]: 127 : if (mObjectMapMatchedPosition == nullptr)
283 : : {
284 [ # # # # ]: 0 : getLogger()->error(
285 : : "RssObjectConversion::updateVelocityOnRoute[ {} ] No map matched position of the object available. "
286 : : "Cannot calculate the orientation of the route at the object position",
287 [ # # # # ]: 0 : getId());
288 : 0 : return false;
289 : : }
290 : 127 : ::ad::map::point::ENUHeading route_heading;
291 [ - + ]: 127 : if (mRssObject.object_type == ::ad::rss::world::ObjectType::ArtificialVehicle)
292 : : {
293 : : // artificial vehicles are expected to drive along the respective route
294 : : // otherwhise in intersection areas they get -- depending on the route constellation --
295 : : // non reasonalbe lateral speeds
296 : : // @todo: if this is required to become possible in future, each route would desire its own
297 : : // artificial vehicle placed with the appropriate route heading
298 : 0 : route_heading = mObjectMapMatchedPosition->enu_position.heading;
299 : : }
300 : : else
301 : : {
302 [ + - ]: 127 : route_heading = getENUHeadingOfRoute(*mObjectMapMatchedPosition, route);
303 [ + - - + ]: 127 : if (std::fabs(route_heading) >= ad::map::point::ENUHeading(2 * M_PI))
304 : : {
305 : : // if the map matched bounding box is outside, getENUHeadingOfRoute() returns 2*M_PI
306 : : // this should not happen
307 : : // - if the route input is derived from a connecting route,
308 : : // the updateVelocityOnRoute() function with the provided route_heading from the connected route
309 : : // should be taken as on creation of the ConnectedRoute the bounding boxes of the two vehicles are
310 : : // are removed from the routes to ensure the connected route is actually in between the two vehicles
311 : : // - otherwise, the route should come either from an actually planned or predicted route of this vehicles.
312 : : // In such case the
313 [ # # # # ]: 0 : getLogger()->error(
314 : : "RssObjectConversion::updateVelocityOnRoute[ {} ] object seems to be located outside the route,"
315 : : "so calculating route heading seems to be impossible. {} {}",
316 [ # # ]: 0 : getId(),
317 [ # # ]: 0 : *mObjectMapMatchedPosition,
318 : : route);
319 : 0 : return false;
320 : : }
321 : : }
322 : :
323 [ + - ]: 127 : updateVelocityOnRoute(route_heading);
324 : 127 : return true;
325 : : }
326 : :
327 : 261 : ::ad::physics::Distance RssObjectConversion::getDistanceEstimate(RssObjectConversion::ConstPtr other) const
328 : : {
329 [ + - ]: 261 : auto const d_x = (mRssObject.state.center_point.x - other->mRssObject.state.center_point.x);
330 [ + - ]: 261 : auto const d_y = (mRssObject.state.center_point.y - other->mRssObject.state.center_point.y);
331 [ + - + - : 261 : return std::sqrt(d_x * d_x + d_y * d_y);
+ - + - ]
332 : : }
333 : :
334 : : } // namespace map
335 : : } // namespace rss
336 : : } // namespace ad
|
