From 93a75a30e68e6872f241d3dbb42a0897d2c5f72f Mon Sep 17 00:00:00 2001 From: schroedtert Date: Thu, 16 Nov 2023 15:14:46 +0100 Subject: [PATCH] Add prototype for direct steering agents --- libjupedsim/include/jupedsim/jupedsim.h | 35 ++++++++- libjupedsim/src/jupedsim.cpp | 72 ++++++++++++++++++- libsimulator/src/GenericAgent.hpp | 1 + libsimulator/src/Simulation.cpp | 3 + libsimulator/src/Simulation.hpp | 1 + libsimulator/src/Stage.hpp | 33 ++++++++- libsimulator/src/StageDescription.hpp | 4 ++ libsimulator/src/StageManager.hpp | 4 ++ .../bindings_jupedsim.cpp | 44 +++++++++++- python_modules/jupedsim/jupedsim/agent.py | 8 +++ .../jupedsim/jupedsim/simulation.py | 3 + 11 files changed, 199 insertions(+), 9 deletions(-) diff --git a/libjupedsim/include/jupedsim/jupedsim.h b/libjupedsim/include/jupedsim/jupedsim.h index 76e250313f..a1d2d37b69 100644 --- a/libjupedsim/include/jupedsim/jupedsim.h +++ b/libjupedsim/include/jupedsim/jupedsim.h @@ -737,7 +737,13 @@ JPS_CollisionFreeSpeedModelState_SetRadius(JPS_CollisionFreeSpeedModelState hand /** * Identifies the type of stage */ -enum JPS_StageType { JPS_NotifiableQueueType, JPS_WaitingSetType, JPS_WaypointType, JPS_ExitType }; +enum JPS_StageType { + JPS_NotifiableQueueType, + JPS_WaitingSetType, + JPS_WaypointType, + JPS_ExitType, + JPS_DirectSteeringType +}; /** * Opaque type of an NotifiableQueueProxy @@ -784,6 +790,12 @@ JUPEDSIM_API size_t JPS_ExitProxy_GetCountTargeting(JPS_ExitProxy handle); JUPEDSIM_API void JPS_ExitProxy_Free(JPS_ExitProxy handle); +typedef struct JPS_DirectSteeringProxy_t* JPS_DirectSteeringProxy; + +JUPEDSIM_API size_t JPS_DirectSteeringProxy_GetCountTargeting(JPS_DirectSteeringProxy handle); + +JUPEDSIM_API void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle); + /** * Opaque type of an agent */ @@ -817,6 +829,16 @@ JUPEDSIM_API JPS_StageId JPS_Agent_GetStageId(JPS_Agent handle); */ JUPEDSIM_API JPS_Point JPS_Agent_GetPosition(JPS_Agent handle); +/** + * Access the agents waypoint. + * @param handle of the agent to access. + * @return waypoint + */ +JUPEDSIM_API JPS_Point JPS_Agent_GetWayPoint(JPS_Agent handle); + +JUPEDSIM_API bool +JPS_Agent_SetWayPoint(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage); + /** * Access the agents orientation. * The orientation is unit length. @@ -1091,6 +1113,12 @@ JUPEDSIM_API JPS_StageId JPS_Simulation_AddStageWaitingSet( size_t len_waiting_positions, JPS_ErrorMessage* errorMessage); +/** + * @todo + */ +JUPEDSIM_API JPS_StageId +JPS_Simulation_AddStageDirectSteering(JPS_Simulation handle, JPS_ErrorMessage* errorMessage); + /** * Adds a new agent to the simulation. * This can be called at any time, i.e. agents can be added at any iteration. @@ -1264,6 +1292,11 @@ JUPEDSIM_API JPS_ExitProxy JPS_Simulation_GetExitProxy( JPS_StageId stageId, JPS_ErrorMessage* errorMessage); +JUPEDSIM_API JPS_DirectSteeringProxy JPS_Simulation_GetDirectSteeringProxy( + JPS_Simulation handle, + JPS_StageId stageId, + JPS_ErrorMessage* errorMessage); + /** * Enable / disable collection of performance data. * @param handle of the Simulation to operate on diff --git a/libjupedsim/src/jupedsim.cpp b/libjupedsim/src/jupedsim.cpp index 5fa387f4e5..f936c01b0d 100644 --- a/libjupedsim/src/jupedsim.cpp +++ b/libjupedsim/src/jupedsim.cpp @@ -659,7 +659,22 @@ void JPS_WaitingSetProxy_Free(JPS_WaitingSetProxy handle) } //////////////////////////////////////////////////////////////////////////////////////////////////// -/// WaypointProxy +/// DirectSteeringProxy +//////////////////////////////////////////////////////////////////////////////////////////////////// +size_t JPS_DirectSteeringProxy_GetCountTargeting(JPS_DirectSteeringProxy handle) +{ + assert(handle); + auto proxy = reinterpret_cast(handle); + return proxy->CountTargeting(); +} + +void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle) +{ + delete reinterpret_cast(handle); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// WaitPointProxy //////////////////////////////////////////////////////////////////////////////////////////////////// size_t JPS_WaypointProxy_GetCountTargeting(JPS_WaypointProxy handle) { @@ -674,7 +689,7 @@ void JPS_WaypointProxy_Free(JPS_WaypointProxy handle) } //////////////////////////////////////////////////////////////////////////////////////////////////// -/// WaitingSetProxy +/// ExitProxy //////////////////////////////////////////////////////////////////////////////////////////////////// size_t JPS_ExitProxy_GetCountTargeting(JPS_ExitProxy handle) { @@ -719,6 +734,33 @@ JPS_Point JPS_Agent_GetPosition(JPS_Agent handle) return intoJPS_Point(agent->pos); } +JPS_Point JPS_Agent_GetWayPoint(JPS_Agent handle) +{ + assert(handle); + const auto agent = reinterpret_cast(handle); + return intoJPS_Point(agent->waypoint); +} + +bool JPS_Agent_SetWayPoint(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage) +{ + assert(handle); + try { + auto agent = reinterpret_cast(handle); + agent->waypoint = intoPoint(waypoint); + return true; + } catch(const std::exception& ex) { + if(errorMessage) { + *errorMessage = reinterpret_cast(new JPS_ErrorMessage_t{ex.what()}); + } + } catch(...) { + if(errorMessage) { + *errorMessage = reinterpret_cast( + new JPS_ErrorMessage_t{"Unknown internal error."}); + } + } + return false; +} + JPS_Point JPS_Agent_GetOrientation(JPS_Agent handle) { assert(handle); @@ -1089,6 +1131,12 @@ JPS_StageId JPS_Simulation_AddStageWaitingSet( return add_stage(handle, NotifiableWaitingSetDescription{positions}, errorMessage); } +JPS_StageId +JPS_Simulation_AddStageDirectSteering(JPS_Simulation handle, JPS_ErrorMessage* errorMessage) +{ + return add_stage(handle, DirectSteeringDescription{}, errorMessage); +} + JPS_AgentId JPS_Simulation_AddGeneralizedCentrifugalForceModelAgent( JPS_Simulation handle, JPS_GeneralizedCentrifugalForceModelAgentParameters parameters, @@ -1357,6 +1405,8 @@ JPS_StageType JPS_Simulation_GetStageType(JPS_Simulation handle, JPS_StageId id) return JPS_NotifiableQueueType; case 3: return JPS_ExitType; + case 4: + return JPS_DirectSteeringType; } UNREACHABLE(); }; @@ -1434,6 +1484,24 @@ JUPEDSIM_API JPS_ExitProxy JPS_Simulation_GetExitProxy( } } +JPS_DirectSteeringProxy JPS_Simulation_GetDirectSteeringProxy( + JPS_Simulation handle, + JPS_StageId stageId, + JPS_ErrorMessage* errorMessage) +{ + assert(handle); + auto simulation = reinterpret_cast(handle); + try { + return reinterpret_cast( + new DirectSteeringProxy(std::get(simulation->Stage(stageId)))); + } catch(const std::exception& ex) { + if(errorMessage) { + *errorMessage = reinterpret_cast(new JPS_ErrorMessage_t{ex.what()}); + } + return nullptr; + } +} + void JPS_Simulation_SetTracing(JPS_Simulation handle, bool status) { assert(handle); diff --git a/libsimulator/src/GenericAgent.hpp b/libsimulator/src/GenericAgent.hpp index 538b0aae18..9807d5ece1 100644 --- a/libsimulator/src/GenericAgent.hpp +++ b/libsimulator/src/GenericAgent.hpp @@ -40,6 +40,7 @@ struct GenericAgent { : id(id_ != ID::Invalid ? id_ : ID{}) , journeyId(journeyId_) , stageId(stageId_) + , waypoint(pos_) , pos(pos_) , orientation(orientation_) , model(std::move(model_)) diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp index 9161d46000..d2bce7f34b 100644 --- a/libsimulator/src/Simulation.cpp +++ b/libsimulator/src/Simulation.cpp @@ -151,6 +151,9 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) "NotifiableQueue point {} not inside walkable area", point); } } + }, + [](const DirectSteeringDescription&) -> void { + }}, stageDescription); diff --git a/libsimulator/src/Simulation.hpp b/libsimulator/src/Simulation.hpp index c616f2b614..0b6d3c2ad5 100644 --- a/libsimulator/src/Simulation.hpp +++ b/libsimulator/src/Simulation.hpp @@ -77,6 +77,7 @@ class Simulation double DT() const; void SwitchAgentJourney(GenericAgent::ID agent_id, Journey::ID journey_id, BaseStage::ID stage_id); + void ChangeAgentWaypoint(GenericAgent::ID agent_id, Point waypoint); uint64_t Iteration() const; std::vector AgentsInRange(Point p, double distance); /// Returns IDs of all agents inside the defined polygon diff --git a/libsimulator/src/Stage.hpp b/libsimulator/src/Stage.hpp index ff7f8d17b6..24e1c376e8 100644 --- a/libsimulator/src/Stage.hpp +++ b/libsimulator/src/Stage.hpp @@ -78,8 +78,20 @@ class ExitProxy : public BaseProxy ExitProxy(Simulation* simulation_, BaseStage* stage_) : BaseProxy(simulation_, stage_) {} }; -using StageProxy = - std::variant; +class DirectSteeringProxy : public BaseProxy +{ +public: + DirectSteeringProxy(Simulation* simulation_, BaseStage* stage_) : BaseProxy(simulation_, stage_) + { + } +}; + +using StageProxy = std::variant< + WaypointProxy, + NotifiableWaitingSetProxy, + NotifiableQueueProxy, + ExitProxy, + DirectSteeringProxy>; class BaseStage { @@ -96,7 +108,7 @@ class BaseStage virtual Point Target(const GenericAgent& agent) = 0; virtual StageProxy Proxy(Simulation* simulation_) = 0; ID Id() const { return id; } - size_t CountTargeting() const { return targeting; } + virtual size_t CountTargeting() const { return targeting; } void IncreaseTargeting() { targeting = targeting + 1; } void DecreaseTargeting() { @@ -248,3 +260,18 @@ void NotifiableQueue::Update(const NeighborhoodSearch& neighborhoodSearch) } } } + +class DirectSteering : public BaseStage +{ +public: + DirectSteering() = default; + ~DirectSteering() override = default; + bool IsCompleted(const GenericAgent&) override { return false; }; + Point Target(const GenericAgent& agent) override { return agent.waypoint; }; + StageProxy Proxy(Simulation* simulation) override + { + return DirectSteeringProxy(simulation, this); + }; + + size_t CountTargeting() const override { return 1; }; +}; diff --git a/libsimulator/src/StageDescription.hpp b/libsimulator/src/StageDescription.hpp index 4c2040e581..51fe20b0cb 100644 --- a/libsimulator/src/StageDescription.hpp +++ b/libsimulator/src/StageDescription.hpp @@ -8,6 +8,9 @@ #include #include +struct DirectSteeringDescription { +}; + struct WaypointDescription { Point position; double distance; @@ -26,6 +29,7 @@ struct NotifiableQueueDescription { }; using StageDescription = std::variant< + DirectSteeringDescription, WaypointDescription, ExitDescription, NotifiableWaitingSetDescription, diff --git a/libsimulator/src/StageManager.hpp b/libsimulator/src/StageManager.hpp index 219dbb6864..016fa229a1 100644 --- a/libsimulator/src/StageManager.hpp +++ b/libsimulator/src/StageManager.hpp @@ -38,6 +38,9 @@ class StageManager }, [](const NotifiableQueueDescription& d) -> std::unique_ptr { return std::make_unique(d.slots); + }, + [](const DirectSteeringDescription&) -> std::unique_ptr { + return std::make_unique(); }}, stageDescription); if(stages.find(stage->Id()) != stages.end()) { @@ -45,6 +48,7 @@ class StageManager } const auto id = stage->Id(); stages.emplace(id, std::move(stage)); + return id; } diff --git a/python_bindings_jupedsim/bindings_jupedsim.cpp b/python_bindings_jupedsim/bindings_jupedsim.cpp index 52c31d9fcc..4b421a3ea4 100644 --- a/python_bindings_jupedsim/bindings_jupedsim.cpp +++ b/python_bindings_jupedsim/bindings_jupedsim.cpp @@ -71,6 +71,7 @@ OWNED_WRAPPER(JPS_NotifiableQueueProxy); OWNED_WRAPPER(JPS_WaitingSetProxy); OWNED_WRAPPER(JPS_WaypointProxy); OWNED_WRAPPER(JPS_ExitProxy); +OWNED_WRAPPER(JPS_DirectSteeringProxy); WRAPPER(JPS_Agent); WRAPPER(JPS_GeneralizedCentrifugalForceModelState); WRAPPER(JPS_CollisionFreeSpeedModelState); @@ -384,8 +385,7 @@ PYBIND11_MODULE(py_jupedsim, m) py::arg("max_geometry_repulsion_force")) .def("build", [](JPS_GeneralizedCentrifugalForceModelBuilder_Wrapper& w) { JPS_ErrorMessage errorMsg{}; - auto result = - JPS_GeneralizedCentrifugalForceModelBuilder_Build(w.handle, &errorMsg); + auto result = JPS_GeneralizedCentrifugalForceModelBuilder_Build(w.handle, &errorMsg); if(result) { return std::make_unique(result); } @@ -662,6 +662,11 @@ PYBIND11_MODULE(py_jupedsim, m) .def("count_targeting", [](const JPS_ExitProxy_Wrapper& w) { return JPS_ExitProxy_GetCountTargeting(w.handle); }); + py::class_(m, "DirectSteeringProxy") + .def("count_targeting", [](const JPS_DirectSteeringProxy_Wrapper& w) { + return JPS_DirectSteeringProxy_GetCountTargeting(w.handle); + }); + py::class_(m, "Agent") .def_property_readonly( "id", [](const JPS_Agent_Wrapper& w) { return JPS_Agent_GetId(w.handle); }) @@ -678,6 +683,18 @@ PYBIND11_MODULE(py_jupedsim, m) [](const JPS_Agent_Wrapper& w) { return intoTuple(JPS_Agent_GetOrientation(w.handle)); }) + .def_property( + "waypoint", + [](const JPS_Agent_Wrapper& w) { return intoTuple(JPS_Agent_GetWayPoint(w.handle)); }, + [](JPS_Agent_Wrapper& w, std::tuple waypoint) { + JPS_ErrorMessage errorMsg{}; + auto success = JPS_Agent_SetWayPoint(w.handle, intoJPS_Point(waypoint), &errorMsg); + if(!success) { + auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg)); + JPS_ErrorMessage_Free(errorMsg); + throw std::runtime_error{msg}; + } + }) .def_property_readonly( "model", [](const JPS_Agent_Wrapper& w) @@ -769,6 +786,18 @@ PYBIND11_MODULE(py_jupedsim, m) JPS_ErrorMessage_Free(errorMsg); throw std::runtime_error{msg}; }) + .def( + "add_direct_steering_stage", + [](JPS_Simulation_Wrapper& w) { + JPS_ErrorMessage errorMsg{}; + const auto result = JPS_Simulation_AddStageDirectSteering(w.handle, &errorMsg); + if(result != 0) { + return result; + } + auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg)); + JPS_ErrorMessage_Free(errorMsg); + throw std::runtime_error{msg}; + }) .def( "add_journey", [](JPS_Simulation_Wrapper& simulation, JPS_JourneyDescription_Wrapper& journey) { @@ -920,7 +949,8 @@ PYBIND11_MODULE(py_jupedsim, m) std::unique_ptr, std::unique_ptr, std::unique_ptr, - std::unique_ptr> { + std::unique_ptr, + std::unique_ptr> { const auto type = JPS_Simulation_GetStageType(w.handle, id); JPS_ErrorMessage errorMessage{}; const auto raise = [](JPS_ErrorMessage err) { @@ -962,6 +992,14 @@ PYBIND11_MODULE(py_jupedsim, m) } return ptr; } + case JPS_DirectSteeringType: { + auto ptr = std::make_unique( + JPS_Simulation_GetDirectSteeringProxy(w.handle, id, &errorMessage)); + if(!ptr) { + raise(errorMessage); + } + return ptr; + } } UNREACHABLE(); }) diff --git a/python_modules/jupedsim/jupedsim/agent.py b/python_modules/jupedsim/jupedsim/agent.py index 64acc389ce..3acdb8a357 100644 --- a/python_modules/jupedsim/jupedsim/agent.py +++ b/python_modules/jupedsim/jupedsim/agent.py @@ -67,6 +67,14 @@ def orientation(self) -> tuple[float, float]: """Orientation of the agent.""" return self._obj.orientation + @property + def waypoint(self): + return self._obj.waypoint + + @waypoint.setter + def waypoint(self, waypoint: tuple[float, float]): + self._obj.waypoint = waypoint + @property def model( self, diff --git a/python_modules/jupedsim/jupedsim/simulation.py b/python_modules/jupedsim/jupedsim/simulation.py index 97f451aff9..52446a7d05 100644 --- a/python_modules/jupedsim/jupedsim/simulation.py +++ b/python_modules/jupedsim/jupedsim/simulation.py @@ -177,6 +177,9 @@ def add_exit_stage( exit_geometry = build_geometry(polygon) return self._obj.add_exit_stage(exit_geometry.boundary()) + def add_direct_steering_stage(self) -> int: + return self._obj.add_direct_steering_stage() + def add_journey(self, journey: JourneyDescription) -> int: return self._obj.add_journey(journey._obj)