From 32918b613ffc583d3ea70fb5e13c3b699c0e4f7b Mon Sep 17 00:00:00 2001 From: Ozaq Date: Fri, 12 Apr 2024 04:20:20 +0200 Subject: [PATCH] Adding distance map to Routing --- libdistance/CMakeLists.txt | 14 ++-- libdistance/src/DistanceMap.hpp | 15 ++-- libjupedsim/src/agent.cpp | 6 +- libsimulator/CMakeLists.txt | 3 + libsimulator/src/GenericAgent.hpp | 3 +- libsimulator/src/Journey.hpp | 2 +- libsimulator/src/Routing.cpp | 86 +++++++++++++++++++++ libsimulator/src/Routing.hpp | 35 +++++++++ libsimulator/src/RoutingEngine.hpp | 3 - libsimulator/src/Simulation.cpp | 48 +++++------- libsimulator/src/Simulation.hpp | 9 +-- libsimulator/src/Stage.cpp | 12 +-- libsimulator/src/Stage.hpp | 13 ++-- libsimulator/src/TacticalDecisionSystem.hpp | 6 +- libsimulator/test/TestJourney.cpp | 2 +- 15 files changed, 188 insertions(+), 69 deletions(-) diff --git a/libdistance/CMakeLists.txt b/libdistance/CMakeLists.txt index f19c8e402a..f820f8bcd0 100644 --- a/libdistance/CMakeLists.txt +++ b/libdistance/CMakeLists.txt @@ -12,13 +12,13 @@ target_link_libraries(distance INTERFACE set_property(TARGET distance PROPERTY INTERPROCEDURAL_OPTIMIZATION ${USE_IPO}) set_property(TARGET distance PROPERTY INTERPROCEDURAL_OPTIMIZATION_DEBUG OFF) -add_executable(distance_map_test - src/distance_test.cpp -) -target_link_libraries(distance_map_test - PRIVATE - distance -) +# add_executable(distance_map_test +# src/distance_test.cpp +# ) +# target_link_libraries(distance_map_test +# PRIVATE +# distance +# ) #if(BUILD_TESTS) # add_executable(libdistance-tests diff --git a/libdistance/src/DistanceMap.hpp b/libdistance/src/DistanceMap.hpp index d3154238ac..e919d007e0 100644 --- a/libdistance/src/DistanceMap.hpp +++ b/libdistance/src/DistanceMap.hpp @@ -17,6 +17,8 @@ #include #include +namespace distance +{ template concept SignedIntegral = std::is_integral_v && std::is_signed_v; @@ -565,7 +567,7 @@ void PrintDistanceMap(const Map& distance) std::cout << output << "\n"; } -void PrintBoolDistanceMap(const std::vector& visited, size_t xDim, size_t yDim) +static void PrintBoolDistanceMap(const std::vector& visited, size_t xDim, size_t yDim) { using I = typename std::make_signed_t; // Get the corresponding signed type @@ -978,7 +980,7 @@ class DistanceMapBuilder void AddArc(const Arc& arc) { arcs.emplace_back(arc); } void AddCircle(const Circle& circle) { circles.emplace_back(circle); } void AddExitLine(const Line& exitLine) { exitLines.emplace_back(exitLine); } - void AddExitPolygon(const Polygon exitPolygon); + void AddExitPolygon(const Polygon exitPolygon){}; // TODO(TS): needed for Simulex? // void AddBoundingBox(const ) @@ -1006,13 +1008,13 @@ class DistanceMapBuilder // PrintDistanceMap(distance, xDim, yDim); PrepareDistanceMap(distance, xMin, yMin); -// std::cout << "Prepared: \n"; -// PrintDistanceMap(distance); + // std::cout << "Prepared: \n"; + // PrintDistanceMap(distance); ComputeDistanceMap(distance, localDistance); -// std::cout << "Final: \n"; -// PrintDistanceMap(distance); + // std::cout << "Final: \n"; + // PrintDistanceMap(distance); // auto bytes = DumpDistanceMap(distance); // std::fstream out("dump.data", std::ios::trunc | std::ios::binary | @@ -1042,3 +1044,4 @@ class DistanceMapBuilder // CELL_SIZE); } }; +} // namespace distance diff --git a/libjupedsim/src/agent.cpp b/libjupedsim/src/agent.cpp index d09fc003fa..083d387550 100644 --- a/libjupedsim/src/agent.cpp +++ b/libjupedsim/src/agent.cpp @@ -10,6 +10,7 @@ #include #include +#include using jupedsim::detail::intoJPS_Point; using jupedsim::detail::intoPoint; @@ -49,7 +50,10 @@ JPS_Point JPS_Agent_GetTarget(JPS_Agent handle) { assert(handle); const auto agent = reinterpret_cast(handle); - return intoJPS_Point(agent->target); + if(auto val = std::get_if(&agent->target); val != nullptr) { + return intoJPS_Point(*val); + } + return {}; } bool JPS_Agent_SetTarget(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage) diff --git a/libsimulator/CMakeLists.txt b/libsimulator/CMakeLists.txt index 8c05a34d0f..65637ab41c 100644 --- a/libsimulator/CMakeLists.txt +++ b/libsimulator/CMakeLists.txt @@ -54,6 +54,8 @@ add_library(simulator STATIC src/Point.hpp src/Polygon.cpp src/Polygon.hpp + src/Routing.cpp + src/Routing.hpp src/RoutingEngine.cpp src/RoutingEngine.hpp src/Simulation.cpp @@ -90,6 +92,7 @@ target_compile_definitions(simulator PUBLIC ) target_link_libraries(simulator PUBLIC common + distance fmt::fmt CGAL::CGAL build_info diff --git a/libsimulator/src/GenericAgent.hpp b/libsimulator/src/GenericAgent.hpp index c53f38aa00..f87d24e414 100644 --- a/libsimulator/src/GenericAgent.hpp +++ b/libsimulator/src/GenericAgent.hpp @@ -23,7 +23,8 @@ struct GenericAgent { // This is evaluated by the "operational level" Point destination{}; - Point target{}; + // TODO(kkratz): Type needs to be BaseStage::Location + std::variant> target{}; // Agent fields common for all models Point pos{}; diff --git a/libsimulator/src/Journey.hpp b/libsimulator/src/Journey.hpp index 8c51a99caa..5dc376dec4 100644 --- a/libsimulator/src/Journey.hpp +++ b/libsimulator/src/Journey.hpp @@ -179,7 +179,7 @@ class Journey ID Id() const { return id; } - std::tuple Target(const GenericAgent& agent) const + std::tuple Target(const GenericAgent& agent) const { auto& node = stages.at(agent.stageId); auto stage = node.stage; diff --git a/libsimulator/src/Routing.cpp b/libsimulator/src/Routing.cpp index e69de29bb2..baf0eb14e3 100644 --- a/libsimulator/src/Routing.cpp +++ b/libsimulator/src/Routing.cpp @@ -0,0 +1,86 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#include "Routing.hpp" + +#include "CfgCgal.hpp" +#include "Unreachable.hpp" + +#include + +distance::Point cvt_pt(const Poly::Point_2& p) +{ + return distance::Point{CGAL::to_double(p.x()), CGAL::to_double(p.y())}; +}; + +distance::Polygon cvt_poly(const Poly& cgalContainer) +{ + distance::Polygon poly; + poly.points.reserve(cgalContainer.size()); + std::transform( + std::begin(cgalContainer), + std::end(cgalContainer), + std::back_inserter(poly.points), + cvt_pt); + return poly; +}; + +Routing::Routing(std::unique_ptr geometry) + : _geometry(std::move(geometry)) + , _routingEngine(std::make_unique(_geometry->Polygon())) +{ +} + +void Routing::AddDistanceMapForStage( + const BaseStage::ID id, + const StageDescription stageDescription) +{ + std::visit( + overloaded{ + [this, id](const WaypointDescription& d) -> void { + auto builder = PrepareDistanceMapBuilder(); + builder->AddExitLine({{d.position.x, d.position.y}, {d.position.x, d.position.y}}); + _distanceMaps.emplace(id, builder->Build()); + // HACK(kkratz): + // distance::DumpDistanceMapMatplotlibCSV(*_distanceMaps[id]); + }, + [this, id](const ExitDescription& d) -> void { + auto builder = PrepareDistanceMapBuilder(); + const Poly& p = d.polygon; + builder->AddExitPolygon(cvt_poly(p)); + _distanceMaps.emplace(id, builder->Build()); + // HACK(kkratz): + // distance::DumpDistanceMapMatplotlibCSV(*_distanceMaps[id]); + }, + [](const NotifiableWaitingSetDescription&) -> void {}, + [](const NotifiableQueueDescription&) -> void {}, + [](const DirectSteeringDescription&) -> void {}}, + stageDescription); +} + +std::unique_ptr Routing::PrepareDistanceMapBuilder() const +{ + auto builder = std::make_unique(); + + const auto poly = _geometry->Polygon(); + builder->AddPolygon(cvt_poly(poly.outer_boundary())); + for(const auto& hole : poly.holes()) { + builder->AddPolygon(cvt_poly(hole)); + } + + return builder; +} + +Point Routing::ComputeWaypoint(Point currentPosition, BaseStage::Location destination) const +{ + return std::visit( + overloaded{ + [this, currentPosition](const Point& destination) { + return _routingEngine->ComputeWaypoint(currentPosition, destination); + }, + [this, currentPosition](const BaseStage::ID id) { + const auto val = + _distanceMaps.at(id)->GetNextTarget({currentPosition.x, currentPosition.y}); + return Point{val.x, val.y}; + }}, + destination); +} diff --git a/libsimulator/src/Routing.hpp b/libsimulator/src/Routing.hpp index e69de29bb2..94a1118c17 100644 --- a/libsimulator/src/Routing.hpp +++ b/libsimulator/src/Routing.hpp @@ -0,0 +1,35 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#pragma once + +#include "DistanceMap.hpp" +#include "RoutingEngine.hpp" +#include "Stage.hpp" +#include "StageDescription.hpp" + +class Routing +{ + using DMap = distance::DistanceMap; + using DMapBuilder = distance::DistanceMapBuilder; + using DPoly = distance::Polygon; + using DPoint = decltype(DPoly::points)::value_type; + std::unique_ptr _geometry; + std::unique_ptr _routingEngine; + std::unordered_map> _distanceMaps{}; + +public: + explicit Routing(std::unique_ptr geometry); + ~Routing() = default; + Routing(const Routing& other) = delete; + Routing& operator=(const Routing& other) = delete; + Routing(Routing&& other) = default; + Routing& operator=(Routing&& other) = default; + void AddDistanceMapForStage(const BaseStage::ID id, const StageDescription stageDescription); + + const CollisionGeometry& Geometry() const { return *_geometry; } + bool InsideGeometry(const Point& p) const { return _geometry->InsideGeometry(p); } + Point ComputeWaypoint(Point currentPosition, BaseStage::Location destination) const; + +private: + std::unique_ptr PrepareDistanceMapBuilder() const; +}; diff --git a/libsimulator/src/RoutingEngine.hpp b/libsimulator/src/RoutingEngine.hpp index a615e335e7..7464c6732b 100644 --- a/libsimulator/src/RoutingEngine.hpp +++ b/libsimulator/src/RoutingEngine.hpp @@ -14,9 +14,6 @@ #include #include -using LocationID = size_t; -using Location = std::variant; - class RoutingEngine : public Clonable { CDT cdt{}; diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp index 37fb4d8858..52489dc532 100644 --- a/libsimulator/src/Simulation.cpp +++ b/libsimulator/src/Simulation.cpp @@ -18,16 +18,12 @@ Simulation::Simulation( double dT) : _clock(dT), _operationalDecisionSystem(std::move(operationalModel)) { - const auto p = geometry->Polygon(); - const auto& [tup, res] = geometries.emplace( - std::piecewise_construct, - std::forward_as_tuple(geometry->Id()), - std::forward_as_tuple(std::move(geometry), std::make_unique(p))); + const auto& [val, res] = + geometries.emplace(geometry->Id(), std::make_unique(std::move(geometry))); if(!res) { throw SimulationError("Internal error"); } - _geometry = std::get<0>(tup->second).get(); - _routingEngine = std::get<1>(tup->second).get(); + _routing = val->second.get(); } const SimulationClock& Simulation::Clock() const { @@ -51,13 +47,13 @@ void Simulation::Iterate() _agentRemovalSystem.Run(_agents, _removedAgentsInLastIteration, _stageManager); _neighborhoodSearch.Update(_agents); - _stageSystem.Run(_stageManager, _neighborhoodSearch, *_geometry); + _stageSystem.Run(_stageManager, _neighborhoodSearch, _routing->Geometry()); _stategicalDecisionSystem.Run(_journeys, _agents, _stageManager); - _tacticalDecisionSystem.Run(*_routingEngine, _agents); + _tacticalDecisionSystem.Run(*_routing, _agents); { auto t2 = _perfStats.TraceOperationalDecisionSystemRun(); _operationalDecisionSystem.Run( - _clock.dT(), _clock.ElapsedTime(), _neighborhoodSearch, *_geometry, _agents); + _clock.dT(), _clock.ElapsedTime(), _neighborhoodSearch, _routing->Geometry(), _agents); } _clock.Advance(); } @@ -142,18 +138,18 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) std::visit( overloaded{ [this](const WaypointDescription& d) -> void { - if(!this->_geometry->InsideGeometry(d.position)) { + if(!this->_routing->InsideGeometry(d.position)) { throw SimulationError("WayPoint {} not inside walkable area", d.position); } }, [this](const ExitDescription& d) -> void { - if(!this->_geometry->InsideGeometry(d.polygon.Centroid())) { + if(!this->_routing->InsideGeometry(d.polygon.Centroid())) { throw SimulationError("Exit {} not inside walkable area", d.polygon.Centroid()); } }, [this](const NotifiableWaitingSetDescription& d) -> void { for(const auto& point : d.slots) { - if(!this->_geometry->InsideGeometry(point)) { + if(!this->_routing->InsideGeometry(point)) { throw SimulationError( "NotifiableWaitingSet point {} not inside walkable area", point); } @@ -161,7 +157,7 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) }, [this](const NotifiableQueueDescription& d) -> void { for(const auto& point : d.slots) { - if(!this->_geometry->InsideGeometry(point)) { + if(!this->_routing->InsideGeometry(point)) { throw SimulationError( "NotifiableQueue point {} not inside walkable area", point); } @@ -172,17 +168,19 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) }}, stageDescription); - return _stageManager.AddStage(stageDescription, _removedAgentsInLastIteration); + const auto id = _stageManager.AddStage(stageDescription, _removedAgentsInLastIteration); + + return id; } GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent) { - if(!_geometry->InsideGeometry(agent.pos)) { + if(!_routing->InsideGeometry(agent.pos)) { throw SimulationError("Agent {} not inside walkable area", agent.pos); } agent.orientation = agent.orientation.Normalized(); - _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch, *_geometry); + _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch, _routing->Geometry()); if(_journeys.count(agent.journeyId) == 0) { throw SimulationError("Unknown journey id: {}", agent.journeyId); @@ -197,7 +195,7 @@ GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent) auto v = IteratorPair(std::prev(std::end(_agents)), std::end(_agents)); _stategicalDecisionSystem.Run(_journeys, v, _stageManager); - _tacticalDecisionSystem.Run(*_routingEngine, v); + _tacticalDecisionSystem.Run(*_routing, v); return _agents.back().id.getID(); } @@ -326,26 +324,22 @@ StageProxy Simulation::Stage(BaseStage::ID stageId) } CollisionGeometry Simulation::Geo() const { - return *_geometry; + return _routing->Geometry(); } void Simulation::SwitchGeometry(std::unique_ptr&& geometry) { ValidateGeometry(geometry); if(const auto& iter = geometries.find(geometry->Id()); iter != std::end(geometries)) { - _geometry = std::get<0>(iter->second).get(); - _routingEngine = std::get<1>(iter->second).get(); + _routing = iter->second.get(); } else { const auto p = geometry->Polygon(); - const auto& [tup, res] = geometries.emplace( - std::piecewise_construct, - std::forward_as_tuple(geometry->Id()), - std::forward_as_tuple(std::move(geometry), std::make_unique(p))); + const auto& [val, res] = + geometries.emplace(geometry->Id(), std::make_unique(std::move(geometry))); if(!res) { throw SimulationError("Internal error"); } - _geometry = std::get<0>(tup->second).get(); - _routingEngine = std::get<1>(tup->second).get(); + _routing = val->second.get(); } } diff --git a/libsimulator/src/Simulation.hpp b/libsimulator/src/Simulation.hpp index f9c37c5785..7f3dd6b76f 100644 --- a/libsimulator/src/Simulation.hpp +++ b/libsimulator/src/Simulation.hpp @@ -10,6 +10,7 @@ #include "OperationalModel.hpp" #include "OperationalModelType.hpp" #include "Point.hpp" +#include "Routing.hpp" #include "SimulationClock.hpp" #include "Stage.hpp" #include "StageDescription.hpp" @@ -35,12 +36,8 @@ class Simulation StageManager _stageManager{}; StageSystem _stageSystem{}; NeighborhoodSearch _neighborhoodSearch{2.2}; - std::unordered_map< - CollisionGeometry::ID, - std::tuple, std::unique_ptr>> - geometries{}; - RoutingEngine* _routingEngine; - CollisionGeometry* _geometry; + std::unordered_map> geometries{}; + Routing* _routing; std::vector _agents; std::vector _removedAgentsInLastIteration; std::unordered_map> _journeys; diff --git a/libsimulator/src/Stage.cpp b/libsimulator/src/Stage.cpp index 5a05532613..80545ed2cf 100644 --- a/libsimulator/src/Stage.cpp +++ b/libsimulator/src/Stage.cpp @@ -91,9 +91,9 @@ bool Waypoint::IsCompleted(const GenericAgent& agent) return actual_distance <= distance; } -Point Waypoint::Target(const GenericAgent&) +BaseStage::Location Waypoint::Target(const GenericAgent&) { - return position; + return id; } StageProxy Waypoint::Proxy(Simulation* simulation) @@ -121,9 +121,9 @@ bool Exit::IsCompleted(const GenericAgent& agent) return hasReachedExit; } -Point Exit::Target(const GenericAgent&) +BaseStage::Location Exit::Target(const GenericAgent&) { - return area.Centroid(); + return id; } StageProxy Exit::Proxy(Simulation* simulation) @@ -152,7 +152,7 @@ bool NotifiableWaitingSet::IsCompleted(const GenericAgent& agent) return distance <= 1; } -Point NotifiableWaitingSet::Target(const GenericAgent& agent) +BaseStage::Location NotifiableWaitingSet::Target(const GenericAgent& agent) { if(state == WaitingSetState::Inactive) { return slots[0]; @@ -211,7 +211,7 @@ bool NotifiableQueue::IsCompleted(const GenericAgent& agent) return completed; } -Point NotifiableQueue::Target(const GenericAgent& agent) +BaseStage::Location NotifiableQueue::Target(const GenericAgent& agent) { if(const auto index_opt = IndexInContainer(occupants, agent.id); index_opt) { diff --git a/libsimulator/src/Stage.hpp b/libsimulator/src/Stage.hpp index 233a4d7aba..eb340d73d0 100644 --- a/libsimulator/src/Stage.hpp +++ b/libsimulator/src/Stage.hpp @@ -99,6 +99,7 @@ class BaseStage { public: using ID = jps::UniqueID; + using Location = std::variant; protected: ID id; @@ -107,7 +108,7 @@ class BaseStage public: virtual ~BaseStage() = default; virtual bool IsCompleted(const GenericAgent& agent) = 0; - virtual Point Target(const GenericAgent& agent) = 0; + virtual Location Target(const GenericAgent& agent) = 0; virtual StageProxy Proxy(Simulation* simulation_) = 0; ID Id() const { return id; } size_t CountTargeting() const { return targeting; } @@ -140,7 +141,7 @@ class Waypoint : public BaseStage Waypoint(Point position_, double distance_); ~Waypoint() override = default; bool IsCompleted(const GenericAgent& agent) override; - Point Target(const GenericAgent& agent) override; + Location Target(const GenericAgent& agent) override; StageProxy Proxy(Simulation* simulation_) override; Point Position() const { return position; }; }; @@ -155,7 +156,7 @@ class Exit : public BaseStage Exit(Polygon area, std::vector& toRemove_); ~Exit() override = default; bool IsCompleted(const GenericAgent& agent) override; - Point Target(const GenericAgent& agent) override; + Location Target(const GenericAgent& agent) override; StageProxy Proxy(Simulation* simulation_) override; Polygon Position() const { return area; }; }; @@ -170,7 +171,7 @@ class NotifiableWaitingSet : public BaseStage NotifiableWaitingSet(std::vector slots_); ~NotifiableWaitingSet() override = default; bool IsCompleted(const GenericAgent& agent) override; - Point Target(const GenericAgent& agent) override; + Location Target(const GenericAgent& agent) override; StageProxy Proxy(Simulation* simulation_) override; void State(WaitingSetState s); WaitingSetState State() const; @@ -250,7 +251,7 @@ class NotifiableQueue : public BaseStage NotifiableQueue(std::vector slots_); ~NotifiableQueue() override = default; bool IsCompleted(const GenericAgent& agent) override; - Point Target(const GenericAgent& agent) override; + Location Target(const GenericAgent& agent) override; StageProxy Proxy(Simulation* simulation_) override; template void Update(const NeighborhoodSearch& neighborhoodSearch, const CollisionGeometry& geometry); @@ -319,7 +320,7 @@ class DirectSteering : public BaseStage DirectSteering() = default; ~DirectSteering() override = default; bool IsCompleted(const GenericAgent&) override { return false; }; - Point Target(const GenericAgent& agent) override { return agent.target; }; + Location Target(const GenericAgent& agent) override { return agent.target; }; StageProxy Proxy(Simulation* simulation) override { return DirectSteeringProxy(simulation, this); diff --git a/libsimulator/src/TacticalDecisionSystem.hpp b/libsimulator/src/TacticalDecisionSystem.hpp index a71224ed1a..acb408dc6f 100644 --- a/libsimulator/src/TacticalDecisionSystem.hpp +++ b/libsimulator/src/TacticalDecisionSystem.hpp @@ -2,9 +2,7 @@ // SPDX-License-Identifier: LGPL-3.0-or-later #pragma once -#include "RoutingEngine.hpp" - -#include +#include "Routing.hpp" class TacticalDecisionSystem { @@ -16,7 +14,7 @@ class TacticalDecisionSystem TacticalDecisionSystem(TacticalDecisionSystem&& other) = delete; TacticalDecisionSystem& operator=(TacticalDecisionSystem&& other) = delete; - void Run(RoutingEngine& routingEngine, auto&& agents) const + void Run(const Routing& routingEngine, auto&& agents) const { for(auto& agent : agents) { const auto dest = agent.target; diff --git a/libsimulator/test/TestJourney.cpp b/libsimulator/test/TestJourney.cpp index a7d928289b..cb66f6a2b7 100644 --- a/libsimulator/test/TestJourney.cpp +++ b/libsimulator/test/TestJourney.cpp @@ -83,7 +83,7 @@ TEST(LeastTargetedTransition, NextIsCorrect) } MOCK_METHOD(size_t, CountTargeting, (), (const)); MOCK_METHOD(bool, IsCompleted, (const GenericAgent& agent), (override)); - MOCK_METHOD(Point, Target, (const GenericAgent& agent), (override)); + MOCK_METHOD(BaseStage::Location, Target, (const GenericAgent& agent), (override)); MOCK_METHOD(StageProxy, Proxy, (Simulation * simulation_), (override)); void SetTargeting(size_t targeting_) { targeting = targeting_; } };