fixed speed limit

Signed-off-by: hpcwoess <woessner@hlrs.de>

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmSituationAssessment.cpp

@@ -232,7 +232,12 @@ double SituationAssessment::AdaptionOnSpeedLimit(double SpeedLimit, double Dista
             lastSpeedLimit = SpeedLimit;
             double v_Wish_target = SpeedLimit + commonSpeedLimit_Violation;
             const double v_ego = SA_Input->MM_O->Ego->GetState_Ego()->velocity_long;
-            v_Wish_new = v_ego + (v_Wish_target - v_ego)/Distance * (cycleTime*0.001);
+            //v_Wish_new = v_ego + (v_Wish_target - v_ego)/Distance * (cycleTime*0.001); // please explain this to me I would rather say that the wish speed should be the speed limit when the distance reaches 0 (whell the stvo sais something else) but this should be the minimum...
+            //double a = ((v_Wish_target - v_ego) * (v_Wish_target - v_ego)) / (2 * Distance);
+            double tToSpeedlimit = 2 * Distance / sqrt(v_ego -v_Wish_target);
+            v_Wish_new = v_ego - (((v_ego - v_Wish_target) / tToSpeedlimit) * (cycleTime * 0.001));
+            if (v_Wish_new < lastSpeedLimit + commonSpeedLimit_Violation)
+                v_Wish_new = lastSpeedLimit + commonSpeedLimit_Violation; // just to be safe
             return v_Wish_new;
         }
         if (Distance < 0)
@@ -240,7 +245,7 @@ double SituationAssessment::AdaptionOnSpeedLimit(double SpeedLimit, double Dista
             lastSpeedLimit = SpeedLimit;
         }
     }
-    if (vWish > lastSpeedLimit)
+    if (vWish > lastSpeedLimit + commonSpeedLimit_Violation)
     {
         vWish = lastSpeedLimit + commonSpeedLimit_Violation;
         return lastSpeedLimit + commonSpeedLimit_Violation;

OpenPass_Source_Code/openPASS/Components/Sensor_Modular_Driver/Container/ContainerStructures.h

@@ -86,7 +86,7 @@ public:
     //!
     void UpdateSpeedLimitAndDistance(double SpeedLimit, double Distance)
     {
-        if (SpeedLimits.size()>0 && SpeedLimit != SpeedLimits.front())
+        if (SpeedLimits.size()==0 || SpeedLimit != SpeedLimits.front()) // don't check front speed if the list is empty
         {
             SpeedLimits.push_front(SpeedLimit);
             DistanceToSpeedLimits.push_front(Distance);

OpenPass_Source_Code/openPASS/CoreModules/World_OSI/OWL/DataTypes.cpp

@@ -1368,7 +1368,37 @@ void TrafficSign::SetSpecification(RoadSignalInterface* signal)
 
         mutableOsiClassification->set_type(valueAndUnit.first);
 
-        const double value = std::stoi(signal->GetSubType());
+        std::string subtype = signal->GetSubType();
+        double value = signal->GetValue();
+        if (value <= 0.0)
+        {
+            if (subtype == "50")
+                value = 5.0;
+            else if (subtype == "51")
+                value = 10.0;
+            else if (subtype == "52")
+                value = 20.0;
+            else if (subtype == "53")
+                value = 30.0;
+            else if (subtype == "54")
+                value = 40.0;
+            else if (subtype == "55")
+                value = 50.0;
+            else if (subtype == "56")
+                value = 60.0;
+            else if (subtype == "58")
+                value = 80.0;
+            else if (subtype == "59")
+                value = 90.0;
+            else if (subtype == "60")
+                value = 100.0;
+            else if (subtype == "61")
+                value = 110.0;
+            else if (subtype == "62")
+                value = 120.0;
+            else if (subtype == "63")
+                value = 130.0;
+        }
         mutableOsiClassification->mutable_value()->set_value(value);
         mutableOsiClassification->mutable_value()->set_value_unit(valueAndUnit.second);
     }

[Jan Dobberstein]

@kblenz please explain this to Uwe :)

[konstantin blenz]

The value of "commonSpeedLimit_Violation" is set stochastically by the parameters "MeanSpeedLimitViolation" and "MeanSpeedLimitViolationDeviation" from the system-config of the current agent. These values can be set to 0. It just provides an interface to equip the driver with intentional misbehaviour. To compare the simulated traffic with real-traffic, the simulation should be able to show a variance of driven velocities, even around a speed-limit.

[Uwe Woessner]

I did not change anything regarding commonSpeedLimit_Violation`this should still be the same. My cars did not reach v_Wish_target in time with the previous algorithm. I hope that the new approach (linear deceleration works better)

[konstantin blenz]

That was also my problem, so i tried to make the distance to the traffic-sign as long as possible. The new approach looks good! I hope this fixes the problem!