Added odometer for vehicles, updated test, refs eclipse-sumo#2465

rxd199682 · Dec 4, 2019 · 5331369 · 5331369
1 parent 3806d64
commit 5331369
Show file tree

Hide file tree

Showing 8 changed files with 31 additions and 21 deletions.
diff --git a/src/libsumo/Vehicle.cpp b/src/libsumo/Vehicle.cpp
@@ -492,23 +492,7 @@ double
 Vehicle::getDistance(const std::string& vehicleID) {
     MSVehicle* veh = Helper::getVehicle(vehicleID);
     if (veh->isOnRoad()) {
-        double distance;
-        if (veh->getLane()->isInternal()) {
-            // route edge still points to the edge before the intersection
-            const double normalEnd = (*veh->getCurrentRouteEdge())->getLength();
-            distance = (veh->getRoute().getDistanceBetween(veh->getDepartPos(), normalEnd,
-                        veh->getRoute().begin(), veh->getCurrentRouteEdge())
-                        + veh->getRoute().getDistanceBetween(normalEnd, veh->getPositionOnLane(),
-                                *veh->getCurrentRouteEdge(), &veh->getLane()->getEdge()));
-        } else {
-            distance = veh->getRoute().getDistanceBetween(veh->getDepartPos(), veh->getPositionOnLane(),
-                       veh->getRoute().begin(), veh->getCurrentRouteEdge());
-        }
-        if (distance == std::numeric_limits<double>::max()) {
-            return INVALID_DOUBLE_VALUE;
-        } else {
-            return distance;
-        }
+       return veh->getOdometer();
     } else {
         return INVALID_DOUBLE_VALUE;
     }

diff --git a/src/mesosim/MEVehicle.cpp b/src/mesosim/MEVehicle.cpp
@@ -415,6 +415,9 @@ MEVehicle::updateDetectors(SUMOTime currentTime, const bool isLeave, const MSMov
             }
 #endif
             ++rem;
+            if (reason == MSMoveReminder::NOTIFICATION_JUNCTION || reason == MSMoveReminder::NOTIFICATION_TELEPORT) {
+                myOdometer += getEdge()->getLength();
+            }
         } else {
 #ifdef _DEBUG
             if (myTraceMoveReminders) {

diff --git a/src/microsim/MSBaseVehicle.cpp b/src/microsim/MSBaseVehicle.cpp
@@ -85,6 +85,7 @@ MSBaseVehicle::MSBaseVehicle(SUMOVehicleParameter* pars, const MSRoute* route,
     myArrivalPos(-1),
     myArrivalLane(-1),
     myNumberReroutes(0),
+    myOdometer(0.),
     myNumericalID(myCurrentNumericalIndex++)
 #ifdef _DEBUG
     , myTraceMoveReminders(myShallTraceMoveReminders.count(pars->id) > 0)
@@ -390,6 +391,11 @@ MSBaseVehicle::resetRoutePosition(int index, DepartLaneDefinition departLaneProc
     myArrivalPos = (*(myRoute->end() - 1))->getLanes()[0]->getLength();
 }
 
+double
+MSBaseVehicle::getOdometer() const {
+    return -myDepartPos + myOdometer + getPositionOnLane();
+}
+
 
 void
 MSBaseVehicle::addPerson(MSTransportable* person) {

diff --git a/src/microsim/MSBaseVehicle.h b/src/microsim/MSBaseVehicle.h
@@ -299,6 +299,10 @@ class MSBaseVehicle : public SUMOVehicle {
     /// @brief reset index of edge within route
     void resetRoutePosition(int index, DepartLaneDefinition departLaneProcedure);
 
+    /** @brief Returns the distance that was already driven by this vehicle
+     * @return the distance driven [m]
+     */
+    double getOdometer() const;
 
     /** @brief Returns the number of new routes this vehicle got
      * @return the number of new routes this vehicle got
@@ -553,6 +557,9 @@ class MSBaseVehicle : public SUMOVehicle {
     /// @brief The number of reroutings
     int myNumberReroutes;
 
+    /// @brief A simple odometer to keep track of the length of the route already driven
+    double myOdometer;
+
     /* @brief magic value for undeparted vehicles
      * @note: in previous versions this was -1
      */

diff --git a/src/microsim/MSVehicle.cpp b/src/microsim/MSVehicle.cpp
@@ -4629,6 +4629,9 @@ MSVehicle::leaveLane(const MSMoveReminder::Notification reason, const MSLane* ap
             rem = myMoveReminders.erase(rem);
         }
     }
+    if (reason == MSMoveReminder::NOTIFICATION_JUNCTION || reason == MSMoveReminder::NOTIFICATION_TELEPORT) {
+        myOdometer += getLane()->getLength();
+    }
     if (reason != MSMoveReminder::NOTIFICATION_JUNCTION && reason != MSMoveReminder::NOTIFICATION_LANE_CHANGE) {
         // @note. In case of lane change, myFurtherLanes and partial occupation
         // are handled in enterLaneAtLaneChange()

diff --git a/src/microsim/MSVehicle.h b/src/microsim/MSVehicle.h
@@ -1155,8 +1155,6 @@ class MSVehicle : public MSBaseVehicle {
     double getHarmonoise_NoiseEmissions() const;
     //@}
 
-
-
     /// @name Interaction with persons
     //@{
 
@@ -2178,6 +2176,8 @@ class MSVehicle : public MSBaseVehicle {
     /// @brief An instance of a velocity/lane influencing instance; built in "getInfluencer"
     Influencer* myInfluencer;
 
+
+
 private:
     /// @brief invalidated default constructor
     MSVehicle();

diff --git a/tests/complex/traci/pythonApi/distance_after_reroute/output.complex b/tests/complex/traci/pythonApi/distance_after_reroute/output.complex
@@ -1,8 +1,9 @@
 Loading configuration ... done.
- Retrying in 1 seconds
 1145.0
 ('ed0', 'ed1', 'ed5')
 1145.0
 1165.0
 ('ed1', 'ed2')
-162.04
+1165.0
+129 1235.0
+130 1245.0
diff --git a/tests/complex/traci/pythonApi/distance_after_reroute/runner.py b/tests/complex/traci/pythonApi/distance_after_reroute/runner.py
@@ -44,6 +44,12 @@ def main(args):
             traci.vehicle.setRouteID('Stapler_00', "short")
             print(traci.vehicle.getRoute('Stapler_00'))
             print(traci.vehicle.getDistance('Stapler_00'))
+		# We assume, that we reach an internal lane at step 130, 
+		# if distance calc is correct, there should be a 
+		# 10m distance difference between the
+		# output of step 129 and 130
+        if step in (129,130): 
+            print(step, round(traci.vehicle.getDistance('Stapler_00'),2))
     traci.close()