Skip to content

Commit 3d6d7a0

Browse files
fpaschfpasch
authored
Fix unstructured vehicle curvature (#86)
* Update unstructured constellation polygon creation Fix vehicle curvature calculation after response time, make DebugDrawing accessible from python * Update version to 4.3.0 * Cleanup Change-Id: I2b185f59b8545d150e8049534d2eeb0dbdc447b5 * Cleanup unit tests Change-Id: I910c03f1d0aaa259a5c1d45529b9927386e0b8ca
1 parent 4fa723b commit 3d6d7a0

29 files changed

+181
-777
lines changed

‎ad_rss/generated/CMakeLists.txt

+1-1
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.2.0)
17+
project(ad_rss VERSION 4.3.0)
1818

1919
include(GNUInstallDirs)
2020
include(CMakePackageConfigHelpers)

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

-10
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,6 @@ struct UnstructuredSettings
108108
&& (vehicleTrajectoryCalculationStep == other.vehicleTrajectoryCalculationStep)
109109
&& (vehicleFrontIntermediateRatioSteps == other.vehicleFrontIntermediateRatioSteps)
110110
&& (vehicleBackIntermediateRatioSteps == other.vehicleBackIntermediateRatioSteps)
111-
&& (vehicleResponseTimeIntermediateAccelerationSteps == other.vehicleResponseTimeIntermediateAccelerationSteps)
112111
&& (vehicleBrakeIntermediateAccelerationSteps == other.vehicleBrakeIntermediateAccelerationSteps)
113112
&& (vehicleContinueForwardIntermediateAccelerationSteps
114113
== other.vehicleContinueForwardIntermediateAccelerationSteps);
@@ -175,12 +174,6 @@ struct UnstructuredSettings
175174
*/
176175
uint32_t vehicleBackIntermediateRatioSteps{0};
177176

178-
/*!
179-
* Specifies the intermediate acceleration steps (between brakeMax and accelMax) used while calculating the states at
180-
* response time. These are later used for calculating the trajectory sets.
181-
*/
182-
uint32_t vehicleResponseTimeIntermediateAccelerationSteps{0};
183-
184177
/*!
185178
* Specifies the intermediate acceleration steps (between brakeMax and brakeMin) used while calculating the cbrake
186179
* trajectory set. This is applied to all vehicleResponseIntermediateAccelerationSteps, therefore it has only an
@@ -251,9 +244,6 @@ inline std::ostream &operator<<(std::ostream &os, UnstructuredSettings const &_v
251244
os << "vehicleBackIntermediateRatioSteps:";
252245
os << _value.vehicleBackIntermediateRatioSteps;
253246
os << ",";
254-
os << "vehicleResponseTimeIntermediateAccelerationSteps:";
255-
os << _value.vehicleResponseTimeIntermediateAccelerationSteps;
256-
os << ",";
257247
os << "vehicleBrakeIntermediateAccelerationSteps:";
258248
os << _value.vehicleBrakeIntermediateAccelerationSteps;
259249
os << ",";

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

+2-2
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 2
28+
#define AD_RSS_VERSION_MINOR 3
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.2.0"
38+
#define AD_RSS_VERSION_STRING "4.3.0"

‎ad_rss/impl/include/ad/rss/unstructured/DebugDrawing.hpp

+67-4
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818

1919
#define DEBUG_DRAWING_POLYGON(polygon, color, ns) (DebugDrawing::getInstance()->drawPolygon(polygon, color, ns))
2020
#define DEBUG_DRAWING_LINE(line, color, ns) (DebugDrawing::getInstance()->drawLine(line, color, ns))
21+
#define DEBUG_DRAWING_IS_ENABLED() (DebugDrawing::getInstance()->isEnabled())
2122

2223
/*!
2324
* @brief namespace ad
@@ -35,6 +36,21 @@ namespace unstructured {
3536
class DebugDrawing
3637
{
3738
public:
39+
struct NullDeleter
40+
{
41+
void operator()(const void *)
42+
{
43+
}
44+
};
45+
struct DebugPoint
46+
{
47+
DebugPoint(double inX, double inY)
48+
: x(inX)
49+
, y(inY){};
50+
double x;
51+
double y;
52+
};
53+
3854
struct DebugLine
3955
{
4056
DebugLine(Line const &iLine, std::string const &iColor, std::string const &iNs)
@@ -46,6 +62,16 @@ class DebugDrawing
4662
Line line;
4763
std::string color{"white"};
4864
std::string ns;
65+
66+
std::vector<DebugPoint> getVector()
67+
{
68+
std::vector<DebugPoint> vectorLine;
69+
for (auto const &pt : line)
70+
{
71+
vectorLine.push_back(DebugPoint(pt.x(), pt.y()));
72+
}
73+
return vectorLine;
74+
}
4975
};
5076

5177
struct DebugPolygon
@@ -59,28 +85,58 @@ class DebugDrawing
5985
Polygon polygon;
6086
std::string color{"white"};
6187
std::string ns;
88+
89+
std::vector<DebugPoint> getVector()
90+
{
91+
std::vector<DebugPoint> vectorPolygon;
92+
for (auto const &pt : polygon.outer())
93+
{
94+
vectorPolygon.push_back(DebugPoint(pt.x(), pt.y()));
95+
}
96+
return vectorPolygon;
97+
}
6298
};
6399

64100
explicit DebugDrawing() = default;
65101

66102
/**
67103
* @brief singelton instance getter
68104
*/
69-
static DebugDrawing *getInstance()
105+
static std::shared_ptr<DebugDrawing> getInstance()
70106
{
71107
static DebugDrawing *mSingleton = new DebugDrawing();
72-
return mSingleton;
108+
return std::shared_ptr<DebugDrawing>(mSingleton, NullDeleter());
73109
}
74110

