added option for stopping after collision refs #1102

git-svn-id: file:///home/behr_mi/git/sumo_synched/trunk@23601 afbd958f-9f77-42d5-a016-97a22340ccf4

sumo/src/mesosim/MEVehicle.cpp

@@ -215,7 +215,7 @@ MEVehicle::replaceRoute(const MSRoute* newRoute, bool onInit, int offset, bool a
 
 
 bool
-MEVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& /*errorMsg*/, SUMOTime /* untilOffset */) {
+MEVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& /*errorMsg*/, SUMOTime /* untilOffset */, bool /*collision*/) {
     const MSEdge* const edge = MSEdge::dictionary(stopPar.lane.substr(0, stopPar.lane.rfind('_')));
     assert(edge != 0);
     MESegment* stopSeg = MSGlobals::gMesoNet->getSegmentForEdge(*edge, stopPar.endPos);

sumo/src/mesosim/MEVehicle.h

@@ -152,7 +152,7 @@ class MEVehicle : public MSBaseVehicle {
      * @param[in] stop The stop to add
      * @return Whether the stop could be added
      */
-    bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0);
+    bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0, bool collision=false);
 
 
     /** @brief Returns whether the vehicle is at a stop

sumo/src/microsim/MSFrame.cpp

@@ -259,6 +259,9 @@ MSFrame::fillOptions() {
     oc.doRegister("collision.action", new Option_String("teleport"));
     oc.addDescription("collision.action", "Processing", "How to deal with collisions: [none,warn,teleport,remove]");
 
+    oc.doRegister("collision.stoptime", new Option_String("0", "TIME"));
+    oc.addDescription("collision.stoptime", "Processing", "Let vehicle stop for TIME before performing collision.action (except for action 'none')");
+
     oc.doRegister("collision.check-junctions", new Option_Bool(false));
     oc.addDescription("collision.check-junctions", "Processing", "Enables collisions checks on junctions");
 

sumo/src/microsim/MSLane.cpp

@@ -87,6 +87,7 @@
 MSLane::DictType MSLane::myDict;
 MSLane::CollisionAction MSLane::myCollisionAction(MSLane::COLLISION_ACTION_TELEPORT);
 bool MSLane::myCheckJunctionCollisions(false);
+SUMOTime MSLane::myCollisionStopTime(0);
 
 // ===========================================================================
 // internal class method definitions
@@ -1062,7 +1063,7 @@ MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
                                      getRightSideOnEdge() - shadowLane->getRightSideOnEdge(),
                                      follow->getPositionOnLane());
                 for (int i = 0; i < ahead.numSublanes(); ++i) {
-                    const MSVehicle* lead = ahead[i];
+                    MSVehicle* lead = const_cast<MSVehicle*>(ahead[i]);
                     if (lead != 0 && lead != follow && shadowLane->detectCollisionBetween(timestep, stage, follow, lead, toRemove, toTeleport)) {
                         break;
                     }
@@ -1076,7 +1077,7 @@ MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
         //std::cout << SIMTIME << " checkJunctionCollisions " << getID() << "\n";
         const std::vector<const MSLane*>& foeLanes = myLinks.front()->getFoeLanes();
         for (VehCont::iterator veh = myVehicles.begin(); veh != myVehicles.end(); ++veh) {
-            const MSVehicle* collider = *veh;
+            MSVehicle* collider = *veh;
             //std::cout << "   collider " << collider->getID() << "\n";
             PositionVector colliderBoundary = collider->getBoundingBox();
             for (std::vector<const MSLane*>::const_iterator it = foeLanes.begin(); it != foeLanes.end(); ++it) {
@@ -1112,12 +1113,12 @@ MSLane::detectCollisions(SUMOTime timestep, const std::string& stage) {
 
 
 bool
-MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
+MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
                                std::set<const MSVehicle*, SUMOVehicle::ComparatorIdLess>& toRemove,
                                std::set<const MSVehicle*>& toTeleport) const {
 #ifndef NO_TRACI
-    if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer()->isVTDAffected(timestep)) ||
-            (collider->hasInfluencer() && collider->getInfluencer()->isVTDAffected(timestep)))) {
+    if (myCollisionAction == COLLISION_ACTION_TELEPORT && ((victim->hasInfluencer() && victim->getInfluencer().isVTDAffected(timestep)) ||
+            (collider->hasInfluencer() && collider->getInfluencer().isVTDAffected(timestep)))) {
         return false;
     }
 #endif
@@ -1169,49 +1170,83 @@ MSLane::detectCollisionBetween(SUMOTime timestep, const std::string& stage, cons
 
 
 void
-MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
+MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
                                double gap, double latGap, std::set<const MSVehicle*, SUMOVehicle::ComparatorIdLess>& toRemove,
                                std::set<const MSVehicle*>& toTeleport) const {
-    std::string prefix;
-    switch (myCollisionAction) {
-        case COLLISION_ACTION_WARN:
-            prefix = "Vehicle '" + collider->getID() + "'; collision with vehicle '" + victim->getID() ;
-            break;
-        case COLLISION_ACTION_TELEPORT:
-            prefix = "Teleporting vehicle '" + collider->getID() + "'; collision with vehicle '" + victim->getID() ;
-            toRemove.insert(collider);
-            toTeleport.insert(collider);
-            break;
-        case COLLISION_ACTION_REMOVE: {
-            prefix = "Removing collision participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
-            bool removeCollider = true;
-            bool removeVictim = true;
-#ifndef NO_TRACI
-            removeVictim = !(victim->hasInfluencer() && victim->getInfluencer()->isVTDAffected(timestep));
-            removeCollider = !(collider->hasInfluencer() && collider->getInfluencer()->isVTDAffected(timestep));
-            if (removeVictim) {
-                toRemove.insert(victim);
-            }
-            if (removeCollider) {
+    std::string prefix = "Vehicle '" + collider->getID() + "'; collision with vehicle '" + victim->getID() ;
+    if (myCollisionStopTime > 0) {
+        if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
+            return;
+        }
+        std::string dummyError;
+        SUMOVehicleParameter::Stop stop;
+        stop.duration = myCollisionStopTime;
+        stop.busstop = "";
+        stop.containerstop = "";
+        stop.chargingStation = "";
+        stop.parkingarea = "";
+        stop.until = 0;
+        stop.triggered = false;
+        stop.containerTriggered = false;
+        stop.parking = false;
+        stop.index = 0;
+        const double victimStopPos = MIN2(victim->getLane()->getLength(), 
+                    victim->getPositionOnLane() + victim->getCarFollowModel().brakeGap(victim->getSpeed()));
+        if (victim->collisionStopTime() < 0) {
+            stop.lane = victim->getLane()->getID();
+            // @todo: push victim forward?
+            stop.startPos = victimStopPos;
+            stop.endPos = stop.startPos;
+            stop.duration = myCollisionStopTime;
+            victim->addStop(stop, dummyError, 0, true);
+        }
+        if (collider->collisionStopTime() < 0) {
+            stop.lane = collider->getLane()->getID();
+            stop.startPos = MIN2(collider->getPositionOnLane() + collider->getCarFollowModel().brakeGap(collider->getSpeed()), 
+                    MAX2(0.0, victimStopPos - 0.75 * victim->getVehicleType().getLength()));
+            stop.endPos = stop.startPos;
+            collider->addStop(stop, dummyError, 0, true);
+        }
+    } else {
+        switch (myCollisionAction) {
+            case COLLISION_ACTION_WARN:
+                break;
+            case COLLISION_ACTION_TELEPORT:
+                prefix = "Teleporting vehicle '" + collider->getID() + "'; collision with vehicle '" + victim->getID() ;
                 toRemove.insert(collider);
-            }
-            if (!removeVictim) {
-                if (!removeCollider) {
-                    prefix = "Keeping remote-controlled collision participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
-                } else {
-                    prefix = "Removing collision participant: vehicle '" + collider->getID() + "', keeping remote-controlled vehicle '" + victim->getID();
+                toTeleport.insert(collider);
+                break;
+            case COLLISION_ACTION_REMOVE: {
+                prefix = "Removing collision participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
+                bool removeCollider = true;
+                bool removeVictim = true;
+#ifndef NO_TRACI
+                removeVictim = !(victim->hasInfluencer() && victim->getInfluencer().isVTDAffected(timestep));
+                removeCollider = !(collider->hasInfluencer() && collider->getInfluencer().isVTDAffected(timestep));
+                if (removeVictim) {
+                    toRemove.insert(victim);
+                }
+                if (removeCollider) {
+                    toRemove.insert(collider);
+                }
+                if (!removeVictim) {
+                    if (!removeCollider) {
+                        prefix = "Keeping remote-controlled collision participants: vehicle '" + collider->getID() + "', vehicle '" + victim->getID();
+                    } else {
+                        prefix = "Removing collision participant: vehicle '" + collider->getID() + "', keeping remote-controlled vehicle '" + victim->getID();
+                    }
+                } else if (!removeCollider) {
+                    prefix = "Keeping remote-controlled collision participant: vehicle '" + collider->getID() + "', removing vehicle '" + victim->getID();
                 }
-            } else if (!removeCollider) {
-                prefix = "Keeping remote-controlled collision participant: vehicle '" + collider->getID() + "', removing vehicle '" + victim->getID();
-            }
 #else
-            toRemove.insert(victim);
-            toRemove.insert(collider);
+                toRemove.insert(victim);
+                toRemove.insert(collider);
 #endif
-            break;
+                break;
+            }
+            default:
+                break;
         }
-        default:
-            break;
     }
     WRITE_WARNING(prefix
                   + "', lane='" + getID()
@@ -1258,6 +1293,21 @@ MSLane::executeMovements(SUMOTime t, std::vector<MSLane*>& lanesWithVehiclesToIn
                           time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
             MSNet::getInstance()->getVehicleControl().registerCollision();
             MSVehicleTransfer::getInstance()->add(t, veh);
+        } else if (veh->collisionStopTime() == 0) {
+            veh->resumeFromStopping();
+            if (getCollisionAction() == COLLISION_ACTION_REMOVE) {
+                WRITE_WARNING("Removing vehicle '" + veh->getID() + "' after earlier collision, lane='" + veh->getLane()->getID() + ", time=" + 
+                        time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
+                veh->onRemovalFromNet(MSMoveReminder::NOTIFICATION_VAPORIZED);
+                MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
+            } else if (getCollisionAction() == COLLISION_ACTION_TELEPORT) {
+                WRITE_WARNING("Teleporting vehicle '" + veh->getID() + "' after earlier collision, lane='" + veh->getLane()->getID() + ", time=" + 
+                        time2string(MSNet::getInstance()->getCurrentTimeStep()) + ".");
+                MSVehicleTransfer::getInstance()->add(MSNet::getInstance()->getCurrentTimeStep(), veh);
+            } else {
+                ++i;
+                continue;
+            }
         } else {
             ++i;
             continue;
@@ -2838,6 +2888,7 @@ MSLane::initCollisionOptions(const OptionsCont& oc) {
         throw ProcessError("Invalid collision.action '" + action + "'.");
     }
     myCheckJunctionCollisions = oc.getBool("collision.check-junctions");
+    myCollisionStopTime = string2time(oc.getString("collision.stoptime"));
 }
 
 

sumo/src/microsim/MSLane.h

@@ -1015,6 +1015,10 @@ class MSLane : public Named, public Parameterised {
         return myCollisionAction == COLLISION_ACTION_TELEPORT;
     }
 
+    static CollisionAction getCollisionAction() {
+        return myCollisionAction;
+    }
+
     static const long CHANGE_PERMISSIONS_PERMANENT = 0;
     static const long CHANGE_PERMISSIONS_GUI = 1;
 
@@ -1039,12 +1043,12 @@ class MSLane : public Named, public Parameterised {
 
 
     /// @brief detect whether there is a collision between the two vehicles
-    bool detectCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
+    bool detectCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
                                 std::set<const MSVehicle*, SUMOVehicle::ComparatorIdLess>& toRemove,
                                 std::set<const MSVehicle*>& toTeleport) const;
 
     /// @brief take action upon collision
-    void handleCollisionBetween(SUMOTime timestep, const std::string& stage, const MSVehicle* collider, const MSVehicle* victim,
+    void handleCollisionBetween(SUMOTime timestep, const std::string& stage, MSVehicle* collider, MSVehicle* victim,
                                 double gap, double latGap,
                                 std::set<const MSVehicle*, SUMOVehicle::ComparatorIdLess>& toRemove,
                                 std::set<const MSVehicle*>& toTeleport) const;
@@ -1197,6 +1201,7 @@ class MSLane : public Named, public Parameterised {
     /// @brief the action to take on collisions
     static CollisionAction myCollisionAction;
     static bool myCheckJunctionCollisions;
+    static SUMOTime myCollisionStopTime;
 
     /**
      * @class vehicle_position_sorter

sumo/src/microsim/MSVehicle.cpp

@@ -534,6 +534,7 @@ MSVehicle::MSVehicle(SUMOVehicleParameter* pars, const MSRoute* route,
     myHaveToWaitOnNextLink(false),
     myAngle(0),
     myStopDist(std::numeric_limits<double>::max()),
+    myCollisionImmunity(-1),
     myCachedPosition(Position::INVALID),
     myEdgeWeights(0)
 #ifndef NO_TRACI
@@ -878,7 +879,7 @@ MSVehicle::getBackPosition() const {
 
 // ------------
 bool
-MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset) {
+MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset, bool collision) {
     Stop stop;
     stop.lane = MSLane::dictionary(stopPar.lane);
     if (!stop.lane->allowsVehicleClass(myType->getVehicleClass())) {
@@ -903,6 +904,7 @@ MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& error
     stop.triggered = stopPar.triggered;
     stop.containerTriggered = stopPar.containerTriggered;
     stop.parking = stopPar.parking;
+    stop.collision = collision;
     stop.reached = false;
     if (stop.startPos < 0 || stop.endPos > stop.lane->getLength()) {
         if (stop.busstop != 0) {
@@ -957,7 +959,7 @@ MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& error
         }
     }
     if (stop.edge == myRoute->end() || prevStopEdge > stop.edge ||
-            (prevStopEdge == stop.edge && prevStopPos > stop.endPos)) {
+            (prevStopEdge == stop.edge && prevStopPos > stop.endPos && !collision)) {
         if (stop.busstop != 0) {
             errorMsg = "Bus stop '" + stop.busstop->getID() + "'";
         } else {
@@ -968,7 +970,11 @@ MSVehicle::addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& error
     }
     // David.C:
     //if (!stop.parking && (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed))) {
-    if (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed)) {
+    if (collision) {
+        assert(myCurrEdge == stop.edge);
+        myState.myPos = stop.endPos;
+        myState.mySpeed = 0;
+    } else if (myCurrEdge == stop.edge && myState.myPos > stop.endPos - getCarFollowModel().brakeGap(myState.mySpeed)) {
         errorMsg = "Stop for vehicle '" + myParameter->id + "' on lane '" + stopPar.lane + "' is too close to break.";
         return false;
     }
@@ -1100,6 +1106,12 @@ MSVehicle::isStopped() const {
 }
 
 
+SUMOTime
+MSVehicle::collisionStopTime() const {
+    return (myStops.empty() || !myStops.front().collision) ? myCollisionImmunity : MAX2((SUMOTime)0, myStops.front().duration);
+}
+
+
 bool
 MSVehicle::isParking() const {
     return isStopped() && myStops.begin()->parking;
@@ -1185,7 +1197,7 @@ MSVehicle::processNextStop(double currentVelocity) {
 #endif
             }
         }
-        if (stop.duration <= 0 && !stop.triggered && !stop.containerTriggered) {
+        if (stop.duration <= 0 && !stop.triggered && !stop.containerTriggered && !stop.collision) {
 #ifdef DEBUG_STOPS
             if (DEBUG_COND) {
                 std::cout << SIMTIME << " vehicle '" << getID() << "' resumes from stopping." << std::endl;
@@ -2184,6 +2196,7 @@ MSVehicle::executeMove() {
             myTimeLoss += TIME2STEPS(TS * (vmax - vNext) / vmax);
         }
     }
+    myCollisionImmunity = MAX2((SUMOTime)-1, myCollisionImmunity - DELTA_T);
 
     if (!hasArrived() && !myLane->getEdge().isVaporizing()) {
         if (myState.myPos > myLane->getLength()) {
@@ -3903,6 +3916,9 @@ MSVehicle::resumeFromStopping() {
         if (MSStopOut::active()) {
             MSStopOut::getInstance()->stopEnded(this, myStops.front());
         }
+        if (myStops.front().collision && MSLane::getCollisionAction() == MSLane::COLLISION_ACTION_WARN) {
+            myCollisionImmunity = TIME2STEPS(5); // leave the conflict area
+        }
         myStops.pop_front();
         // do not count the stopping time towards gridlock time.
         // Other outputs use an independent counter and are not affected.

sumo/src/microsim/MSVehicle.h

@@ -850,6 +850,8 @@ class MSVehicle : public MSBaseVehicle {
         SUMOTime timeToBoardNextPerson;
         /// @brief The time at which the vehicle is able to load another container
         SUMOTime timeToLoadNextContainer;
+        /// @brief Whether this stop was triggered by a collision
+        bool collision;
 
         void write(OutputDevice& dev) const;
 
@@ -864,7 +866,7 @@ class MSVehicle : public MSBaseVehicle {
      * @param[in] stop The stop to add
      * @return Whether the stop could be added
      */
