Skip to content
This repository was archived by the owner on Aug 7, 2025. It is now read-only.

Commit f748a0f

Browse files
fpaschfpasch
authored
Fixes in unstructured constellation polygon creation (#87)
- Add several parameters to adjust polygon calculation - Rename parameters: vehicleFrontIntermediateRatioSteps -> vehicleFrontIntermediateYawRateChangeRatioSteps vehicleBackIntermediateRatioSteps -> vehicleBackIntermediateYawRateChangeRatioSteps - code refactoring (pedestrian and vehicle calculations now use common functions)
1 parent 3d6d7a0 commit f748a0f

31 files changed

Lines changed: 4056 additions & 1577 deletions

‎ad_rss/generated/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
##
1515

1616
cmake_minimum_required(VERSION 3.5)
17-
project(ad_rss VERSION 4.3.0)
17+
project(ad_rss VERSION 4.4.0)
1818

1919
include(GNUInstallDirs)
2020
include(CMakePackageConfigHelpers)

‎ad_rss/generated/include/ad/rss/world/UnstructuredSettings.hpp‎

Lines changed: 75 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -106,11 +106,21 @@ struct UnstructuredSettings
106106
return (pedestrianTurningRadius == other.pedestrianTurningRadius) && (driveAwayMaxAngle == other.driveAwayMaxAngle)
107107
&& (vehicleYawRateChange == other.vehicleYawRateChange) && (vehicleMinRadius == other.vehicleMinRadius)
108108
&& (vehicleTrajectoryCalculationStep == other.vehicleTrajectoryCalculationStep)
109-
&& (vehicleFrontIntermediateRatioSteps == other.vehicleFrontIntermediateRatioSteps)
110-
&& (vehicleBackIntermediateRatioSteps == other.vehicleBackIntermediateRatioSteps)
109+
&& (vehicleFrontIntermediateYawRateChangeRatioSteps == other.vehicleFrontIntermediateYawRateChangeRatioSteps)
110+
&& (vehicleBackIntermediateYawRateChangeRatioSteps == other.vehicleBackIntermediateYawRateChangeRatioSteps)
111111
&& (vehicleBrakeIntermediateAccelerationSteps == other.vehicleBrakeIntermediateAccelerationSteps)
112112
&& (vehicleContinueForwardIntermediateAccelerationSteps
113-
== other.vehicleContinueForwardIntermediateAccelerationSteps);
113+
== other.vehicleContinueForwardIntermediateAccelerationSteps)
114+
&& (vehicleContinueForwardIntermediateYawRateChangeRatioSteps
115+
== other.vehicleContinueForwardIntermediateYawRateChangeRatioSteps)
116+
&& (pedestrianContinueForwardIntermediateHeadingChangeRatioSteps
117+
== other.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps)
118+
&& (pedestrianContinueForwardIntermediateAccelerationSteps
119+
== other.pedestrianContinueForwardIntermediateAccelerationSteps)
120+
&& (pedestrianBrakeIntermediateAccelerationSteps == other.pedestrianBrakeIntermediateAccelerationSteps)
121+
&& (pedestrianFrontIntermediateHeadingChangeRatioSteps
122+
== other.pedestrianFrontIntermediateHeadingChangeRatioSteps)
123+
&& (pedestrianBackIntermediateHeadingChangeRatioSteps == other.pedestrianBackIntermediateHeadingChangeRatioSteps);
114124
}
115125

116126
/**
@@ -164,29 +174,63 @@ struct UnstructuredSettings
164174
* specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
165175
* used for the front of the trajectory set.
166176
*/
167-
uint32_t vehicleFrontIntermediateRatioSteps{0};
177+
uint32_t vehicleFrontIntermediateYawRateChangeRatioSteps{0};
168178

169179
/*!
170180
* During calculation of the trajectory set, multiple yaw rate ratios are used. The default is max left, max right and
171181
* no yaw rate change. By specifying a value larger than zero more intermediate steps are used. The value is
172182
* specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
173183
* used for the back of the trajectory set.
174184
*/
175-
uint32_t vehicleBackIntermediateRatioSteps{0};
185+
uint32_t vehicleBackIntermediateYawRateChangeRatioSteps{0};
176186