75111
/**
76112
* @brief clean up all geometries to draw (before a new cycle)
77113
*/
78114
void reset()
79115
{
116+
if (!mEnabled)
117+
{
118+
return;
119+
}
80120
mLines.clear();
81121
mPolygons.clear();
82122
}
83123

124+
/**
125+
* @brief enable/disable debug drawing
126+
*/
127+
void enable(bool value)
128+
{
129+
mEnabled = value;
130+
}
131+
132+
/**
133+
* @brief enable/disable debug drawing
134+
*/
135+
bool isEnabled()
136+
{
137+
return mEnabled;
138+
}
139+
84140
/**
85141
* @brief draw a line
86142
*
@@ -90,6 +146,10 @@ class DebugDrawing
90146
*/
91147
void drawLine(Line const &line, std::string const &color = "white", std::string const &ns = "")
92148
{
149+
if (!mEnabled)
150+
{
151+
return;
152+
}
93153
mLines.push_back(DebugLine(line, color, ns));
94154

95155
spdlog::trace("DRAW;{};{};{};LINE", ns, color, std::to_string(line));
@@ -104,13 +164,16 @@ class DebugDrawing
104164
*/
105165
void drawPolygon(Polygon const &polygon, std::string const &color = "white", std::string const &ns = "")
106166
{
167+
if (!mEnabled)
168+
{
169+
return;
170+
}
107171
mPolygons.push_back(DebugPolygon(polygon, color, ns));
108-
109-
spdlog::trace("DRAW;{};{};{};POLYGON", ns, color, std::to_string(polygon));
110172
}
111173

112174
std::vector<DebugLine> mLines;
113175
std::vector<DebugPolygon> mPolygons;
176+
bool mEnabled{false};
114177
};
115178

116179
} // namespace unstructured

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

-6
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,6 @@
1717
#include "ad/rss/situation/VehicleState.hpp"
1818
#include "ad/rss/unstructured/Geometry.hpp"
1919

20-
#define DEBUG_DRAWING 0
21-
22-
#if DEBUG_DRAWING
23-
#include "ad/rss/unstructured/DebugDrawing.hpp"
24-
#endif
25-
2620
/*!
2721
* @brief namespace ad
2822
*/

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

+27-42
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
#include "TrajectoryPedestrian.hpp"
1313
#include <ad/physics/Operation.hpp>
1414
#include "ad/rss/situation/Physics.hpp"
15+
#include "ad/rss/unstructured/DebugDrawing.hpp"
1516

