Skip to content

Commit 2be99ad

Browse files
GrufoonyCopilot
Grufoony
and
Copilot
authored
Enhance right of way management for Intersection class (#298)
* This is the easy implementation * Maybe this is better... still lacks directions * Add turnDirection * Take right of way into account * Includes * Update src/dsm/headers/Road.hpp Co-authored-by: Copilot <[email protected]> * Small fix * Add test * Copilot suggestions --------- Co-authored-by: Copilot <[email protected]>
1 parent 9eb43b4 commit 2be99ad

File tree

6 files changed

+152
-21
lines changed

6 files changed

+152
-21
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 = 21;
9+
static constexpr uint8_t DSM_VERSION_PATCH = 22;
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: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,16 @@ namespace dsm {
7979
/// @details The forbidden turns are the road ids that are not allowed to be used by the agents
8080
/// when they are on the road.
8181
std::set<Id> const& forbiddenTurns() const;
82+
/// @brief Get the road's turn direction given the previous road angle
83+
/// @param previousStreetAngle The angle of the previous road
84+
/// @return Direction The turn direction
85+
/// @details The turn direction is the direction that the agent must take when it is on the road.
86+
/// The possible values are:
87+
/// - UTURN (abs of delta is greater than pi)
88+
/// - STRAIGHT (abs of delta is less than pi /8)
89+
/// - RIGHT (delta is negative and not covered by the above conditions)
90+
/// - LEFT (delta is positive and not covered by the above conditions)
91+
Direction turnDirection(double const& previousStreetAngle) const;
8292

8393
virtual int nAgents() const = 0;
8494
virtual int nMovingAgents() const = 0;

‎src/dsm/headers/RoadDynamics.hpp‎

Lines changed: 105 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -638,6 +638,99 @@ namespace dsm {
638638
Logger::debug(std::format("Green light on street {} and direction {}",
639639
pStreet->id(),
640640
directionToString.at(direction)));
641+
} else if (destinationNode->isIntersection() &&
642+
pAgentTemp->nextStreetId().has_value()) {
643+
auto& intersection = static_cast<Intersection&>(*destinationNode);
644+
bool bCanPass{true};
645+
auto const& thisDirection{this->graph()
646+
.edge(pAgentTemp->nextStreetId().value())
647+
->turnDirection(pStreet->angle())};
648+
if (!intersection.streetPriorities().contains(pStreet->id())) {
649+
// I have to check if the agent has right of way
650+
auto const& inNeighbours{
651+
this->graph().adjacencyMatrix().getCol(destinationNode->id())};
652+
for (auto const& sourceId : inNeighbours) {
653+
auto const& streetId{sourceId * this->graph().nNodes() +
654+
destinationNode->id()};
655+
if (streetId == pStreet->id()) {
656+
continue;
657+
}
658+
auto const& pStreetTemp{this->graph().edge(streetId)};
659+
if (pStreetTemp->nExitingAgents() == 0) {
660+
continue;
661+
}
662+
if (intersection.streetPriorities().contains(streetId)) {
663+
Logger::debug(std::format(
664+
"Skipping agent emission from street {} -> {} due to right of way.",
665+
pStreet->source(),
666+
pStreet->target()));
667+
bCanPass = false;
668+
break;
669+
} else if (thisDirection >= Direction::LEFT) {
670+
// Check if the agent has right of way using direction
671+
// The problem arises only when you have to turn left
672+
for (auto i{0}; i < pStreetTemp->nLanes(); ++i) {
673+
// check queue is not empty and take the top agent
674+
if (pStreetTemp->queue(i).empty()) {
675+
continue;
676+
}
677+
auto const& pAgentTemp2{pStreetTemp->queue(i).front()};
678+
if (!pAgentTemp2->streetId().has_value()) {
679+
continue;
680+
}
681+
auto const& otherDirection{
682+
this->graph()
683+
.edge(pAgentTemp2->nextStreetId().value())
684+
->turnDirection(this->graph()
685+
.edge(pAgentTemp2->streetId().value())
686+
->angle())};
687+
if (otherDirection < Direction::LEFT) {
688+
Logger::debug(std::format(
689+
"Skipping agent emission from street {} -> {} due to right of "
690+
"way with other agents.",
691+
pStreet->source(),
692+
pStreet->target()));
693+
bCanPass = false;
694+
break;
695+
}
696+
}
697+
}
698+
}
699+
} else if (thisDirection >= Direction::LEFT) {
700+
for (auto const& streetId : intersection.streetPriorities()) {
701+
if (streetId == pStreet->id()) {
702+
continue;
703+
}
704+
auto const& pStreetTemp{this->graph().edge(streetId)};
705+
for (auto i{0}; i < pStreetTemp->nLanes(); ++i) {
706+
// check queue is not empty and take the top agent
707+
if (pStreetTemp->queue(i).empty()) {
708+
continue;
709+
}
710+
auto const& pAgentTemp2{pStreetTemp->queue(i).front()};
711+
if (!pAgentTemp2->streetId().has_value()) {
712+
continue;
713+
}
714+
auto const& otherDirection{
715+
this->graph()
716+
.edge(pAgentTemp2->nextStreetId().value())
717+
->turnDirection(
718+
this->graph().edge(pAgentTemp2->streetId().value())->angle())};
719+
if (otherDirection < thisDirection) {
720+
Logger::debug(std::format(
721+
"Skipping agent emission from street {} -> {} due to right of "
722+
"way with other agents.",
723+
pStreet->source(),
724+
pStreet->target()));
725+
bCanPass = false;
726+
break;
727+
}
728+
}
729+
}
730+
}
731+
if (!bCanPass) {
732+
continue;
733+
}
641734
}
642735
bool bArrived{false};
643736
if (!(uniformDist(this->m_generator) < m_passageProbability.value_or(1.1))) {
@@ -1055,10 +1148,10 @@ namespace dsm {
10551148
tbb::blocked_range<size_t>(0, numNodes, grainSize),
10561149
[&](const tbb::blocked_range<size_t>& range) {
10571150
for (size_t i = range.begin(); i != range.end(); ++i) {
1058-
const auto& pNode = nodes.at(i);
1151+
auto const& pNode = nodes.at(i);
10591152
for (auto const& sourceId :
10601153
this->graph().adjacencyMatrix().getCol(pNode->id())) {
1061-
auto const streetId = sourceId * N + pNode->id();
1154+
auto const streetId{sourceId * N + pNode->id()};
10621155
auto const& pStreet{this->graph().edge(streetId)};
10631156
if (bUpdateData && pNode->isTrafficLight()) {
10641157
if (!m_queuesAtTrafficLights.contains(streetId)) {
@@ -1107,10 +1200,16 @@ namespace dsm {
11071200
pStreet->enqueue(0);
11081201
continue;
11091202
}
1110-
auto const deltaAngle{pNextStreet->deltaAngle(pStreet->angle())};
1111-
if (std::abs(deltaAngle) < std::numbers::pi) {
1112-
// Lanes are counted as 0 is the far right lane
1113-
if (std::abs(deltaAngle) < std::numbers::pi / 8) {
1203+
auto const direction{pNextStreet->turnDirection(pStreet->angle())};
1204+
switch (direction) {
1205+
case Direction::UTURN:
1206+
case Direction::LEFT:
1207+
pStreet->enqueue(nLanes - 1);
1208+
break;
1209+
case Direction::RIGHT:
1210+
pStreet->enqueue(0);
1211+
break;
1212+
default:
11141213
std::vector<double> weights;
11151214
for (auto const& queue : pStreet->exitQueues()) {
11161215
weights.push_back(1. / (queue.size() + 1));
@@ -1134,13 +1233,6 @@ namespace dsm {
11341233
std::discrete_distribution<size_t> laneDist{weights.begin(),
11351234
weights.end()};
11361235
pStreet->enqueue(laneDist(this->m_generator));
1137-
} else if (deltaAngle < 0.) { // Right
1138-
pStreet->enqueue(0); // Always the first lane
1139-
} else { // Left (deltaAngle > 0.)
1140-
pStreet->enqueue(nLanes - 1); // Always the last lane
1141-
}
1142-
} else { // U turn
1143-
pStreet->enqueue(nLanes - 1); // Always the last lane
11441236
}
11451237
}
11461238
}

‎src/dsm/sources/Road.cpp‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include <cassert>
55
#include <cmath>
66
#include <format>
7+
#include <numbers>
78
#include <stdexcept>
89

910
namespace dsm {
@@ -65,6 +66,19 @@ namespace dsm {
6566
assert(priority >= 0);
6667
m_priority = priority;
6768
}
69+
Direction Road::turnDirection(double const& previousStreetAngle) const {
70+
auto const deltaAngle{this->deltaAngle(previousStreetAngle)};
71+
if (std::abs(deltaAngle) >= std::numbers::pi) {
72+
return Direction::UTURN;
73+
}
74+
if (std::abs(deltaAngle) < std::numbers::pi / 8) {
75+
return Direction::STRAIGHT;
76+
}
77+
if (deltaAngle < 0.) {
78+
return Direction::RIGHT;
79+
}
80+
return Direction::LEFT;
81+
}
6882

6983
double Road::length() const { return m_length; }
7084
double Road::maxSpeed() const { return m_maxSpeed; }

‎src/dsm/sources/RoadNetwork.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,7 @@ namespace dsm {
9191
intersection.setStreetPriorities(newStreetPriorities);
9292
}
9393
if (node->isTrafficLight()) {
94-
auto& trafficLight = dynamic_cast<TrafficLight&>(*node);
94+
auto& trafficLight = static_cast<TrafficLight&>(*node);
9595
std::unordered_map<Id, std::unordered_map<Direction, TrafficLightCycle>> newCycles;
9696
for (auto const& [streetId, cycles] : trafficLight.cycles()) {
9797
newCycles.emplace(newStreetIds.at(streetId), std::move(cycles));
@@ -106,7 +106,7 @@ namespace dsm {
106106
if (!pNode->isTrafficLight()) {
107107
continue;
108108
}
109-
auto& tl = dynamic_cast<TrafficLight&>(*pNode);
109+
auto& tl = static_cast<TrafficLight&>(*pNode);
110110
if (!tl.streetPriorities().empty() || !tl.cycles().empty()) {
111111
continue;
112112
}
@@ -369,7 +369,7 @@ namespace dsm {
369369
// If allowedTurns contains all RIGHT, STRAIGHT and LEFT, transform RIGHT into RIGHTANDSTRAIGHT
370370
if (allowedTurns.size() > static_cast<size_t>(nLanes)) {
371371
if (pair.second->isTrafficLight()) {
372-
auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
372+
auto& tl = static_cast<TrafficLight&>(*pair.second);
373373
auto const& cycles{tl.cycles()};
374374
if (cycles.contains(pInStreet->id())) {
375375
if (cycles.size() == static_cast<size_t>(nLanes)) {
@@ -429,7 +429,7 @@ namespace dsm {
429429
allowedTurns.contains(Direction::RIGHT) &&
430430
allowedTurns.contains(Direction::LEFT)) {
431431
if (pair.second->isTrafficLight()) {
432-
auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
432+
auto& tl = static_cast<TrafficLight&>(*pair.second);
433433
auto const& cycles{tl.cycles()};
434434
if (cycles.contains(pInStreet->id())) {
435435
auto const& cycle{cycles.at(pInStreet->id())};

‎test/Test_dynamics.cpp‎

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -989,7 +989,7 @@ TEST_CASE("FirstOrderDynamics") {
989989
CHECK_EQ(pStreet->queue(0).size(), 3);
990990
CHECK_EQ(dynamics.streetMeanSpeed(1), 0.);
991991
}
992-
SUBCASE("Intersection priorities") {
992+
SUBCASE("Intersection right of way") {
993993
GIVEN("A dynamics object with five nodes and eight streets") {
994994
RoadNetwork graph2;
995995
graph2.addNode<Intersection>(0, std::make_pair(0, 0));
@@ -1025,7 +1025,7 @@ TEST_CASE("FirstOrderDynamics") {
10251025
dynamics.evolve(false);
10261026
THEN("The agent in A passes first") {
10271027
CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 1);
1028-
CHECK_EQ(nodeO.agents().size(), 2);
1028+
CHECK_EQ(nodeO.agents().size(), 1);
10291029
CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 20);
10301030
}
10311031
dynamics.evolve(false);
@@ -1050,7 +1050,7 @@ TEST_CASE("FirstOrderDynamics") {
10501050
dynamics.evolve(false);
10511051
THEN("The agent in D passes first") {
10521052
CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
1053-
CHECK_EQ(nodeO.agents().size(), 2);
1053+
CHECK_EQ(nodeO.agents().size(), 1);
10541054
CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 15);
10551055
}
10561056
dynamics.evolve(false);
@@ -1065,6 +1065,21 @@ TEST_CASE("FirstOrderDynamics") {
10651065
CHECK(nodeO.agents().empty());
10661066
}
10671067
}
1068+
WHEN("We set street priorities and add agents") {
1069+
nodeO.addStreetPriority(5);
1070+
nodeO.addStreetPriority(16);
1071+
dynamics.addAgent(2, 1);
1072+
dynamics.addAgent(2, 4);
1073+
dynamics.evolve(false);
1074+
dynamics.evolve(false);
1075+
dynamics.evolve(false);
1076+
dynamics.evolve(false);
1077+
THEN("The agent in A passes first because it's on the main road") {
1078+
CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 1);
1079+
CHECK_EQ(nodeO.agents().size(), 1);
1080+
CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 20);
1081+
}
1082+
}
10681083
}
10691084
}
10701085
SUBCASE("meanSpireFlow") {

0 commit comments

Comments
 (0)