177187
/*!
178188
* Specifies the intermediate acceleration steps (between brakeMax and brakeMin) used while calculating the cbrake
179-
* trajectory set. This is applied to all vehicleResponseIntermediateAccelerationSteps, therefore it has only an
180-
* effect if that value is >0.
189+
* trajectory set.
181190
*/
182191
uint32_t vehicleBrakeIntermediateAccelerationSteps{0};
183192

184193
/*!
185194
* Specifies the intermediate acceleration steps (between brakeMin and accelMax) used while calculating the continue
186-
* forward trajectory set. This is applied to all vehicleResponseIntermediateAccelerationSteps, therefore it has only
187-
* an effect if that value is >0.
195+
* forward trajectory set.
188196
*/
189197
uint32_t vehicleContinueForwardIntermediateAccelerationSteps{0};
198+
199+
/*!
200+
* Specifies the intermediate yaw rate change ratio steps used while calculating the continue forward trajectory set.
201+
*/
202+
uint32_t vehicleContinueForwardIntermediateYawRateChangeRatioSteps{0};
203+
204+
/*!
205+
* Specifies the intermediate heading change ratio steps used while calculating the continue forward trajectory set.
206+
*/
207+
uint32_t pedestrianContinueForwardIntermediateHeadingChangeRatioSteps{0};
208+
209+
/*!
210+
* Specifies the intermediate steps used while calculating the continue forward trajectory set.
211+
*/
212+
uint32_t pedestrianContinueForwardIntermediateAccelerationSteps{0};
213+
214+
/*!
215+
* Specifies the intermediate steps used while calculating the brake trajectory set.
216+
*/
217+
uint32_t pedestrianBrakeIntermediateAccelerationSteps{0};
218+
219+
/*!
220+
* During calculation of the trajectory set, multiple heading change ratios are used. The default is max left, max
221+
* right and no heading change. By specifying a value larger than zero more intermediate steps are used. The value is
222+
* specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
223+
* used for the front of the trajectory set.
224+
*/
225+
uint32_t pedestrianFrontIntermediateHeadingChangeRatioSteps{0};
226+
227+
/*!
228+
* During calculation of the trajectory set, multiple heading change ratios are used. The default is max left, max
229+
* right and no yaw rate change. By specifying a value larger than zero more intermediate steps are used. The value is
230+
* specifying the steps on one side, therefore the resulting intermedate steps are twice this value. This value is
231+
* used for the back of the trajectory set.
232+
*/
233+
uint32_t pedestrianBackIntermediateHeadingChangeRatioSteps{0};
190234
};
191235

192236
} // namespace world
@@ -238,17 +282,35 @@ inline std::ostream &operator<<(std::ostream &os, UnstructuredSettings const &_v
238282
os << "vehicleTrajectoryCalculationStep:";
239283
os << _value.vehicleTrajectoryCalculationStep;
240284
os << ",";
241-
os << "vehicleFrontIntermediateRatioSteps:";
242-
os << _value.vehicleFrontIntermediateRatioSteps;
285+
os << "vehicleFrontIntermediateYawRateChangeRatioSteps:";
286+
os << _value.vehicleFrontIntermediateYawRateChangeRatioSteps;
243287
os << ",";
244-
os << "vehicleBackIntermediateRatioSteps:";
245-
os << _value.vehicleBackIntermediateRatioSteps;
288+
os << "vehicleBackIntermediateYawRateChangeRatioSteps:";
289+
os << _value.vehicleBackIntermediateYawRateChangeRatioSteps;
246290
os << ",";
247291
os << "vehicleBrakeIntermediateAccelerationSteps:";
248292
os << _value.vehicleBrakeIntermediateAccelerationSteps;
249293
os << ",";
250294
os << "vehicleContinueForwardIntermediateAccelerationSteps:";
251295
os << _value.vehicleContinueForwardIntermediateAccelerationSteps;
296+
os << ",";
297+
os << "vehicleContinueForwardIntermediateYawRateChangeRatioSteps:";
298+
os << _value.vehicleContinueForwardIntermediateYawRateChangeRatioSteps;
299+
os << ",";
300+
os << "pedestrianContinueForwardIntermediateHeadingChangeRatioSteps:";
301+
os << _value.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps;
302+
os << ",";
303+
os << "pedestrianContinueForwardIntermediateAccelerationSteps:";
304+
os << _value.pedestrianContinueForwardIntermediateAccelerationSteps;
305+
os << ",";
306+
os << "pedestrianBrakeIntermediateAccelerationSteps:";
307+
os << _value.pedestrianBrakeIntermediateAccelerationSteps;
308+
os << ",";
309+
os << "pedestrianFrontIntermediateHeadingChangeRatioSteps:";
310+
os << _value.pedestrianFrontIntermediateHeadingChangeRatioSteps;
311+
os << ",";
312+
os << "pedestrianBackIntermediateHeadingChangeRatioSteps:";
313+
os << _value.pedestrianBackIntermediateHeadingChangeRatioSteps;
252314
os << ")";
253315
return os;
254316
}

