Meso-fcd output position interpolate in all outputs

data/xsd/fcd_file.xsd

@@ -31,6 +31,7 @@
             <xsd:attribute name="type" type="xsd:string" use="optional"/>
 
             <xsd:attribute name="speed" type="nonNegativeFloatType" use="optional"/>
+            <xsd:attribute name="speedRelative" type="nonNegativeFloatType" use="optional"/>
             <xsd:attribute name="pos" type="nonNegativeFloatType" use="optional"/>
             <xsd:attribute name="lane" type="xsd:string" use="optional"/>
             <xsd:attribute name="edge" type="xsd:string" use="optional"/>
@@ -63,6 +64,7 @@
         <xsd:attribute name="type" use="optional" type="xsd:string"/>
 
         <xsd:attribute name="speed" type="nonNegativeFloatType" use="optional"/>
+        <xsd:attribute name="speedRelative" type="nonNegativeFloatType" use="optional"/>
         <xsd:attribute name="pos" type="nonNegativeFloatType" use="optional"/>
         <xsd:attribute name="edge" type="xsd:string" use="optional"/>
         <xsd:attribute name="slope" type="floatType" use="optional"/>

src/libsumo/Vehicle.cpp

@@ -46,6 +46,8 @@
 #include <microsim/devices/MSDevice_Taxi.h>
 #include <microsim/devices/MSDispatch_TraCI.h>
 #include <mesosim/MEVehicle.h>
+#include <mesosim/MESegment.h>
+#include <mesosim/MELoop.h>
 #include <libsumo/StorageHelper.h>
 #include <libsumo/TraCIDefs.h>
 #include <libsumo/TraCIConstants.h>
@@ -65,7 +67,61 @@
 //#define DEBUG_MOVEXY
 #define DEBUG_COND (veh->isSelected())
 
