After building a rescue lane, the vehicles return to their normal lat…

…eral alignment, refs #1967
eclipse-sumo · Jul 4, 2018 · 93de28d · 93de28d
1 parent 22fed77
commit 93de28d
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 7 deletions.
diff --git a/src/microsim/devices/MSDevice_Bluelight.cpp b/src/microsim/devices/MSDevice_Bluelight.cpp
@@ -134,7 +134,8 @@ MSDevice_Bluelight::notifyMove(SUMOVehicle& veh, double /* oldPos */,
             double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
             // the perception of the sound of the siren should be around 25 meters
             // todo only vehicles in front of the emergency vehicle should react
-            if (distanceDelta <= 25 && veh.getID() != veh2->getID()) {
+            if (distanceDelta <= 25 && veh.getID() != veh2->getID() && influencedVehicles.count(veh2->getID())== 0) {
+                influencedVehicles.insert(static_cast<std::string>(veh2->getID()));
                 //std::cout << "In Range Vehicle '" << veh2->getID() << "\n";
                 MSVehicleType& t = static_cast<MSVehicle*>(veh2)->getSingularType();
                 MSVehicle::Influencer& lanechange = static_cast<MSVehicle*>(veh2)->getInfluencer();
@@ -144,16 +145,37 @@ MSDevice_Bluelight::notifyMove(SUMOVehicle& veh, double /* oldPos */,
                 const int numLanes= (int)veh2->getEdge()->getLanes().size();
                 //Setting the lateral alignment to build a rescue lane
                 if (veh2->getLane()->getIndex() == numLanes-1) {
+                    influenced.insert(std::make_pair(static_cast<std::string>(veh2->getID()), t.getPreferredLateralAlignment()));
                     t.setPreferredLateralAlignment(LATALIGN_LEFT);
-                    //std::cout << "New alignment to left for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
+                    std::cout << "New alignment to left for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
                 } else {
+                    influenced.insert(std::make_pair(static_cast<std::string>(veh2->getID()), t.getPreferredLateralAlignment()));
                     t.setPreferredLateralAlignment(LATALIGN_RIGHT);
-                    //std::cout << "New alignment to right for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
+                    std::cout << "New alignment to right for vehicle: " << veh2->getID() << " " << veh2->getVehicleType().getPreferredLateralAlignment() << "\n";
                 }
-                //influencedVehicles->push_back(static_cast<MSVehicle*>(veh2));
+
+
+                //std::cout << "Vehcile in influencedVehicleList: " << influencedVehicles.size() << "\n";
+                std::cout << "Vehcile in influenced: " << influenced.size() << "\n";
                 //std::cout << "Vehcile in influencedVehicleList: " << veh2->getID() << "\n";
+                //influencedVehicles->push_back(static_cast<MSVehicle>(veh2));
+
+            }
+
+        }
+        else {//if vehicle is passed all vehicles which had to react should get their state back after they leave the communication range
+            double distanceDelta = veh.getPosition().distanceTo(veh2->getPosition());
+            if (distanceDelta > 25 && veh.getID() != veh2->getID() && influencedVehicles.count(veh2->getID()) > 0) {
+                influencedVehicles.erase(veh2->getID());
+                MSVehicleType& t = static_cast<MSVehicle*>(veh2)->getSingularType();
+                std::map<std::string, LateralAlignment>::iterator it = influenced.find(veh2->getID());
+                if (it != influenced.end()) {
+                    //it->second();
+                    std::cout << "Change allingnment for" << veh2->getID() << "\n";
+                    t.setPreferredLateralAlignment(LATALIGN_CENTER);
+                }
+
             }
-            //todo if vehicle is passed all vehicles which had to react should get their state back after they leave the communication range
         }
     }
     return true; // keep the device

diff --git a/src/microsim/devices/MSDevice_Bluelight.h b/src/microsim/devices/MSDevice_Bluelight.h
@@ -145,8 +145,10 @@ class MSDevice_Bluelight : public MSDevice {
 
 
 private:
-    // @brief collects all vehicle which had to react to the emergency vehicle 
-    //std::vector<MSVehicle*>* influencedVehicles;
+    // @brief collects all vehicleIDs which had to react to the emergency vehicle 
+    std::set<std::string> influencedVehicles;
+
+    std::map<std::string, LateralAlignment> influenced;
 
     /// @brief a value which is initialised based on a commandline/configuration option
     double myCustomValue1;