1617
/*!
1718
* @brief namespace ad
@@ -52,10 +53,8 @@ bool TrajectoryPedestrian::calculateTrajectorySets(situation::VehicleState const
5253
result = calculateTrajectorySetsMoving(vehicleState, timeToStop, brakePolygon, continueForwardPolygon);
5354
}
5455
}
55-
#if DEBUG_DRAWING
56-
DEBUG_DRAWING_POLYGON(brakePolygon, "red", "brake_pedestrian");
57-
DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "continueForward_pedestrian");
58-
#endif
56+
DEBUG_DRAWING_POLYGON(brakePolygon, "red", "pedestrian_brake");
57+
DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "pedestrian_continueForward");
5958
return result;
6059
}
6160

@@ -170,11 +169,8 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
170169
//-------------
171170
if (result)
172171
{
173-
result = calculateStepPolygon(vehicleState,
174-
minBrakeDistance,
175-
responseTimeFrontSide,
176-
"", //"brake_front",
177-
brakePolygon);
172+
result = calculateStepPolygon(
173+
vehicleState, minBrakeDistance, responseTimeFrontSide, "pedestrian_brake_front", brakePolygon);
178174
if (!result)
179175
{
180176
spdlog::debug("TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate brake max step polygon.");
@@ -216,9 +212,7 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
216212
boost::geometry::append(
217213
back, getVehicleCorner(finalLeftMaxBrakeDistance, vehicleState.objectState.dimension, VehicleCorner::frontLeft));
218214
boost::geometry::convex_hull(back, maxBrakePolygon);
219-
#if DEBUG_DRAWING
220-
DEBUG_DRAWING_POLYGON(maxBrakePolygon, "red", "brake_back");
221-
#endif
215+
DEBUG_DRAWING_POLYGON(maxBrakePolygon, "red", "pedestrian_brake_back");
222216
combinePolygon(maxBrakePolygon, brakePolygon, brakePolygon);
223217
}
224218

@@ -230,8 +224,11 @@ bool TrajectoryPedestrian::calculateTrajectorySetsMoving(situation::VehicleState
230224
//-------------
231225
if (result)
232226
{
233-
result = calculateStepPolygon(
234-
vehicleState, accelMaxDistance, responseTimeFrontSide, "continueForward_front", continueForwardPolygon);
227+
result = calculateStepPolygon(vehicleState,
228+
accelMaxDistance,
229+
responseTimeFrontSide,
230+
"pedestrian_continueForward_front",
231+
continueForwardPolygon);
235232
if (!result)
236233
{
237234
spdlog::debug(
@@ -399,21 +396,18 @@ bool TrajectoryPedestrian::calculateStepPolygon(situation::VehicleState const &v
399396
auto finalCenterFrontRight
400397
= getVehicleCorner(finalStep.center, vehicleState.objectState.dimension, VehicleCorner::frontRight);
401398

402-
//----
403-
// left
404-
//----
405-
#if DEBUG_DRAWING
399+
//----
400+
// left
401+
//----
406402
int idx = 0;
407-
#endif
408403
for (auto it = finalStep.left.begin(); (it != finalStep.left.end()) && result; ++it)
409404
{
410-
#if DEBUG_DRAWING
411-
auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
412-
DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_left_" + std::to_string(idx));
405+
if (DEBUG_DRAWING_IS_ENABLED())
406+
{
407+
auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
408+
DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_left_" + std::to_string(idx));
409+
}
413410
++idx;
414-
#else
415-
(void)debugNamespace;
416-
#endif
417411
boost::geometry::append(ptsLeft,
418412
getVehicleCorner(*it, vehicleState.objectState.dimension, VehicleCorner::frontRight));
419413
boost::geometry::append(ptsLeft,
@@ -422,21 +416,18 @@ bool TrajectoryPedestrian::calculateStepPolygon(situation::VehicleState const &v
422416
boost::geometry::append(ptsLeft, finalCenterFrontLeft);
423417
boost::geometry::append(ptsLeft, finalCenterFrontRight);
424418

425-
//-----
426-
// right
427-
//-----
428-
#if DEBUG_DRAWING
419+
//-----
420+
// right
421+
//-----
429422
idx = 0;
430-
#endif
431423
for (auto it = finalStep.right.begin(); (it != finalStep.right.end()) && result; ++it)
432424
{
433-
#if DEBUG_DRAWING
434-
auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
435-
DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_right_" + std::to_string(idx));
425+
if (DEBUG_DRAWING_IS_ENABLED())
426+
{
427+
auto vehicleLocation = TrafficParticipantLocation(*it, vehicleState);
428+
DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", debugNamespace + "_right_" + std::to_string(idx));
429+
}
436430
++idx;
437-
#else
438-
(void)debugNamespace;
439-
#endif
440431
boost::geometry::append(ptsRight,
441432
getVehicleCorner(*it, vehicleState.objectState.dimension, VehicleCorner::frontLeft));
442433
boost::geometry::append(ptsRight,
@@ -529,10 +520,8 @@ bool TrajectoryPedestrian::getResponseTimeTrajectoryPoint(situation::VehicleStat
529520
speed,
530521
maxDistance);
531522

532-
#if DEBUG_DRAWING
533523
Line linePts;
534524
boost::geometry::append(linePts, startingPoint);
535-
#endif
536525

537526
TrajectoryPoint currentPoint(vehicleState);
538527

@@ -564,16 +553,12 @@ bool TrajectoryPedestrian::getResponseTimeTrajectoryPoint(situation::VehicleStat
564553
pointAfterResponseTime
565554
= startingPoint + toPoint(-std::sin(startingAngle) * maxDistance, std::cos(startingAngle) * maxDistance);
566555
}
567-
#if DEBUG_DRAWING
568556
boost::geometry::append(linePts, pointAfterResponseTime);
569-
#endif
570557

571558
resultTrajectoryPoint = TrajectoryPoint(
572559
pointAfterResponseTime, vehicleState.objectState.yaw + angleChange, speed, physics::AngularVelocity(0.));
573-
#if DEBUG_DRAWING
574560
DEBUG_DRAWING_LINE(
575561
linePts, "orange", std::to_string(aUntilResponseTime) + "_" + std::to_string(angleChangeRatio) + "_trajectory");
576-
#endif
577562
return result;
578563
}
579564

0 commit comments

Comments
 (0)