+// ===========================================================================
+// Helper functions for meso interpolation
+// ===========================================================================
+
+// Helper function to get interpolated position for mesoscopic vehicles
+static Position
+getInterpolatedMesoPosition(const MEVehicle* mesoVeh) {
+    const MESegment* segment = mesoVeh->getSegment();
+    if (segment == nullptr || mesoVeh->getQueIndex() == MESegment::PARKING_QUEUE) {
+        return mesoVeh->getPosition();
+    }
+
+    const double now = SIMTIME;
+    const double intendedLeave = MIN2(mesoVeh->getEventTimeSeconds(), mesoVeh->getBlockTimeSeconds());
+    const double entry = mesoVeh->getLastEntryTimeSeconds();
+
+    // Calculate segment offset (position of segment start)
+    double segmentOffset = 0;
+    for (MESegment* seg = MSGlobals::gMesoNet->getSegmentForEdge(*mesoVeh->getEdge());
+            seg != nullptr && seg != segment; seg = seg->getNextSegment()) {
+        segmentOffset += seg->getLength();
+    }
+
+    // Calculate interpolated position within segment
+    const double length = segment->getLength();
+    const double relPos = segmentOffset + length * (now - entry) / (intendedLeave - entry);
+
+    // Convert to cartesian coordinates
+    const MSLane* const lane = mesoVeh->getEdge()->getLanes()[0];
+    return lane->geometryPositionAtOffset(relPos);
+}
+
+// Helper function to get interpolated position on lane for mesoscopic vehicles
+static double
+getInterpolatedMesoPositionOnLane(const MEVehicle* mesoVeh) {
+    const MESegment* segment = mesoVeh->getSegment();
+    if (segment == nullptr || mesoVeh->getQueIndex() == MESegment::PARKING_QUEUE) {
+        return mesoVeh->getPositionOnLane();
+    }
 
+    const double now = SIMTIME;
+    const double intendedLeave = MIN2(mesoVeh->getEventTimeSeconds(), mesoVeh->getBlockTimeSeconds());
+    const double entry = mesoVeh->getLastEntryTimeSeconds();
+
+    // Calculate segment offset (position of segment start)
+    double segmentOffset = 0;
+    for (MESegment* seg = MSGlobals::gMesoNet->getSegmentForEdge(*mesoVeh->getEdge());
+            seg != nullptr && seg != segment; seg = seg->getNextSegment()) {
+        segmentOffset += seg->getLength();
+    }
+
+    // Calculate interpolated position within segment
+    const double length = segment->getLength();
+    return segmentOffset + length * (now - entry) / (intendedLeave - entry);
+}
 
 namespace libsumo {
 // ===========================================================================
@@ -140,7 +196,15 @@ TraCIPosition
 Vehicle::getPosition(const std::string& vehID, const bool includeZ) {
     MSBaseVehicle* veh = Helper::getVehicle(vehID);
     if (isVisible(veh)) {
-        return Helper::makeTraCIPosition(veh->getPosition(), includeZ);
+        Position pos;
+        // Use interpolated position for meso vehicles
+        MEVehicle* mesoVeh = dynamic_cast<MEVehicle*>(veh);
+        if (mesoVeh != nullptr) {
+            pos = getInterpolatedMesoPosition(mesoVeh);
+        } else {
+            pos = veh->getPosition();
+        }
+        return Helper::makeTraCIPosition(pos, includeZ);
     }
     return TraCIPosition();
 }
@@ -248,7 +312,16 @@ Vehicle::getColor(const std::string& vehID) {
 double
 Vehicle::getLanePosition(const std::string& vehID) {
     MSBaseVehicle* veh = Helper::getVehicle(vehID);
-    return (veh->isOnRoad() || veh->isParking()) ? veh->getPositionOnLane() : INVALID_DOUBLE_VALUE;
+    if (veh->isOnRoad() || veh->isParking()) {
+        // Use interpolated position for meso vehicles
+        MEVehicle* mesoVeh = dynamic_cast<MEVehicle*>(veh);
+        if (mesoVeh != nullptr) {
+            return getInterpolatedMesoPositionOnLane(mesoVeh);
+        } else {
+            return veh->getPositionOnLane();
+        }
+    }
+    return INVALID_DOUBLE_VALUE;
 }
 
 double
@@ -704,9 +777,12 @@ double
 Vehicle::getDrivingDistance(const std::string& vehID, const std::string& edgeID, double pos, int laneIndex) {
     MSBaseVehicle* veh = Helper::getVehicle(vehID);
     MSVehicle* microVeh = dynamic_cast<MSVehicle*>(veh);
+    MEVehicle* mesoVeh = dynamic_cast<MEVehicle*>(veh);
     if (veh->isOnRoad()) {
         const MSLane* lane = microVeh != nullptr ? veh->getLane() : veh->getEdge()->getLanes()[0];
-        double distance = veh->getRoute().getDistanceBetween(veh->getPositionOnLane(), pos,
+        // Use interpolated position for meso vehicles
+        double lanePos = mesoVeh != nullptr ? getInterpolatedMesoPositionOnLane(mesoVeh) : veh->getPositionOnLane();
+        double distance = veh->getRoute().getDistanceBetween(lanePos, pos,
                           lane, Helper::getLaneChecking(edgeID, laneIndex, pos), veh->getRoutePosition());
         if (distance == std::numeric_limits<double>::max()) {
             return INVALID_DOUBLE_VALUE;
@@ -726,9 +802,12 @@ Vehicle::getDrivingDistance2D(const std::string& vehID, double x, double y) {
     }
     if (veh->isOnRoad()) {
         MSVehicle* microVeh = dynamic_cast<MSVehicle*>(veh);
+        MEVehicle* mesoVeh = dynamic_cast<MEVehicle*>(veh);
         const MSLane* lane = microVeh != nullptr ? veh->getLane() : veh->getEdge()->getLanes()[0];
         std::pair<MSLane*, double> roadPos = Helper::convertCartesianToRoadMap(Position(x, y), veh->getVehicleType().getVehicleClass());
-        double distance = veh->getRoute().getDistanceBetween(veh->getPositionOnLane(), roadPos.second,
+        // Use interpolated position for meso vehicles
+        double lanePos = mesoVeh != nullptr ? getInterpolatedMesoPositionOnLane(mesoVeh) : veh->getPositionOnLane();
+        double distance = veh->getRoute().getDistanceBetween(lanePos, roadPos.second,
                           lane, roadPos.first, veh->getRoutePosition());
         if (distance == std::numeric_limits<double>::max()) {
             return INVALID_DOUBLE_VALUE;

src/microsim/MSFrame.cpp

@@ -173,6 +173,8 @@ MSFrame::fillOptions() {
     oc.addDescription("fcd-output.distance", "Output", TL("Add kilometrage to the FCD output (linear referencing)"));
     oc.doRegister("fcd-output.acceleration", new Option_Bool(false));
     oc.addDescription("fcd-output.acceleration", "Output", TL("Add acceleration to the FCD output"));
+    oc.doRegister("fcd-output.speed-relative", new Option_Bool(false));
+    oc.addDescription("fcd-output.speed-relative", "Output", TL("Add relative speed (vehicle speed / edge speed limit) to the FCD output"));
     oc.doRegister("fcd-output.max-leader-distance", new Option_Float(-1));
     oc.addDescription("fcd-output.max-leader-distance", "Output", TL("Add leader vehicle information to the FCD output (within the given distance)"));
     oc.doRegister("fcd-output.params", new Option_StringVector());

src/microsim/devices/MSDevice_FCD.cpp

@@ -203,6 +203,7 @@ MSDevice_FCD::initOnce() {
     misc.set(SUMO_ATTR_ODOMETER);
     misc.set(SUMO_ATTR_POSITION_LAT);
     misc.set(SUMO_ATTR_SPEED_LAT);
+    misc.set(SUMO_ATTR_SPEEDREL);
     misc.set(SUMO_ATTR_LEADER_ID);
     misc.set(SUMO_ATTR_LEADER_SPEED);
     misc.set(SUMO_ATTR_ARRIVALDELAY);
@@ -233,6 +234,9 @@ MSDevice_FCD::initOnce() {
     if (oc.getBool("fcd-output.distance")) {
         myWrittenAttributes.set(SUMO_ATTR_DISTANCE);
     }
+    if (oc.getBool("fcd-output.speed-relative")) {
+        myWrittenAttributes.set(SUMO_ATTR_SPEEDREL);
+    }
 
     if (oc.isSet("fcd-output.filter-shapes")) {
         // build the shape filter if it is desired

src/microsim/output/MSFCDExport.cpp

@@ -40,6 +40,8 @@
 #include <microsim/transportables/MSPerson.h>
 #include <microsim/transportables/MSTransportableControl.h>
 #include <microsim/MSVehicleControl.h>
+#include <mesosim/MESegment.h>
+#include <mesosim/MELoop.h>
 #include <mesosim/MEVehicle.h>
 #include "MSEmissionExport.h"
 #include "MSFCDExport.h"
@@ -48,6 +50,35 @@
 // ===========================================================================
 // method definitions
 // ===========================================================================
+
+// Helper function to get interpolated position for mesoscopic vehicles
+static Position
+getInterpolatedMesoPosition(const MEVehicle* mesoVeh) {
+    const MESegment* segment = mesoVeh->getSegment();
+    if (segment == nullptr) {
+        return mesoVeh->getPosition();
+    }
+
+    const double now = SIMTIME;
+    const double intendedLeave = MIN2(mesoVeh->getEventTimeSeconds(), mesoVeh->getBlockTimeSeconds());
+    const double entry = mesoVeh->getLastEntryTimeSeconds();
+
+    // Calculate segment offset (position of segment start)
+    double segmentOffset = 0;
+    for (MESegment* seg = MSGlobals::gMesoNet->getSegmentForEdge(*mesoVeh->getEdge());
+            seg != nullptr && seg != segment; seg = seg->getNextSegment()) {
+        segmentOffset += seg->getLength();
+    }
+
+    // Calculate interpolated position within segment
+    const double length = segment->getLength();
+    const double relPos = segmentOffset + length * (now - entry) / (intendedLeave - entry);
+
+    // Convert to cartesian coordinates
+    const MSLane* const lane = mesoVeh->getEdge()->getLanes()[0];
+    return lane->geometryPositionAtOffset(relPos);
+}
+
 void
 MSFCDExport::write(OutputDevice& of, const SUMOTime timestep, const SumoXMLTag tag) {
     MSDevice_FCD::initOnce();
@@ -86,7 +117,13 @@ MSFCDExport::write(OutputDevice& of, const SUMOTime timestep, const SumoXMLTag t
             const bool hasOutput = (tag == SUMO_TAG_NOTHING || tag == SUMO_TAG_VEHICLE) && hasOwnOutput(veh, filter, shapeFilter, (radius > 0 && inRadius.count(veh) > 0));
             if (hasOutput) {
                 const MSVehicle* const microVeh = MSGlobals::gUseMesoSim ? nullptr : static_cast<const MSVehicle*>(veh);
-                Position pos = veh->getPosition();
+                Position pos;
+                if (MSGlobals::gUseMesoSim) {
+                    const MEVehicle* mesoVeh = static_cast<const MEVehicle*>(veh);
+                    pos = getInterpolatedMesoPosition(mesoVeh);
+                } else {
+                    pos = veh->getPosition();
+                }
                 if (useGeo) {
                     of.setPrecision(gPrecisionGeo);
                     GeoConvHelper::getFinal().cartesian2geo(pos);
@@ -106,8 +143,35 @@ MSFCDExport::write(OutputDevice& of, const SUMOTime timestep, const SumoXMLTag t
                 of.writeFuncAttr(SUMO_ATTR_SPEED, [ = ]() {
                     return veh->getSpeed();
                 }, mask);
+                of.writeFuncAttr(SUMO_ATTR_SPEEDREL, [ = ]() {
+                    const double speedLimit = veh->getEdge()->getSpeedLimit();
+                    return speedLimit > 0 ? veh->getSpeed() / speedLimit : 0.;
+                }, mask);
                 of.writeFuncAttr(SUMO_ATTR_POSITION, [ = ]() {
-                    return veh->getPositionOnLane();
+                    if (MSGlobals::gUseMesoSim) {
+                        const MEVehicle* mesoVeh = static_cast<const MEVehicle*>(veh);
+                        const MESegment* segment = mesoVeh->getSegment();
+                        if (segment == nullptr) {
+                            return veh->getPositionOnLane();
+                        }
+
+                        const double now = SIMTIME;
+                        const double intendedLeave = MIN2(mesoVeh->getEventTimeSeconds(), mesoVeh->getBlockTimeSeconds());
+                        const double entry = mesoVeh->getLastEntryTimeSeconds();
+
+                        // Calculate segment offset (position of segment start)
+                        double segmentOffset = 0;
+                        for (MESegment* seg = MSGlobals::gMesoNet->getSegmentForEdge(*mesoVeh->getEdge());
+                                seg != nullptr && seg != segment; seg = seg->getNextSegment()) {
+                            segmentOffset += seg->getLength();
+                        }
+
+                        // Calculate interpolated position within segment
+                        const double length = segment->getLength();
+                        return segmentOffset + length * (now - entry) / (intendedLeave - entry);
+                    } else {
+                        return veh->getPositionOnLane();
+                    }
                 }, mask);
                 of.writeFuncAttr(SUMO_ATTR_LANE, [ = ]() {
                     return MSGlobals::gUseMesoSim ? "" : microVeh->getLane()->getID();
@@ -130,10 +194,34 @@ MSFCDExport::write(OutputDevice& of, const SUMOTime timestep, const SumoXMLTag t
                     }, mask);
                 }
                 of.writeFuncAttr(SUMO_ATTR_DISTANCE, [ = ]() {
-                    double lanePos = veh->getPositionOnLane();
-                    if (!MSGlobals::gUseMesoSim && microVeh->getLane()->isInternal()) {
-                        lanePos = microVeh->getRoute().getDistanceBetween(0., lanePos, microVeh->getEdge()->getLanes()[0], microVeh->getLane(),
-                                  microVeh->getRoutePosition());
+                    double lanePos;
+                    if (MSGlobals::gUseMesoSim) {
+                        const MEVehicle* mesoVeh = static_cast<const MEVehicle*>(veh);
+                        const MESegment* segment = mesoVeh->getSegment();
+                        if (segment == nullptr) {
+                            lanePos = veh->getPositionOnLane();
+                        } else {
+                            const double now = SIMTIME;
+                            const double intendedLeave = MIN2(mesoVeh->getEventTimeSeconds(), mesoVeh->getBlockTimeSeconds());
+                            const double entry = mesoVeh->getLastEntryTimeSeconds();
+
+                            // Calculate segment offset (position of segment start)
+                            double segmentOffset = 0;
+                            for (MESegment* seg = MSGlobals::gMesoNet->getSegmentForEdge(*mesoVeh->getEdge());
+                                    seg != nullptr && seg != segment; seg = seg->getNextSegment()) {
+                                segmentOffset += seg->getLength();
+                            }
+
+                            // Calculate interpolated position within segment
+                            const double length = segment->getLength();
+                            lanePos = segmentOffset + length * (now - entry) / (intendedLeave - entry);
+                        }
+                    } else {
+                        lanePos = veh->getPositionOnLane();
+                        if (microVeh->getLane()->isInternal()) {
+                            lanePos = microVeh->getRoute().getDistanceBetween(0., lanePos, microVeh->getEdge()->getLanes()[0], microVeh->getLane(),
+                                      microVeh->getRoutePosition());
+                        }
                     }
                     return veh->getEdge()->getDistanceAt(lanePos);
                 }, mask);
@@ -294,6 +382,9 @@ MSFCDExport::writeTransportable(OutputDevice& of, const MSEdge* const e, const M
     of.writeOptionalAttr(SUMO_ATTR_ANGLE, GeomHelper::naviDegree(p->getAngle()), mask);
     of.writeOptionalAttr(SUMO_ATTR_TYPE, p->getVehicleType().getID(), mask);
     of.writeOptionalAttr(SUMO_ATTR_SPEED, p->getSpeed(), mask);
+    // Calculate relative speed for transportables based on edge speed limit
+    const double speedLimit = e->getSpeedLimit();
+    of.writeOptionalAttr(SUMO_ATTR_SPEEDREL, speedLimit > 0 ? p->getSpeed() / speedLimit : 0., mask);
     of.writeOptionalAttr(SUMO_ATTR_POSITION, p->getEdgePos(), mask);
     of.writeOptionalAttr(SUMO_ATTR_LANE, "", mask, true);
     of.writeOptionalAttr(SUMO_ATTR_EDGE, e->getID(), mask);
@@ -304,4 +395,4 @@ MSFCDExport::writeTransportable(OutputDevice& of, const MSEdge* const e, const M
 }
 
 
-/****************************************************************************/
+/****************************************************************************/