‎ad_rss/generated/include/ad_rss/Version.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
/*!
2626
* The minor version of ad_rss
2727
*/
28-
#define AD_RSS_VERSION_MINOR 3
28+
#define AD_RSS_VERSION_MINOR 4
2929

3030
/*!
3131
* The revision of ad_rss
@@ -35,4 +35,4 @@
3535
/*!
3636
* The version of ad_rss as string
3737
*/
38-
#define AD_RSS_VERSION_STRING "4.3.0"
38+
#define AD_RSS_VERSION_STRING "4.4.0"

‎ad_rss/impl/src/unstructured/Geometry.cpp‎

Lines changed: 36 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -188,9 +188,42 @@ bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result)
188188
boost::geometry::union_(a.outer(), b.outer(), unionPolygons);
189189
if (unionPolygons.size() != 1)
190190
{
191-
spdlog::debug("Could not calculate combined polygon. Expected 1 polygon after union, found {}",
192-
unionPolygons.size());
193-
return false;
191+
auto fixed = false;
192+
if (unionPolygons.size() == 0)
193+
{
194+
// if union reports zero polygons, it might happen that one polygon is covered by the other
195+
auto aCoveredByB = true;
196+
for (auto it = a.outer().cbegin(); (it != a.outer().cend()) && aCoveredByB; ++it)
197+
{
198+
aCoveredByB &= boost::geometry::covered_by(*it, b.outer());
199+
}
200+
if (aCoveredByB)
201+
{
202+
fixed = true;
203+
result = b;
204+
}
205+
if (!fixed)
206+
{
207+
auto bCoveredByA = true;
208+
for (auto it = b.outer().cbegin(); (it != b.outer().cend()) && bCoveredByA; ++it)
209+
{
210+
bCoveredByA &= boost::geometry::covered_by(*it, a.outer());
211+
}
212+
if (bCoveredByA)
213+
{
214+
fixed = true;
215+
result = a;
216+
}
217+
}
218+
}
219+
if (!fixed)
220+
{
221+
spdlog::warn("Could not calculate combined polygon. Expected 1 polygon after union, found {}. A:\n{}\nB:\n{}",
222+
unionPolygons.size(),
223+
std::to_string(a),
224+
std::to_string(b));
225+
return false;
226+
}
194227
}
195228
else
196229
{

‎ad_rss/impl/src/unstructured/TrajectoryCommon.cpp‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212

1313
#include "TrajectoryCommon.hpp"
1414
#include <ad/physics/Operation.hpp>
15+
#include "ad/rss/unstructured/DebugDrawing.hpp"
1516

1617
/*!
1718
* @brief namespace ad
@@ -57,6 +58,180 @@ Point getVehicleCorner(TrajectoryPoint const &point,
5758
return resultPoint;
5859
}
5960

61+
bool calculateStepPolygon(situation::VehicleState const &vehicleState,
62+
TrajectorySetStep const &step,
63+
std::string const &debugNamespace,
64+
Polygon &polygon,
65+
TrajectorySetStepVehicleLocation &stepVehicleLocation)
66+
{
67+
MultiPoint frontPtsLeft;
68+
MultiPoint frontPtsRight;
69+
70+
int idx = 0;
71+
for (auto it = step.left.cbegin(); it != step.left.cend(); ++it)
72+
{
73+
auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState);
74+
DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx));
75+
boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint());
76+
if (it == step.left.end() - 1)
77+
{
78+
stepVehicleLocation.left = vehicleLocationLeft;
79+
}
80+
++idx;
81+
}
82+
83+
// center
84+
auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState);
85+
DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center");
86+
boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint());
87+
stepVehicleLocation.center = vehicleLocationCenter;
88+
boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint());
89+
90+
idx = 0;
91+
for (auto it = step.right.cbegin(); it != step.right.cend(); ++it)
92+
{
93+
auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState);
94+
DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx));
95+
boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint());
96+
if (it == step.right.begin())
97+
{
98+
stepVehicleLocation.right = vehicleLocationRight;
99+
}
100+
++idx;
101+
}
102+
103+
Polygon hullLeft;
104+
Polygon hullRight;
105+
boost::geometry::convex_hull(frontPtsLeft, hullLeft);
106+
boost::geometry::convex_hull(frontPtsRight, hullRight);
107+
auto result = combinePolygon(hullLeft, hullRight, polygon);
108+
return result;
109+
}
110+
111+
bool calculateEstimationBetweenSteps(Polygon &polygon,
112+
TrajectorySetStepVehicleLocation const &previousStepVehicleLocation,
113+
TrajectorySetStepVehicleLocation const &currentStepVehicleLocation,
114+
std::string const &debugNamespace)
115+
{
116+
// Fill potential gap between two calculation steps by using the previous and current step
117+
if (previousStepVehicleLocation == currentStepVehicleLocation)
118+
{
119+
return true;
120+
}
121+
122+
Polygon hull;
123+
MultiPoint interimPtsLeft;
124+
boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint());
125+
boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint());
126+
boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint());
127+
boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint());
128+
Polygon hullLeft;
129+
boost::geometry::convex_hull(interimPtsLeft, hullLeft);
130+
131+
MultiPoint interimPtsRight;
132+
boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint());
133+
boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint());
134+
boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint());
135+
boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint());
136+
Polygon hullRight;
137+
boost::geometry::convex_hull(interimPtsRight, hullRight);
138+
auto result = combinePolygon(hullRight, hullLeft, hull);
139+
if (!result)
140+
{
141+
spdlog::debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})."
142+
"left {}, right {}",
143+
debugNamespace,
144+
std::to_string(hullLeft),
145+
std::to_string(hullRight));
146+
}
147+
148+
if (result)
149+
{
150+
DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front");
151+
result = combinePolygon(polygon, hull, polygon);
152+
if (!result)
153+
{
154+
spdlog::warn("unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) "
155+
"with final polygon."
156+
"polygon {}, hull {}",
157+
debugNamespace,
158+
std::to_string(polygon),
159+
std::to_string(hull));
160+
}
161+
}
162+
return result;
163+
}
164+
165+
bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState,
166+
TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
167+
std::vector<TrajectorySetStep> const &sideSteps,
168+
TrajectorySetStep const &front,
169+
std::string const &debugNamespace,
170+
Polygon &resultPolygon,
171+
TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation)
172+
{
173+
auto result = true;
174+
auto previousStepVehicleLocation = initialStepVehicleLocation;
175+
//-------------
176+
// sides
177+
//-------------
178+
auto idx = 0;
179+
for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt)
180+
{
181+
Polygon stepPolygon;
182+
TrajectorySetStepVehicleLocation currentStepVehicleLocation;
183+
result = calculateStepPolygon(
184+
vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation);
185+
if (!result)
186+
{
187+
spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon");
188+
}
189+
if (result)
190+
{
191+
result = calculateEstimationBetweenSteps(resultPolygon,
192+
previousStepVehicleLocation,
193+
currentStepVehicleLocation,
194+
debugNamespace + "_step_estimation_" + std::to_string(idx));
195+
if (!result)
196+
{
197+
spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps");
198+
}
199+
previousStepVehicleLocation = currentStepVehicleLocation;
200+
}
201+
202+
if (result)
203+
{
204+
result = combinePolygon(resultPolygon, stepPolygon, resultPolygon);
205+
}
206+
idx++;
207+
}
208+
209+
//-------------
210+
// front
211+
//-------------
212+
Polygon frontPolygon;
213+
if (result)
214+
{
215+
result = calculateStepPolygon(
216+
vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation);
217+
}
218+
if (result)
219+
{
220+
result = calculateEstimationBetweenSteps(
221+
resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation");
222+
if (!result)
223+
{
224+
spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and "
225+
"front polygon.");
226+
}
227+
}
228+
if (result)
229+
{
230+
result = combinePolygon(resultPolygon, frontPolygon, resultPolygon);
231+
}
232+
return result;
233+
}
234+
60235
} // namespace unstructured
61236
} // namespace rss
62237
} // namespace ad

0 commit comments

Comments
 (0)