Add direct steering for agents (#1290)

This change will allow direct steering of agents, they will move towards the user defined target.

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

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<DirectSteeringProxy*>(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<const GenericAgent*>(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<GenericAgent*>(handle);
+        agent->target = intoPoint(waypoint);
+        return true;
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+    } catch(...) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(
+                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<Simulation*>(handle);
+    try {
+        return reinterpret_cast<JPS_DirectSteeringProxy>(
+            new DirectSteeringProxy(std::get<DirectSteeringProxy>(simulation->Stage(stageId))));
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+        return nullptr;
+    }
+}
+
 void JPS_Simulation_SetTracing(JPS_Simulation handle, bool status)
 {
     assert(handle);

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<GenericAgent> {
                         agent.journeyId,
                         agent.stageId,
                         agent.destination,
-                        agent.waypoint,
+                        agent.target,
                         agent.pos,
                         agent.orientation,
                         m);
@@ -78,7 +79,7 @@ struct fmt::formatter<GenericAgent> {
                         agent.journeyId,
                         agent.stageId,
                         agent.destination,
-                        agent.waypoint,
+                        agent.target,
                         agent.pos,
                         agent.orientation,
                         m);

libsimulator/src/Simulation.cpp

@@ -10,6 +10,7 @@
 #include "Stage.hpp"
 #include "Visitor.hpp"
 #include <memory>
+#include <variant>
 
 Simulation::Simulation(
     std::unique_ptr<OperationalModel>&& operationalModel,
@@ -60,6 +61,15 @@ void Simulation::Iterate()
 Journey::ID Simulation::AddJourney(const std::map<BaseStage::ID, TransitionDescription>& stages)
 {
     std::map<BaseStage::ID, JourneyNode> nodes;
+    bool containsDirectSteering =
+        std::find_if(std::begin(stages), std::end(stages), [this](auto const& pair) {
+            return std::holds_alternative<DirectSteeringProxy>(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<BaseStage::ID, TransitionDescr
                             }},
                         desc)}};
         });
+
     auto journey = std::make_unique<Journey>(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);
 

libsimulator/src/Stage.hpp

@@ -78,8 +78,20 @@ class ExitProxy : public BaseProxy
     ExitProxy(Simulation* simulation_, BaseStage* stage_) : BaseProxy(simulation_, stage_) {}
 };
 
-using StageProxy =
-    std::variant<WaypointProxy, NotifiableWaitingSetProxy, NotifiableQueueProxy, ExitProxy>;
+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<T>& 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);
+    };
+};

libsimulator/src/StageDescription.hpp

@@ -8,6 +8,9 @@
 #include <variant>
 #include <vector>
 
+struct DirectSteeringDescription {
+};
+
 struct WaypointDescription {
     Point position;
     double distance;
@@ -26,6 +29,7 @@ struct NotifiableQueueDescription {
 };
 
 using StageDescription = std::variant<
+    DirectSteeringDescription,
     WaypointDescription,
     ExitDescription,
     NotifiableWaitingSetDescription,

libsimulator/src/StageManager.hpp

@@ -38,13 +38,17 @@ class StageManager
                 },
                 [](const NotifiableQueueDescription& d) -> std::unique_ptr<BaseStage> {
                     return std::make_unique<NotifiableQueue>(d.slots);
+                },
+                [](const DirectSteeringDescription&) -> std::unique_ptr<BaseStage> {
+                    return std::make_unique<DirectSteering>();
                 }},
             stageDescription);
         if(stages.find(stage->Id()) != stages.end()) {
             throw SimulationError("Internal error, stage id already in use.");
         }
         const auto id = stage->Id();
         stages.emplace(id, std::move(stage));
+
         return id;
     }
 

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;
         }

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);
         }
     }