MSCFModel_CC: avoid invalid pointer references in special auto-feed a…

…nd lane change cases (leader leaving the simulation)

src/microsim/cfmodels/CC_VehicleVariables.cpp

@@ -45,7 +45,7 @@ const double CC_VehicleVariables::defaultH[MAX_N_CARS] = {0.8, 0.8, 0.8, 0.8, 0.
 CC_VehicleVariables::CC_VehicleVariables() :
     controllerAcceleration(0), frontSpeed(0), frontAcceleration(0),
     frontControllerAcceleration(0), frontDataReadTime(0), frontAngle(0), frontInitialized(false),
-    autoFeed(false), leaderVehicle(0), frontVehicle(0),
+    autoFeed(false), leaderVehicle(nullptr), leaderVehicleId(""), frontVehicle(nullptr), frontVehicleId(""),
     isLeader(true),
     accHeadwayTime(1.5), accLambda(0.1),
     useControllerAcceleration(true), leaderSpeed(0),

src/microsim/cfmodels/CC_VehicleVariables.h

@@ -106,8 +106,12 @@ class CC_VehicleVariables : public MSCFModel::VehicleVariables {
     bool autoFeed;
     /// @brief leader vehicle, used for auto feeding
     MSVehicle* leaderVehicle;
+    /// @brief sumo id of the leader vehicle
+    std::string leaderVehicleId;
     /// @brief front sumo id, used for auto feeding
     MSVehicle* frontVehicle;
+    /// @brief sumo id of the front vehicle
+    std::string frontVehicleId;
     /// @brief whether this vehicle is leader of a platoon or not. This is mainly used by the lane change logic.
     /// By default this is true as a single vehicle is treated as a 1-vehicle platoon
     bool isLeader;

src/microsim/cfmodels/MSCFModel_CC.cpp

@@ -109,9 +109,10 @@ MSCFModel_CC::createVehicleVariables() const {
 }
 
 void
-MSCFModel_CC::setLeader(MSVehicle* veh, MSVehicle* const leader) const {
+MSCFModel_CC::setLeader(MSVehicle* veh, MSVehicle* const leader, std::string leaderId) const {
     auto* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
     vars->leaderVehicle = leader;
+    vars->leaderVehicleId = leaderId;
     if (leader != nullptr)
         vars->isLeader = false;
     else
@@ -123,7 +124,11 @@ int
 MSCFModel_CC::isPlatoonLaneChangeSafe(const MSVehicle* veh, bool left) const {
     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
     if (!vars->isLeader) {
-        return isPlatoonLaneChangeSafe(vars->leaderVehicle, left);
+        // before asking the leader, be sure it is still in the simulation
+        if (findVehicle(vars->leaderVehicleId))
+            return isPlatoonLaneChangeSafe(vars->leaderVehicle, left);
+        else
+            return LCA_BLOCKED;
     }
     int result = 0;
     std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->getID(), left ? +1 : -1);
@@ -258,10 +263,19 @@ MSCFModel_CC::commitToLaneChange(const MSVehicle* veh, bool left) const {
         }
     }
     else {
-        return commitToLaneChange(vars->leaderVehicle, left);
+        // before asking the leader, be sure it is still in the simulation
+        if (findVehicle(vars->leaderVehicleId))
+            return commitToLaneChange(vars->leaderVehicle, left);
+        else
+            return LCA_BLOCKED;
     }
 }
 
+MSVehicle*
+MSCFModel_CC::findVehicle(std::string id) const {
+    return dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(id));
+}
+
 double
 MSCFModel_CC::finalizeSpeed(MSVehicle* const veh, double vPos) const {
     double vNext;
@@ -422,6 +436,14 @@ MSCFModel_CC::_v(const MSVehicle* const veh, double gap2pred, double egoSpeed, d
     if (vars->crashed) {
         return 0;
     }
+    if (vars->autoFeed) {
+        if (!findVehicle(vars->leaderVehicleId) || !findVehicle(vars->frontVehicleId)) {
+            // either the leader or the front vehicle have left the simulation. Disable auto feed
+            vars->autoFeed = false;
+            vars->leaderVehicle = nullptr;
+            vars->frontVehicle = nullptr;
+        }
+    }
     if (vars->activeController == Plexe::DRIVER || !vars->useFixedAcceleration) {
         switch (vars->activeController) {
             case Plexe::ACC:
@@ -817,25 +839,30 @@ void MSCFModel_CC::setParameter(MSVehicle* veh, const std::string& key, const st
             buf >> id >> position;
             vars->members[position] = id;
 
-            auto vehicle = dynamic_cast<MSVehicle*>(libsumo::Helper::getVehicle(id));
+            auto vehicle = findVehicle(id);
+            if (!vehicle) {
+                throw libsumo::TraCIException("Adding " + id + " as member but " + id + " does not exists");
+            }
             auto cfm = dynamic_cast<const MSCFModel_CC*>(&vehicle->getVehicleType().getCarFollowModel());
             if (!cfm) {
                 throw libsumo::TraCIException("Adding " + id + " as member but " + id + " is not using MSCFModel_CC");
             }
-            cfm->setLeader(vehicle, veh);
+            cfm->setLeader(vehicle, veh, veh->getID());
             vars->isLeader = true;
             return;
         }
         if (key.compare(PAR_REMOVE_MEMBER) == 0) {
             for (auto item = vars->members.begin(); item != vars->members.end(); item++)
                 if (item->second.compare(value) == 0) {
-                    auto vehicle = dynamic_cast<MSVehicle*>(libsumo::Helper::getVehicle(value));
+                    auto vehicle = findVehicle(value);
+                    if (!vehicle) {
+                        throw libsumo::TraCIException("Removing " + value + " from members but " + value + " does not exist");
+                    }
                     auto cfm = dynamic_cast<const MSCFModel_CC*>(&vehicle->getVehicleType().getCarFollowModel());
                     if (!cfm) {
                         throw libsumo::TraCIException("Removing " + value + " from members but " + value + " is not using MSCFModel_CC");
                     }
-                    cfm->setLeader(vehicle, nullptr);
-
+                    cfm->setLeader(vehicle, nullptr, "");
                     vars->members.erase(item);
                     break;
                 }
@@ -987,16 +1014,18 @@ void MSCFModel_CC::setParameter(MSVehicle* veh, const std::string& key, const st
                 if (buf.last_empty()) {
                     throw InvalidArgument("Trying to enable auto feeding without providing leader vehicle id");
                 }
-                vars->leaderVehicle = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(leaderId));
-                if (vars->leaderVehicle == 0) {
+                vars->leaderVehicleId = leaderId;
+                vars->leaderVehicle = findVehicle(leaderId);
+                if (!vars->leaderVehicle) {
                     throw libsumo::TraCIException("Vehicle '" + leaderId + "' is not known");
                 }
                 buf >> frontId;
                 if (buf.last_empty()) {
                     throw InvalidArgument("Trying to enable auto feeding without providing front vehicle id");
                 }
-                vars->frontVehicle = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(frontId));
-                if (vars->frontVehicle == 0) {
+                vars->frontVehicleId = frontId;
+                vars->frontVehicle = findVehicle(frontId);
+                if (!vars->frontVehicle) {
                     throw libsumo::TraCIException("Vehicle '" + frontId + "' is not known");
                 }
                 vars->leaderInitialized = true;
@@ -1148,7 +1177,7 @@ void MSCFModel_CC::getRadarMeasurements(const MSVehicle* veh, double& distance,
         relativeSpeed = 0;
     } else {
         distance = l.second;
-        SUMOVehicle* leader = MSNet::getInstance()->getVehicleControl().getVehicle(l.first);
+        SUMOVehicle* leader = findVehicle(l.first);
         relativeSpeed = leader->getSpeed() - veh->getSpeed();
     }
 }

src/microsim/cfmodels/MSCFModel_CC.h

@@ -266,8 +266,9 @@ class MSCFModel_CC : public MSCFModel {
      * @brief Sets the leader for a member of the platoon
      * @param veh platoon member
      * @param leader platoon leader
+     * @param leaderId platoon leader id
      */
-    void setLeader(MSVehicle* veh, MSVehicle* const leader) const;
+    void setLeader(MSVehicle* veh, MSVehicle* const leader, std::string leaderId) const;
 
     /**
      * @brief Returns whether a vehicle is a leader of a platoon or not. By default, a vehicle on its own using an ACC
@@ -292,6 +293,14 @@ class MSCFModel_CC : public MSCFModel {
      */
     int commitToLaneChange(const MSVehicle* veh, bool left) const;
 
+    /**
+     * Searches for a vehicle given its sumo id. Differently from libsumo's getVehicle, this call does not throw an
+     * exception if the vehicle does not exist, so we can check for its existance
+     * @param id sumo vehicle id
+     * @return a pointer to the vehicle if found, nullptr otherwise
+     */
+    MSVehicle* findVehicle(std::string id) const;
+
     /**
      * @brief returns the number of lanes set in the configuration file
      */