diff --git a/libjupedsim/include/jupedsim/jupedsim.h b/libjupedsim/include/jupedsim/jupedsim.h index 76e250313f..66a7f18c75 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,10 @@ 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 void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle); + /** * Opaque type of an agent */ @@ -817,6 +827,16 @@ JUPEDSIM_API JPS_StageId JPS_Agent_GetStageId(JPS_Agent handle); */ JUPEDSIM_API JPS_Point JPS_Agent_GetPosition(JPS_Agent handle); +/** + * Access the agents current target. + * @param handle of the agent to access. + * @return target of the agent + */ +JUPEDSIM_API JPS_Point JPS_Agent_GetTarget(JPS_Agent handle); + +JUPEDSIM_API bool +JPS_Agent_SetTarget(JPS_Agent handle, JPS_Point target, JPS_ErrorMessage* errorMessage); + /** * Access the agents orientation. * The orientation is unit length. @@ -1091,6 +1111,20 @@ JUPEDSIM_API JPS_StageId JPS_Simulation_AddStageWaitingSet( size_t len_waiting_positions, JPS_ErrorMessage* errorMessage); +/** + * Adds a new direct steering stage to the simulation. + * + * This allows a direct control of the target of an agent. + * + * Important: A direct steering stage can only be used if it is the only stage in a Journey. + * + * @param handle to the simulation to act on + * @param[out] errorMessage if not NULL: will be set to a JPS_ErrorMessage in case of an error. + * @return Id of the stage + */ +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 +1298,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..822726bc77 100644 --- a/libjupedsim/src/jupedsim.cpp +++ b/libjupedsim/src/jupedsim.cpp @@ -659,7 +659,15 @@ void JPS_WaitingSetProxy_Free(JPS_WaitingSetProxy handle) } //////////////////////////////////////////////////////////////////////////////////////////////////// -/// WaypointProxy +/// DirectSteeringProxy +//////////////////////////////////////////////////////////////////////////////////////////////////// +void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle) +{ + delete reinterpret_cast(handle); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// WaitPointProxy //////////////////////////////////////////////////////////////////////////////////////////////////// size_t JPS_WaypointProxy_GetCountTargeting(JPS_WaypointProxy handle) { @@ -674,7 +682,7 @@ void JPS_WaypointProxy_Free(JPS_WaypointProxy handle) } //////////////////////////////////////////////////////////////////////////////////////////////////// -/// WaitingSetProxy +/// ExitProxy //////////////////////////////////////////////////////////////////////////////////////////////////// size_t JPS_ExitProxy_GetCountTargeting(JPS_ExitProxy handle) { @@ -719,6 +727,33 @@ JPS_Point JPS_Agent_GetPosition(JPS_Agent handle) return intoJPS_Point(agent->pos); } +JPS_Point JPS_Agent_GetTarget(JPS_Agent handle) +{ + assert(handle); + const auto agent = reinterpret_cast(handle); + return intoJPS_Point(agent->target); +} + +bool JPS_Agent_SetTarget(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage) +{ + assert(handle); + try { + auto agent = reinterpret_cast(handle); + agent->target = 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 +1124,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 +1398,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 +1477,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..687459fedc 100644 --- a/libsimulator/src/GenericAgent.hpp +++ b/libsimulator/src/GenericAgent.hpp @@ -21,7 +21,7 @@ struct GenericAgent { // This is evaluated by the "operational level" Point destination{}; - Point waypoint{}; + Point target{}; // Agent fields common for all models Point pos{}; @@ -40,6 +40,7 @@ struct GenericAgent { : id(id_ != ID::Invalid ? id_ : ID{}) , journeyId(journeyId_) , stageId(stageId_) + , target(pos_) , pos(pos_) , orientation(orientation_) , model(std::move(model_)) @@ -64,7 +65,7 @@ struct fmt::formatter { agent.journeyId, agent.stageId, agent.destination, - agent.waypoint, + agent.target, agent.pos, agent.orientation, m); @@ -78,7 +79,7 @@ struct fmt::formatter { agent.journeyId, agent.stageId, agent.destination, - agent.waypoint, + agent.target, agent.pos, agent.orientation, m); diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp index 9161d46000..8b0b0eb9b7 100644 --- a/libsimulator/src/Simulation.cpp +++ b/libsimulator/src/Simulation.cpp @@ -10,6 +10,7 @@ #include "Stage.hpp" #include "Visitor.hpp" #include +#include Simulation::Simulation( std::unique_ptr&& operationalModel, @@ -60,6 +61,15 @@ void Simulation::Iterate() Journey::ID Simulation::AddJourney(const std::map& stages) { std::map nodes; + bool containsDirectSteering = + std::find_if(std::begin(stages), std::end(stages), [this](auto const& pair) { + return std::holds_alternative(Stage(pair.first)); + }) != std::end(stages); + + if(containsDirectSteering && stages.size() > 1) { + throw SimulationError( + "Journeys containing a DirectSteeringStage, may only contain this stage."); + } std::transform( std::begin(stages), @@ -116,6 +126,7 @@ Journey::ID Simulation::AddJourney(const std::map(std::move(nodes)); const auto id = journey->Id(); _journeys.emplace(id, std::move(journey)); @@ -151,6 +162,9 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) "NotifiableQueue point {} not inside walkable area", point); } } + }, + [](const DirectSteeringDescription&) -> void { + }}, stageDescription); diff --git a/libsimulator/src/Stage.hpp b/libsimulator/src/Stage.hpp index ff7f8d17b6..8c5218c23f 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 { @@ -248,3 +260,16 @@ 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.target; }; + StageProxy Proxy(Simulation* simulation) override + { + return DirectSteeringProxy(simulation, this); + }; +}; 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/libsimulator/src/StrategicalDesicionSystem.hpp b/libsimulator/src/StrategicalDesicionSystem.hpp index 127db9fb44..c231256940 100644 --- a/libsimulator/src/StrategicalDesicionSystem.hpp +++ b/libsimulator/src/StrategicalDesicionSystem.hpp @@ -28,7 +28,7 @@ class StrategicalDecisionSystem { for(auto& agent : agents) { const auto [target, id] = journeys.at(agent.journeyId)->Target(agent); - agent.waypoint = target; + agent.target = target; stageManager.MigrateAgent(agent.stageId, id); agent.stageId = id; } diff --git a/libsimulator/src/TacticalDecisionSystem.hpp b/libsimulator/src/TacticalDecisionSystem.hpp index 9a7bd25881..c926559f2a 100644 --- a/libsimulator/src/TacticalDecisionSystem.hpp +++ b/libsimulator/src/TacticalDecisionSystem.hpp @@ -19,7 +19,7 @@ class TacticalDecisionSystem void Run(RoutingEngine& routingEngine, auto&& agents) const { for(auto& agent : agents) { - const auto dest = agent.waypoint; + const auto dest = agent.target; agent.destination = routingEngine.ComputeWaypoint(agent.pos, dest); } } diff --git a/python_bindings_jupedsim/bindings_jupedsim.cpp b/python_bindings_jupedsim/bindings_jupedsim.cpp index 52c31d9fcc..48ddf1ed77 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,8 @@ PYBIND11_MODULE(py_jupedsim, m) .def("count_targeting", [](const JPS_ExitProxy_Wrapper& w) { return JPS_ExitProxy_GetCountTargeting(w.handle); }); + py::class_(m, "DirectSteeringProxy"); + py::class_(m, "Agent") .def_property_readonly( "id", [](const JPS_Agent_Wrapper& w) { return JPS_Agent_GetId(w.handle); }) @@ -678,6 +680,18 @@ PYBIND11_MODULE(py_jupedsim, m) [](const JPS_Agent_Wrapper& w) { return intoTuple(JPS_Agent_GetOrientation(w.handle)); }) + .def_property( + "target", + [](const JPS_Agent_Wrapper& w) { return intoTuple(JPS_Agent_GetTarget(w.handle)); }, + [](JPS_Agent_Wrapper& w, std::tuple target) { + JPS_ErrorMessage errorMsg{}; + auto success = JPS_Agent_SetTarget(w.handle, intoJPS_Point(target), &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 +783,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 +946,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 +989,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..9102b17bc7 100644 --- a/python_modules/jupedsim/jupedsim/agent.py +++ b/python_modules/jupedsim/jupedsim/agent.py @@ -30,6 +30,7 @@ class Agent: sim.agents_in_polygon(polygon) .. note :: + You need to be aware that currently there are no checks done when setting properties on an Agent instance. For example it is possible to set an Agent position outside the walkable area of the Simulation resulting in a crash. @@ -67,6 +68,33 @@ def orientation(self) -> tuple[float, float]: """Orientation of the agent.""" return self._obj.orientation + @property + def target(self) -> tuple[float, float]: + """Current target of the agent. + + Can be used to directly steer an agent towards the given coordinate. + This will bypass the strategical and tactical level, but the operational level + will still be active. + + .. important:: + + If the agent is not in a Journey with a DirectSteering stage, any change will be + ignored. + + .. important:: + + When setting the target, the given coordinates must lie within the walkable area. + Otherwise, an error will be thrown at the next iteration call. + + Returns: + Current target of the agent. + """ + return self._obj.target + + @target.setter + def target(self, target: tuple[float, float]): + self._obj.target = target + @property def model( self, diff --git a/python_modules/jupedsim/jupedsim/simulation.py b/python_modules/jupedsim/jupedsim/simulation.py index 97f451aff9..7b92bdd367 100644 --- a/python_modules/jupedsim/jupedsim/simulation.py +++ b/python_modules/jupedsim/jupedsim/simulation.py @@ -177,6 +177,22 @@ 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: + """Add an direct steering stage to the simulation. + + This stage allows a direct control of the target the agent is walking to. + Thus, it will bypass the tactical and stragecial level of the simulation, but the + operational level will still be active. + + .. important:: + + A direct steering stage can only be used if it is the only stage in a Journey. + + Returns: + Id of the added direct steering stage. + """ + return self._obj.add_direct_steering_stage() + def add_journey(self, journey: JourneyDescription) -> int: return self._obj.add_journey(journey._obj)