working implementation of stationfinder device ref #9663

Signed-off-by: m-kro <m.barthauer@t-online.de>

src/microsim/MSLane.cpp

@@ -21,6 +21,7 @@
 /// @author  Christoph Sommer
 /// @author  Mario Krumnow
 /// @author  Leonhard Luecken
+/// @author  Mirko Barthauer
 /// @date    Mon, 05 Mar 2001
 ///
 // Representation of a lane in the micro simulation
@@ -2224,6 +2225,12 @@ MSLane::executeMovements(const SUMOTime t) {
                            veh->getID(), getID(), time2string(t));
             MSNet::getInstance()->getVehicleControl().registerCollision(true);
             MSVehicleTransfer::getInstance()->add(t, veh);
+        } else if (veh->brokeDown()) {
+            veh->resumeFromStopping();
+            WRITE_WARNINGF(TL("Removing vehicle '%' after breaking down, lane='%', time=%."),
+                           veh->getID(), veh->getLane()->getID(), time2string(t));
+            veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED_BREAKDOWN);
+            MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
         } else if (veh->collisionStopTime() == 0) {
             veh->resumeFromStopping();
             if (getCollisionAction() == COLLISION_ACTION_REMOVE) {

src/microsim/MSMoveReminder.h

@@ -120,7 +120,9 @@ class MSMoveReminder {
         /// @brief The vehicle got removed via the GUI
         NOTIFICATION_VAPORIZED_GUI,
         /// @brief The vehicle got vaporized with a vaporizer
-        NOTIFICATION_VAPORIZED_VAPORIZER
+        NOTIFICATION_VAPORIZED_VAPORIZER,
+        /// @brief The vehicle got removed via stationfinder device
+        NOTIFICATION_VAPORIZED_BREAKDOWN
     };
 
 

src/microsim/MSRoute.h

@@ -158,6 +158,7 @@ class MSRoute : public Named, public Parameterised {
      */
     double getDistanceBetween(double fromPos, double toPos, const MSRouteIterator& fromEdge, const MSRouteIterator& toEdge, bool includeInternal = true) const;
 
+
     /// @brief Returns the color
     const RGBColor& getColor() const;
 

src/microsim/MSVehicle.cpp

@@ -1579,9 +1579,9 @@ MSVehicle::keepStopping(bool afterProcessing) const {
     if (isStopped()) {
         // when coming out of vehicleTransfer we must shift the time forward
         return (myStops.front().duration - (afterProcessing ? DELTA_T : 0) > 0 || isStoppedTriggered() || myStops.front().pars.collision
-                || (myStops.front().getSpeed() > 0
-                    && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
-                    && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
+                || myStops.front().pars.breakDown || (myStops.front().getSpeed() > 0
+                        && (myState.myPos < MIN2(myStops.front().pars.endPos, myStops.front().lane->getLength() - POSITION_EPS))
+                        && (myStops.front().pars.parking == ParkingType::ONROAD || getSpeed() >= SUMO_const_haltingSpeed)));
     } else {
         return false;
     }
@@ -1603,6 +1603,12 @@ MSVehicle::collisionStopTime() const {
 }
 
 
+bool
+MSVehicle::brokeDown() const {
+    return isStopped() && !myStops.empty() && myStops.front().pars.breakDown;
+}
+
+
 bool
 MSVehicle::ignoreCollision() const {
     return myCollisionImmunity > 0;

src/microsim/MSVehicle.h

@@ -1025,6 +1025,10 @@ class MSVehicle : public MSBaseVehicle {
      */
     SUMOTime collisionStopTime() const;
 
+    /** @brief Returns how long the vehicle has been stopped already due to lack of energy.
+     */
+    bool brokeDown() const;
+
     /** @brief Returns the information whether the vehicle is fully controlled via TraCI
      * @return Whether the vehicle is remote-controlled
      */

src/microsim/devices/MSDevice_StationFinder.h

@@ -13,16 +13,23 @@
 /****************************************************************************/
 /// @file    MSDevice_StationFinder.h
 /// @author  Michael Behrisch
+/// @author  Mirko Barthauer
 /// @date    2023-05-24
 ///
 // A device which triggers rerouting to nearby charging stations
 /****************************************************************************/
 #pragma once
 #include <config.h>
 
+#include <utils/common/WrappingCommand.h>
 #include "MSVehicleDevice.h"
 
 
+#define DEFAULT_SOC_INTERVAL 0.1
+#define DEFAULT_ENERGY_PER_DISTANCE 200 // Wh/km
+#define DEFAULT_AVG_WAITING_TIME 900 // s
+#define DEFAULT_CHARGINGSTATION_VIEW_DIST 10 // m
+
 // ===========================================================================
 // class declarations
 // ===========================================================================
@@ -45,6 +52,19 @@ class MSStoppingPlace;
  */
 class MSDevice_StationFinder : public MSVehicleDevice {
 public:
+    enum ChargeType {
+        CHARGE_TYPE_CHARGING,
+        CHARGE_TYPE_BIDIRECTIONAL,
+        CHARGE_TYPE_BATTERY_EXCHANGE,
+        CHARGE_TYPE_FUEL
+    };
+
+    enum RescueAction {
+        RESCUE_ACTION_NONE,
+        RESCUE_ACTION_REMOVE,
+        RESCUE_ACTION_TOW
+    };
+
     /** @brief Inserts MSDevice_StationFinder-options
      */
     static void insertOptions(OptionsCont& oc);
@@ -65,6 +85,8 @@ class MSDevice_StationFinder : public MSVehicleDevice {
     static void buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into);
 
 
+    static void initRescueAction(const SUMOVehicle& v, const OptionsCont& oc, const std::string& option, RescueAction& myAction);
+
 public:
     /** @brief Constructor
      *
@@ -124,6 +146,9 @@ class MSDevice_StationFinder : public MSVehicleDevice {
         myBattery = battery;
     }
 
+    std::string getParameter(const std::string& key) const;
+
+
 protected:
     /** @brief Internal notification about the vehicle moves, see MSMoveReminder::notifyMoveInternal()
      *
@@ -138,20 +163,114 @@ class MSDevice_StationFinder : public MSVehicleDevice {
                             const double meanLengthOnLane);
 
 private:
+    /** @brief central search function for close charging stations
+     *
+     * @param[in] router
+     * @param[in] expectedConsumption
+     * @param[in] constrainTT whether to constrain the search radius by a maximum travel time
+     * @param[in] skipVisited whether to skip charging stations which have not been available when passing by recently
+     * @return The found charging station, otherwise nullptr
+     */
+    MSChargingStation* findChargingStation(SUMOAbstractRouter<MSEdge, SUMOVehicle>& router, double expectedConsumption, bool constrainTT = true, bool skipVisited = true);
+
+    /** @brief reroute to a charging station
+     *
+     * @param[in] replace if the already planned next stop should be replaced (a new stop will be prepended if false)
+     * @return true if the vehicle has been redirected to a charging station, false otherwise
+     */
+    bool rerouteToChargingStation(bool replace = false);
+
+    /** @brief search for a charging station and teleport the vehicle there as a rescue measure
+     */
+    SUMOTime teleportToChargingStation(const SUMOTime currentTime);
+
+    /** @brief estimate the energy needed for the planned route / up to a target edge
+     *
+     * @param[in] target edge along the route up to which the consumption shall be estimated - the complete route will be used if defaulting to nullptr
+     * @param[in] includeEmptySoC whether to add an additional buffer for the range up to the "empty" threshold
+     * @return energy in Wh needed to complete the planned route
+     */
+    double estimateConsumption(const MSEdge* target = nullptr, const bool includeEmptySoC = true) const;
+
+    /** @brief adopt a planned charging stop outside of the device
+     *
+     * @return whether an already present stop was adopted to be used with the device logic
+     */
+    bool alreadyPlannedCharging();
+
+    /** @brief create the event command for teleporting in case of brake-down
+     */
+    void initRescueCommand();
+
+private:
+    /// @brief myHolder cast to needed type
+    MSVehicle& myVeh;
+
     /// @brief The corresponding battery device
     MSDevice_Battery* myBattery;
 
+    /// @brief To which station we are currently travelling
+    MSStoppingPlace* myChargingStation;
+
+    /// @brief The command responsible for rescue actions
+    WrappingCommand<MSDevice_StationFinder>* myRescueCommand;
+
+    /// @brief The memory of lastly visited charging stations during the search before being able to charge
+    std::vector<MSChargingStation*> myPassedChargingStations;
+
+    /// @brief Last time the SoC was checked
+    SUMOTime myLastChargeCheck;
+
+    /// @brief Time interval after which the SoC has to be checked
+    SUMOTime myCheckInterval;
+
+    /// @brief Arrival time in the vicinity of the target charging station (to track the waiting time before accessing it)
+    SUMOTime myArrivalAtChargingStation;
+
+    /// @brief The time to wait for a rescue vehicle in case the battery is empty
+    double myRescueTime;
+
     /// @brief The safety buffer when calculating expected consumption
     double myReserveFactor;
 
-    /// @brief To which station we are currently travelling
-    MSStoppingPlace* myChargingStation;
+    /// @brief The state of charge threshold below which rescue mode is activated
+    double myEmptySoC;
+
+    /// @brief The max travel time to the next charging station
+    SUMOTime myRadius;
+
+    /// @brief Time interval to search again for a charging station if the first attempt failed
+    SUMOTime myRepeatInterval;
+
+    /// @brief Accepted waiting time at the charging station before a place becomes available
+    SUMOTime myWaitForCharge;
+
+    /// @brief SoC the last time the station finder algorithm was run completely
+    double myUpdateSoC;
+
+    /// @brief The maximum charging speed of the vehicle battery in W
+    double myMaxChargePower;
+
+    /// @brief The target state of charge where the vehicle stops charging
+    double myTargetSoC;
+
+    /// @brief The state of charge at which the vehicle starts looking for charging stations
+    double mySearchSoC;
+
+    /// @brief The marker for executing a rescue action
+    bool myExecuteRescue = false;
+
+    /// @brief The type of charging permitted by the battery (charging, bidirectional, battery exchange)
+    ChargeType myChargeType;
+
+    /// @brief What to do when the state of charge gets very low
+    RescueAction myRescueAction;
+
 
 private:
     /// @brief Invalidated copy constructor.
     MSDevice_StationFinder(const MSDevice_StationFinder&);
 
     /// @brief Invalidated assignment operator.
     MSDevice_StationFinder& operator=(const MSDevice_StationFinder&);
-
 };

src/utils/vehicle/SUMOVehicleParameter.h

@@ -471,6 +471,9 @@ class SUMOVehicleParameter : public Parameterised {
         /// @brief Whether this stop was triggered by a collision
         bool collision = false;
 
+        /// @brief Whether this stop was triggered by a car failure / mechanical problem / lack of energy
+        bool breakDown = false;
+
         /// @brief return flags as per Vehicle::getStops
         int getFlags() const;
     };