From 4d8172f588b74999af79fac5e5aa033745707822 Mon Sep 17 00:00:00 2001 From: Michele Segata Date: Tue, 30 Apr 2024 17:43:40 +0200 Subject: [PATCH] MSCFModel_CC: avoid invalid pointer references in special auto-feed and lane change cases (leader leaving the simulation) --- src/microsim/cfmodels/CC_VehicleVariables.cpp | 2 +- src/microsim/cfmodels/CC_VehicleVariables.h | 4 ++ src/microsim/cfmodels/MSCFModel_CC.cpp | 55 ++++++++++++++----- src/microsim/cfmodels/MSCFModel_CC.h | 11 +++- 4 files changed, 57 insertions(+), 15 deletions(-) diff --git a/src/microsim/cfmodels/CC_VehicleVariables.cpp b/src/microsim/cfmodels/CC_VehicleVariables.cpp index 77edff4794da..d125a5f84a7a 100644 --- a/src/microsim/cfmodels/CC_VehicleVariables.cpp +++ b/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), diff --git a/src/microsim/cfmodels/CC_VehicleVariables.h b/src/microsim/cfmodels/CC_VehicleVariables.h index 62875538efef..8a63aa09bd5f 100644 --- a/src/microsim/cfmodels/CC_VehicleVariables.h +++ b/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; diff --git a/src/microsim/cfmodels/MSCFModel_CC.cpp b/src/microsim/cfmodels/MSCFModel_CC.cpp index 90b444e44dcc..3ee4b710b251 100644 --- a/src/microsim/cfmodels/MSCFModel_CC.cpp +++ b/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 state = libsumo::Vehicle::getLaneChangeState(veh->getID(), left ? +1 : -1); @@ -255,10 +260,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(MSNet::getInstance()->getVehicleControl().getVehicle(id)); +} + double MSCFModel_CC::finalizeSpeed(MSVehicle* const veh, double vPos) const { double vNext; @@ -419,6 +433,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: @@ -814,25 +836,30 @@ void MSCFModel_CC::setParameter(MSVehicle* veh, const std::string& key, const st buf >> id >> position; vars->members[position] = id; - auto vehicle = dynamic_cast(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(&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(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(&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; } @@ -982,16 +1009,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(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(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; @@ -1167,7 +1196,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(); } } diff --git a/src/microsim/cfmodels/MSCFModel_CC.h b/src/microsim/cfmodels/MSCFModel_CC.h index 286a05c35e6c..18b4fc682ef2 100644 --- a/src/microsim/cfmodels/MSCFModel_CC.h +++ b/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 */