Skip to content

Commit 1d3c95a

Browse files
GrufoonyGrufoony
authored
Add forbiddenTurns attribute to Road class (#291)
* Fix angle assignment in automapstreetlanes * Fix * Angles * Adjust angle threshold * Some fixes * Add `forbiddenTurns` attribute to Street class * Handle forbidden turns into automapstreetlanes * Move forbidden turns to Road class
1 parent e986a43 commit 1d3c95a

File tree

6 files changed

+132
-43
lines changed

6 files changed

+132
-43
lines changed

‎src/dsm/dsm.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
static constexpr uint8_t DSM_VERSION_MAJOR = 2;
88
static constexpr uint8_t DSM_VERSION_MINOR = 6;
9-
static constexpr uint8_t DSM_VERSION_PATCH = 11;
9+
static constexpr uint8_t DSM_VERSION_PATCH = 12;
1010

1111
static auto const DSM_VERSION =
1212
std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH);

‎src/dsm/headers/Road.hpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
#include <memory>
66
#include <optional>
7+
#include <set>
78
#include <string>
89

910
namespace dsm {
@@ -15,6 +16,7 @@ namespace dsm {
1516
int m_nLanes;
1617
std::string m_name;
1718
int m_priority;
19+
std::set<Id> m_forbiddenTurns; // Stores the forbidden turns (road ids)
1820

1921
public:
2022
/// @brief Construct a new Road object
@@ -50,6 +52,12 @@ namespace dsm {
5052
/// @brief Set the road's priority
5153
/// @param priority The road's priority
5254
void setPriority(int priority);
55+
/// @brief Add a road id to the forbidden turns
56+
/// @param roadId The road id to add
57+
void addForbiddenTurn(Id roadId);
58+
/// @brief Replace the road's forbidden turns with the given set
59+
/// @param forbiddenTurns The set of forbidden turns
60+
void setForbiddenTurns(std::set<Id> const& forbiddenTurns);
5361

5462
/// @brief Get the length, in meters
5563
/// @return double The length, in meters
@@ -66,6 +74,11 @@ namespace dsm {
6674
/// @brief Get the priority
6775
/// @return int The priority
6876
int priority() const;
77+
/// @brief Get the road's forbidden turns
78+
/// @return std::set<Id> The road's forbidden turns
79+
/// @details The forbidden turns are the road ids that are not allowed to be used by the agents
80+
/// when they are on the road.
81+
std::set<Id> const& forbiddenTurns() const;
6982

7083
virtual int nAgents() const = 0;
7184
virtual int nMovingAgents() const = 0;

‎src/dsm/headers/RoadDynamics.hpp‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -362,8 +362,8 @@ namespace dsm {
362362
pair.second->deltaAngle(this->graph().edge(previousStreetId)->angle())};
363363
if (std::abs(delta) < std::numbers::pi) {
364364
if (delta < 0.) {
365-
m_turnMapping[pair.first][dsm::Direction::RIGHT] = previousStreetId;
366-
; // right
365+
m_turnMapping[pair.first][dsm::Direction::RIGHT] =
366+
previousStreetId; // right
367367
} else if (delta > 0.) {
368368
m_turnMapping[pair.first][dsm::Direction::LEFT] =
369369
previousStreetId; // left
@@ -502,6 +502,18 @@ namespace dsm {
502502
}
503503
}
504504
assert(!possibleMoves.empty());
505+
if (streetId.has_value()) {
506+
auto const& pStreet{this->graph().edge(*streetId)};
507+
for (auto const& foirbiddenStreetId : pStreet->forbiddenTurns()) {
508+
auto const& pForbiddenStreet{this->graph().edge(foirbiddenStreetId)};
509+
// if possible moves contains the forbidden street, remove it
510+
auto it = std::find(
511+
possibleMoves.begin(), possibleMoves.end(), pForbiddenStreet->target());
512+
if (it != possibleMoves.end()) {
513+
possibleMoves.erase(it);
514+
}
515+
}
516+
}
505517
std::uniform_int_distribution<Size> moveDist{
506518
0, static_cast<Size>(possibleMoves.size() - 1)};
507519
// while loop to avoid U turns in non-roundabout junctions
@@ -1049,7 +1061,7 @@ namespace dsm {
10491061
auto const deltaAngle{pNextStreet->deltaAngle(pStreet->angle())};
10501062
if (std::abs(deltaAngle) < std::numbers::pi) {
10511063
// Lanes are counted as 0 is the far right lane
1052-
if (std::abs(deltaAngle) < std::numbers::pi / 4) {
1064+
if (std::abs(deltaAngle) < std::numbers::pi / 8) {
10531065
std::vector<double> weights;
10541066
for (auto const& queue : pStreet->exitQueues()) {
10551067
weights.push_back(1. / (queue.size() + 1));

‎src/dsm/headers/Street.hpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,6 @@ namespace dsm {
169169
/// @tparam Id The type of the street's id
170170
/// @tparam Size The type of the street's capacity
171171
class SpireStreet : public Street, public Counter {
172-
private:
173172
public:
174173
using Street::Street;
175174
SpireStreet(Street&& street) : Street(std::move(street)) {}

‎src/dsm/sources/Road.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,11 @@ namespace dsm {
4949
}
5050
double Road::meanVehicleLength() { return m_meanVehicleLength; }
5151

52+
void Road::addForbiddenTurn(Id roadId) { m_forbiddenTurns.insert(roadId); }
53+
void Road::setForbiddenTurns(std::set<Id> const& forbiddenTurns) {
54+
m_forbiddenTurns = forbiddenTurns;
55+
}
56+
5257
void Road::setMaxSpeed(double speed) {
5358
if (speed < 0.) {
5459
Logger::error(
@@ -66,4 +71,5 @@ namespace dsm {
6671
int Road::nLanes() const { return m_nLanes; }
6772
std::string Road::name() const { return m_name; }
6873
int Road::priority() const { return m_priority; }
74+
std::set<Id> const& Road::forbiddenTurns() const { return m_forbiddenTurns; }
6975
}; // namespace dsm

‎src/dsm/sources/RoadNetwork.cpp‎

Lines changed: 97 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,19 @@ namespace dsm {
6666
}
6767
newStreetIds.emplace(streetId, newStreetId);
6868
}
69+
std::for_each(
70+
m_edges.cbegin(), m_edges.cend(), [this, &newStreetIds](auto const& pair) {
71+
auto const& pStreet{pair.second};
72+
auto const& forbiddenTurns{pStreet->forbiddenTurns()};
73+
if (forbiddenTurns.empty()) {
74+
return;
75+
}
76+
std::set<Id> newForbiddenTurns;
77+
for (auto const& streetId : forbiddenTurns) {
78+
newForbiddenTurns.insert(newStreetIds.at(streetId));
79+
}
80+
pStreet->setForbiddenTurns(newForbiddenTurns);
81+
});
6982
for (const auto& [nodeId, node] : m_nodes) {
7083
// This is probably not the best way to do this
7184
if (node->isIntersection()) {
@@ -267,7 +280,6 @@ namespace dsm {
267280
if (nLanes == 1) {
268281
return;
269282
}
270-
// auto const& laneMapping{pInStreet->laneMapping()};
271283
std::multiset<Direction> allowedTurns;
272284
std::for_each(
273285
outNeighbours.cbegin(),
@@ -276,14 +288,17 @@ namespace dsm {
276288
auto const& outNodeId) {
277289
auto const& pOutStreet{
278290
m_edges.at(pair.first * m_nodes.size() + outNodeId)};
279-
if (pOutStreet->target() == pInStreet->source()) {
291+
if (pOutStreet->target() == pInStreet->source() ||
292+
pInStreet->forbiddenTurns().contains(pOutStreet->id())) {
280293
return;
281294
}
282295
auto const deltaAngle{pOutStreet->deltaAngle(pInStreet->angle())};
283296
auto const& outOppositeStreet{this->street(pair.first, outNodeId)};
297+
if (!outOppositeStreet) {
298+
return;
299+
}
284300
// Actually going straight means remain on the same road, thus...
285-
if (outOppositeStreet &&
286-
((pInStreet->priority() == maxPriority) ==
301+
if (((pInStreet->priority() == maxPriority) ==
287302
(outOppositeStreet->get()->priority() == maxPriority)) &&
288303
!allowedTurns.contains(Direction::STRAIGHT)) {
289304
Logger::debug(
@@ -298,43 +313,29 @@ namespace dsm {
298313
// allowedTurns.emplace(Direction::STRAIGHT);
299314
// return;
300315
// }
301-
} else if (std::abs(deltaAngle) < 5 * std::numbers::pi / 6) {
316+
} else if (std::abs(deltaAngle) < std::numbers::pi) {
302317
// Logger::debug(std::format("Angle in {} - angle out {}",
303318
// pInStreet->angle(),
304319
// pOutStreet->angle()));
305320
// Logger::debug(std::format("Delta: {}", deltaAngle));
306-
if (deltaAngle < -std::numbers::pi / 6.) {
307-
Logger::debug(
308-
std::format("Street {} can turn RIGHT", pInStreet->id()));
321+
if (std::abs(deltaAngle) < std::numbers::pi / 8) {
322+
Logger::debug(std::format("Street {} -> {} can turn STRAIGHT",
323+
pInStreet->source(),
324+
pInStreet->target()));
325+
allowedTurns.emplace(Direction::STRAIGHT);
326+
} else if (deltaAngle < 0.) {
327+
Logger::debug(std::format("Street {} -> {} can turn RIGHT",
328+
pInStreet->source(),
329+
pInStreet->target()));
309330
allowedTurns.emplace(Direction::RIGHT);
310-
} else if (deltaAngle > std::numbers::pi / 6.) {
311-
Logger::debug(
312-
std::format("Street {} can turn LEFT", pInStreet->id()));
331+
} else if (deltaAngle > 0.) {
332+
Logger::debug(std::format("Street {} -> {} can turn LEFT",
333+
pInStreet->source(),
334+
pInStreet->target()));
313335
allowedTurns.emplace(Direction::LEFT);
314-
} else {
315-
Logger::debug(
316-
std::format("Street {} can go STRAIGHT", pInStreet->id()));
317-
if (!allowedTurns.contains(Direction::STRAIGHT)) {
318-
allowedTurns.emplace(Direction::STRAIGHT);
319-
} else if (deltaAngle > 0.) {
320-
allowedTurns.emplace(Direction::LEFT);
321-
} else {
322-
allowedTurns.emplace(Direction::RIGHT);
323-
}
324336
}
325337
}
326338
});
327-
// if (!allowedTurns.contains(Direction::RIGHT)) {
328-
// std::multiset<Direction> updatedTurns;
329-
// for (auto turn : allowedTurns) {
330-
// if (turn > Direction::RIGHT && turn <= Direction::UTURN) {
331-
// updatedTurns.insert(static_cast<Direction>(turn - 1));
332-
// } else {
333-
// updatedTurns.insert(turn);
334-
// }
335-
// }
336-
// allowedTurns = std::move(updatedTurns); // Replace with the updated set
337-
// }
338339
while (allowedTurns.size() < static_cast<size_t>(nLanes)) {
339340
if (allowedTurns.contains(Direction::STRAIGHT)) {
340341
allowedTurns.emplace(Direction::STRAIGHT);
@@ -347,11 +348,36 @@ namespace dsm {
347348
}
348349
}
349350
// If allowedTurns contains all RIGHT, STRAIGHT and LEFT, transform RIGHT into RIGHTANDSTRAIGHT
350-
if (allowedTurns.contains(Direction::STRAIGHT) &&
351-
allowedTurns.contains(Direction::RIGHT) &&
352-
allowedTurns.contains(Direction::LEFT)) {
353-
allowedTurns.erase(Direction::RIGHT);
354-
allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
351+
if (allowedTurns.size() > static_cast<size_t>(nLanes)) {
352+
if (pair.second->isTrafficLight()) {
353+
auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
354+
auto const& cycles{tl.cycles()};
355+
if (cycles.contains(pInStreet->id())) {
356+
if (cycles.size() == static_cast<size_t>(nLanes)) {
357+
// Replace with the traffic light cycles
358+
Logger::debug(
359+
std::format("Using traffic light {} cycles for street {} -> {}",
360+
tl.id(),
361+
pInStreet->source(),
362+
pInStreet->target()));
363+
auto const& cycle{cycles.at(pInStreet->id())};
364+
allowedTurns.clear();
365+
for (auto const& [direction, cycle] : cycle) {
366+
allowedTurns.emplace(direction);
367+
}
368+
} else if (cycles.at(pInStreet->id())
369+
.contains(Direction::LEFTANDSTRAIGHT)) {
370+
allowedTurns.erase(Direction::LEFT);
371+
allowedTurns.erase(Direction::STRAIGHT);
372+
allowedTurns.emplace(Direction::LEFTANDSTRAIGHT);
373+
} else if (cycles.at(pInStreet->id())
374+
.contains(Direction::RIGHTANDSTRAIGHT)) {
375+
allowedTurns.erase(Direction::RIGHT);
376+
allowedTurns.erase(Direction::STRAIGHT);
377+
allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
378+
}
379+
}
380+
}
355381
}
356382
switch (nLanes) {
357383
case 1:
@@ -361,11 +387,44 @@ namespace dsm {
361387
if (allowedTurns.contains(Direction::STRAIGHT) &&
362388
allowedTurns.contains(Direction::RIGHT) &&
363389
allowedTurns.contains(Direction::LEFT)) {
364-
break;
390+
if (pair.second->isTrafficLight()) {
391+
auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
392+
auto const& cycles{tl.cycles()};
393+
if (cycles.contains(pInStreet->id())) {
394+
auto const& cycle{cycles.at(pInStreet->id())};
395+
if (cycle.contains(Direction::LEFTANDSTRAIGHT) &&
396+
cycle.contains(Direction::RIGHT)) {
397+
allowedTurns.erase(Direction::LEFT);
398+
allowedTurns.erase(Direction::STRAIGHT);
399+
allowedTurns.emplace(Direction::LEFTANDSTRAIGHT);
400+
break;
401+
}
402+
}
403+
}
404+
allowedTurns.clear();
405+
allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
406+
allowedTurns.emplace(Direction::LEFT);
407+
}
408+
if (allowedTurns.size() > 2) {
409+
// Remove duplicates
410+
std::set<Direction> uniqueDirections;
411+
std::copy(allowedTurns.begin(),
412+
allowedTurns.end(),
413+
std::inserter(uniqueDirections, uniqueDirections.begin()));
414+
allowedTurns.clear();
415+
std::copy(uniqueDirections.begin(),
416+
uniqueDirections.end(),
417+
std::inserter(allowedTurns, allowedTurns.begin()));
365418
}
366419
[[fallthrough]];
367420
default:
368421
assert(allowedTurns.size() == static_cast<size_t>(nLanes));
422+
// Logger::info(
423+
// std::format("Street {}->{} with {} lanes and {} allowed turns",
424+
// pInStreet->source(),
425+
// pInStreet->target(),
426+
// nLanes,
427+
// allowedTurns.size()));
369428
std::vector<Direction> newMapping(nLanes);
370429
auto it{allowedTurns.cbegin()};
371430
for (size_t i{0}; i < allowedTurns.size(); ++i, ++it) {

0 commit comments

Comments
 (0)