-    bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0);
+    bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0, bool collision = false);
 
     /** @brief replace the current parking area stop with a new stop with merge duration
      */
@@ -887,6 +889,11 @@ class MSVehicle : public MSBaseVehicle {
      */
     bool isStopped() const;
 
+    /** @brief Returns the remaining time a vehicle needs to stop due to a
+     * collision. A negative value indicates that the vehicle is not stopping due to a collision (or at all)
+     */
+    SUMOTime collisionStopTime() const;
+
     /** @brief Returns whether the vehicle is parking
      * @return whether the vehicle is parking
      */
@@ -1538,6 +1545,9 @@ class MSVehicle : public MSBaseVehicle {
     /// @brief distance to the next stop or -1 if there is none
     double myStopDist;
 
+    /// @brief amount of time for which the vehicle is immune from collisions
+    SUMOTime myCollisionImmunity;
+
     mutable Position myCachedPosition;
 
 protected:

sumo/src/utils/vehicle/SUMOVehicle.h

@@ -304,7 +304,7 @@ class SUMOVehicle {
      * @param[in] stop The stop to add
      * @return Whether the stop could be added
      */
-    virtual bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0) = 0;
+    virtual bool addStop(const SUMOVehicleParameter::Stop& stopPar, std::string& errorMsg, SUMOTime untilOffset = 0, bool collision=false) = 0;
 
 
     /**