1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file    MSCFModel_CC.cpp
11 /// @author  Michele Segata
12 /// @date    Wed, 18 Apr 2012
13 /// @version $Id$
14 ///
15 // A series of automatic Cruise Controllers (CC, ACC, CACC)
16 /****************************************************************************/
17 
18 
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include "MSCFModel_CC.h"
23 #include <microsim/MSVehicle.h>
24 #include <microsim/MSVehicleControl.h>
25 #include <microsim/MSNet.h>
26 #include <microsim/MSEdge.h>
27 #include <utils/common/RandHelper.h>
28 #include <utils/common/SUMOTime.h>
29 #include <utils/common/StringUtils.h>
30 #include <microsim/cfmodels/ParBuffer.h>
31 #include <libsumo/Vehicle.h>
32 #include <libsumo/TraCIDefs.h>
33 #include <algorithm>
34 
35 #ifndef sgn
36 #define sgn(x) ((x > 0) - (x < 0))
37 #endif
38 
39 // ===========================================================================
40 // method definitions
41 // ===========================================================================
MSCFModel_CC(const MSVehicleType * vtype)42 MSCFModel_CC::MSCFModel_CC(const MSVehicleType* vtype) : MSCFModel(vtype),
43     myCcDecel(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CCDECEL, 1.5)),
44     myCcAccel(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CCACCEL, 1.5)),
45     myConstantSpacing(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CONSTSPACING, 5.0)),
46     myKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_KP, 1.0)),
47     myLambda(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_LAMBDA, 0.1)),
48     myC1(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_C1, 0.5)),
49     myXi(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_XI, 1.0)),
50     myOmegaN(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_OMEGAN, 0.2)),
51     myTau(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_TAU, 0.5)),
52     myLanesCount((int)vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_LANES_COUNT, -1)),
53     myPloegH(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_H, 0.5)),
54     myPloegKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_KP, 0.2)),
55     myPloegKd(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_KD, 0.7)),
56     myFlatbedKa(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KA, 2.4)),
57     myFlatbedKv(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KV, 0.6)),
58     myFlatbedKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KP, 12.0)),
59     myFlatbedH(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_H, 4.0)),
60     myFlatbedD(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_D, 5.0)) {
61 
62     //if the lanes count has not been specified in the attributes of the model, lane changing cannot properly work
63     if (myLanesCount == -1) {
64         std::cerr << "The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute\n";
65         WRITE_ERROR("The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute");
66         assert(false);
67     }
68 
69     //instantiate the driver model. For now, use Krauss as default, then needs to be parameterized
70     myHumanDriver = new MSCFModel_Krauss(vtype);
71 
72 }
73 
~MSCFModel_CC()74 MSCFModel_CC::~MSCFModel_CC() {}
75 
76 MSCFModel::VehicleVariables*
createVehicleVariables() const77 MSCFModel_CC::createVehicleVariables() const {
78     CC_VehicleVariables* vars = new CC_VehicleVariables();
79     vars->ccKp = myKp;
80     vars->accLambda = myLambda;
81     vars->caccSpacing = myConstantSpacing;
82     vars->caccC1 = myC1;
83     vars->caccXi = myXi;
84     vars->caccOmegaN = myOmegaN;
85     vars->engineTau = myTau;
86     //we cannot invoke recomputeParameters() because we have no pointer to the MSVehicle class
87     vars->caccAlpha1 = 1 - vars->caccC1;
88     vars->caccAlpha2 = vars->caccC1;
89     vars->caccAlpha3 = -(2 * vars->caccXi - vars->caccC1 * (vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1))) * vars->caccOmegaN;
90     vars->caccAlpha4 = -(vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1)) * vars->caccOmegaN * vars->caccC1;
91     vars->caccAlpha5 = -vars->caccOmegaN * vars->caccOmegaN;
92 
93     vars->ploegH = myPloegH;
94     vars->ploegKp = myPloegKp;
95     vars->ploegKd = myPloegKd;
96     vars->flatbedKa = myFlatbedKa;
97     vars->flatbedKv = myFlatbedKv;
98     vars->flatbedKp = myFlatbedKp;
99     vars->flatbedD = myFlatbedD;
100     vars->flatbedH = myFlatbedH;
101     //by default use a first order lag model for the engine
102     vars->engine = new FirstOrderLagModel();
103     vars->engine->setParameter(FOLM_PAR_TAU, vars->engineTau);
104     vars->engine->setParameter(FOLM_PAR_DT, TS);
105     vars->engine->setMaximumAcceleration(myAccel);
106     vars->engine->setMaximumDeceleration(myDecel);
107     vars->engineModel = CC_ENGINE_MODEL_FOLM;
108     return (VehicleVariables*)vars;
109 }
110 
111 void
performAutoLaneChange(MSVehicle * const veh) const112 MSCFModel_CC::performAutoLaneChange(MSVehicle* const veh) const {
113     bool canChange;
114     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
115     // check for left lane change
116     std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->getID(), +1);
117     int traciState = state.first;
118     if (traciState & LCA_LEFT && traciState & LCA_SPEEDGAIN) {
119         // we can gain by moving left. check that all vehicles can move left
120         if (!(state.first & LCA_BLOCKED)) {
121             // leader is not blocked. check all the members
122             canChange = true;
123             for (auto m = vars->members.begin(); m != vars->members.end(); m++) {
124                 const std::pair<int, int> mState = libsumo::Vehicle::getLaneChangeState(m->second, +1);
125                 if (mState.first & LCA_BLOCKED) {
126                     canChange = false;
127                     break;
128                 }
129             }
130             if (canChange) {
131                 libsumo::Vehicle::changeLane(veh->getID(), veh->getLaneIndex() + 1, 0);
132                 for (auto m = vars->members.begin(); m != vars->members.end(); m++) {
133                     libsumo::Vehicle::changeLane(m->second, veh->getLaneIndex() + 1, 0);
134                 }
135             }
136 
137         }
138     }
139     state = libsumo::Vehicle::getLaneChangeState(veh->getID(), -1);
140     traciState = state.first;
141     if (traciState & LCA_RIGHT && traciState & LCA_KEEPRIGHT) {
142         // we should move back right. check that all vehicles can move right
143         if (!(state.first & LCA_BLOCKED)) {
144             // leader is not blocked. check all the members
145             canChange = true;
146             for (auto m = vars->members.begin(); m != vars->members.end(); m++) {
147                 const std::pair<int, int> mState = libsumo::Vehicle::getLaneChangeState(m->second, -1);
148                 if (mState.first & LCA_BLOCKED) {
149                     canChange = false;
150                     break;
151                 }
152             }
153             if (canChange) {
154                 libsumo::Vehicle::changeLane(veh->getID(), veh->getLaneIndex() - 1, 1);
155                 for (auto m = vars->members.begin(); m != vars->members.end(); m++) {
156                     libsumo::Vehicle::changeLane(m->second, veh->getLaneIndex() - 1, 1);
157                 }
158             }
159 
160         }
161     }
162 
163 }
164 
165 double
finalizeSpeed(MSVehicle * const veh,double vPos) const166 MSCFModel_CC::finalizeSpeed(MSVehicle* const veh, double vPos) const {
167     double vNext;
168     //acceleration computed by the controller
169     double controllerAcceleration;
170     //acceleration after engine actuation
171     double engineAcceleration;
172 
173     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
174 
175     //call processNextStop() to ensure vehicle removal in case of crash
176     veh->processNextStop(vPos);
177 
178     if (vars->activeController != Plexe::DRIVER) {
179         veh->setChosenSpeedFactor(vars->ccDesiredSpeed / veh->getLane()->getSpeedLimit());
180     }
181 
182     if (vars->autoLaneChange) {
183         performAutoLaneChange(veh);
184     }
185 
186     if (vars->activeController != Plexe::DRIVER) {
187         controllerAcceleration = SPEED2ACCEL(vPos - veh->getSpeed());
188         controllerAcceleration = std::min(vars->uMax, std::max(vars->uMin, controllerAcceleration));
189         //compute the actual acceleration applied by the engine
190         engineAcceleration = vars->engine->getRealAcceleration(veh->getSpeed(), veh->getAcceleration(), controllerAcceleration, MSNet::getInstance()->getCurrentTimeStep());
191         vNext = MAX2(double(0), veh->getSpeed() + ACCEL2SPEED(engineAcceleration));
192         vars->controllerAcceleration = controllerAcceleration;
193     } else {
194         vNext = myHumanDriver->finalizeSpeed(veh, vPos);
195     }
196 
197     return vNext;
198 }
199 
200 
201 double
followSpeed(const MSVehicle * const veh,double speed,double gap2pred,double predSpeed,double predMaxDecel,const MSVehicle * const pred) const202 MSCFModel_CC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred) const {
203 
204     UNUSED_PARAMETER(pred);
205     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
206 
207     if (vars->activeController != Plexe::DRIVER) {
208         return _v(veh, gap2pred, speed, predSpeed);
209     } else {
210         return myHumanDriver->followSpeed(veh, speed, gap2pred, predSpeed, predMaxDecel);
211     }
212 }
213 
214 double
insertionFollowSpeed(const MSVehicle * const veh,double speed,double gap2pred,double predSpeed,double predMaxDecel) const215 MSCFModel_CC::insertionFollowSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel) const {
216     UNUSED_PARAMETER(veh);
217     UNUSED_PARAMETER(gap2pred);
218     UNUSED_PARAMETER(predSpeed);
219     UNUSED_PARAMETER(predMaxDecel);
220     //by returning speed + 1, we tell sumo that "speed" is always a safe speed
221     return speed + 1;
222 }
223 
224 double
stopSpeed(const MSVehicle * const veh,double speed,double gap2pred) const225 MSCFModel_CC::stopSpeed(const MSVehicle* const veh, double speed, double gap2pred) const {
226 
227     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
228     if (vars->activeController != Plexe::DRIVER) {
229         double gap2pred, relSpeed;
230         getRadarMeasurements(veh, gap2pred, relSpeed);
231         if (gap2pred == -1) {
232             gap2pred = std::numeric_limits<double>().max();
233         }
234         return _v(veh, gap2pred, speed, speed + relSpeed);
235     } else {
236         return myHumanDriver->stopSpeed(veh, speed, gap2pred);
237     }
238 }
239 
freeSpeed(const MSVehicle * const veh,double speed,double seen,double maxSpeed,const bool onInsertion) const240 double MSCFModel_CC::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion) const {
241     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
242     if (vars->activeController != Plexe::DRIVER) {
243         double gap2pred, relSpeed;
244         getRadarMeasurements(veh, gap2pred, relSpeed);
245         if (gap2pred == -1) {
246             gap2pred = std::numeric_limits<double>().max();
247         }
248         return _v(veh, gap2pred, speed, speed + relSpeed);
249     } else {
250         return MSCFModel::freeSpeed(veh, speed, seen, maxSpeed, onInsertion);
251     }
252 }
253 
254 double
interactionGap(const MSVehicle * const veh,double vL) const255 MSCFModel_CC::interactionGap(const MSVehicle* const veh, double vL) const {
256 
257     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
258     if (vars->activeController != Plexe::DRIVER) {
259         //maximum radar range is CC is enabled
260         return 250;
261     } else {
262         return myHumanDriver->interactionGap(veh, vL);
263     }
264 
265 }
266 
267 double
maxNextSpeed(double speed,const MSVehicle * const veh) const268 MSCFModel_CC::maxNextSpeed(double speed, const MSVehicle* const veh) const {
269     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
270     if (vars->engineModel == CC_ENGINE_MODEL_FOLM) {
271         return speed + (double) ACCEL2SPEED(getMaxAccel());
272     } else {
273         return speed + (double) ACCEL2SPEED(20);
274     }
275 }
276 
277 double
minNextSpeed(double speed,const MSVehicle * const veh) const278 MSCFModel_CC::minNextSpeed(double speed, const MSVehicle* const veh) const {
279     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
280     if (vars->engineModel == CC_ENGINE_MODEL_FOLM) {
281         return MSCFModel::minNextSpeed(speed, veh);
282     } else {
283         return MAX2((double)0, speed - (double) ACCEL2SPEED(20));
284     }
285 }
286 
287 double
_v(const MSVehicle * const veh,double gap2pred,double egoSpeed,double predSpeed) const288 MSCFModel_CC::_v(const MSVehicle* const veh, double gap2pred, double egoSpeed, double predSpeed) const {
289 
290     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
291 
292     //acceleration computed by the controller
293     double controllerAcceleration = vars->fixedAcceleration;
294     //speed computed by the model
295     double speed;
296     //acceleration computed by the Cruise Control
297     double ccAcceleration;
298     //acceleration computed by the Adaptive Cruise Control
299     double accAcceleration;
300     //acceleration computed by the Cooperative Adaptive Cruise Control
301     double caccAcceleration;
302     //variables needed by CACC
303     double predAcceleration, leaderAcceleration, leaderSpeed;
304     //dummy variables used for auto feeding
305     Position pos;
306     double time;
307     const double currentTime = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() + DELTA_T);
308 
309     if (vars->crashed || vars->crashedVictim) {
310         return 0;
311     }
312     if (vars->activeController == Plexe::DRIVER || !vars->useFixedAcceleration) {
313         switch (vars->activeController) {
314             case Plexe::ACC:
315                 ccAcceleration = _cc(veh, egoSpeed, vars->ccDesiredSpeed);
316                 accAcceleration = _acc(veh, egoSpeed, predSpeed, gap2pred, vars->accHeadwayTime);
317                 if (gap2pred > 250 || ccAcceleration < accAcceleration) {
318                     controllerAcceleration = ccAcceleration;
319                 } else {
320                     controllerAcceleration = accAcceleration;
321                 }
322                 break;
323 
324             case Plexe::CACC:
325                 if (vars->autoFeed) {
326                     getVehicleInformation(vars->leaderVehicle, vars->leaderSpeed, vars->leaderAcceleration, vars->leaderControllerAcceleration, pos, time);
327                     getVehicleInformation(vars->frontVehicle, vars->frontSpeed, vars->frontAcceleration, vars->frontControllerAcceleration, pos, time);
328                 }
329 
330                 if (vars->useControllerAcceleration) {
331                     predAcceleration = vars->frontControllerAcceleration;
332                     leaderAcceleration = vars->leaderControllerAcceleration;
333                 } else {
334                     predAcceleration = vars->frontAcceleration;
335                     leaderAcceleration = vars->leaderAcceleration;
336                 }
337                 //overwrite pred speed using data obtained through wireless communication
338                 predSpeed = vars->frontSpeed;
339                 leaderSpeed = vars->leaderSpeed;
340                 if (vars->usePrediction) {
341                     predSpeed += (currentTime - vars->frontDataReadTime) * vars->frontAcceleration;
342                     leaderSpeed += (currentTime - vars->leaderDataReadTime) * vars->leaderAcceleration;
343                 }
344 
345                 if (vars->caccInitialized) {
346                     controllerAcceleration = _cacc(veh, egoSpeed, predSpeed, predAcceleration, gap2pred, leaderSpeed, leaderAcceleration, vars->caccSpacing);
347                 } else
348                     //do not let CACC take decisions until at least one packet has been received
349                 {
350                     controllerAcceleration = 0;
351                 }
352 
353                 break;
354 
355             case Plexe::FAKED_CACC:
356 
357                 if (vars->autoFeed) {
358                     getVehicleInformation(vars->leaderVehicle, vars->fakeData.leaderSpeed, vars->fakeData.leaderAcceleration, vars->fakeData.leaderControllerAcceleration, pos, time);
359                     getVehicleInformation(vars->frontVehicle, vars->fakeData.frontSpeed, vars->fakeData.frontAcceleration, vars->fakeData.frontControllerAcceleration, pos, time);
360                     vars->fakeData.frontDistance = pos.distanceTo2D(veh->getPosition());
361                 }
362 
363                 if (vars->useControllerAcceleration) {
364                     predAcceleration = vars->fakeData.frontControllerAcceleration;
365                     leaderAcceleration = vars->fakeData.leaderControllerAcceleration;
366                 } else {
367                     predAcceleration = vars->fakeData.frontAcceleration;
368                     leaderAcceleration = vars->fakeData.leaderAcceleration;
369                 }
370                 ccAcceleration = _cc(veh, egoSpeed, vars->ccDesiredSpeed);
371                 caccAcceleration = _cacc(veh, egoSpeed, vars->fakeData.frontSpeed, predAcceleration, vars->fakeData.frontDistance, vars->fakeData.leaderSpeed, leaderAcceleration, vars->caccSpacing);
372                 //faked CACC can be used to get closer to a platoon for joining
373                 //using the minimum acceleration ensures that we do not exceed
374                 //the CC desired speed
375                 controllerAcceleration = std::min(ccAcceleration, caccAcceleration);
376 
377                 break;
378 
379             case Plexe::PLOEG:
380 
381                 if (vars->autoFeed) {
382                     getVehicleInformation(vars->frontVehicle, vars->frontSpeed, vars->frontAcceleration, vars->frontControllerAcceleration, pos, time);
383                 }
384 
385                 if (vars->useControllerAcceleration) {
386                     predAcceleration = vars->frontControllerAcceleration;
387                 } else {
388                     predAcceleration = vars->frontAcceleration;
389                 }
390                 //check if we received at least one packet
391                 if (vars->frontInitialized)
392                     //ploeg's controller computes \dot{u}_i, so we need to sum such value to the previously computed u_i
393                 {
394                     controllerAcceleration = vars->controllerAcceleration + _ploeg(veh, egoSpeed, predSpeed, predAcceleration, gap2pred);
395                 } else {
396                     controllerAcceleration = 0;
397                 }
398 
399                 break;
400 
401             case Plexe::CONSENSUS:
402                 controllerAcceleration = _consensus(veh, egoSpeed, veh->getPosition(), currentTime);
403                 break;
404 
405             case Plexe::FLATBED:
406 
407                 if (vars->autoFeed) {
408                     getVehicleInformation(vars->leaderVehicle, vars->leaderSpeed, vars->leaderAcceleration, vars->leaderControllerAcceleration, pos, time);
409                     getVehicleInformation(vars->frontVehicle, vars->frontSpeed, vars->frontAcceleration, vars->frontControllerAcceleration, pos, time);
410                 }
411 
412                 //overwrite pred speed using data obtained through wireless communication
413                 predSpeed = vars->frontSpeed;
414                 leaderSpeed = vars->leaderSpeed;
415                 if (vars->usePrediction) {
416                     predSpeed += (currentTime - vars->frontDataReadTime) * vars->frontAcceleration;
417                     leaderSpeed += (currentTime - vars->leaderDataReadTime) * vars->leaderAcceleration;
418                 }
419 
420                 if (vars->caccInitialized) {
421                     controllerAcceleration = _flatbed(veh, veh->getAcceleration(), egoSpeed, predSpeed, gap2pred, leaderSpeed);
422                 } else
423                     //do not let CACC take decisions until at least one packet has been received
424                 {
425                     controllerAcceleration = 0;
426                 }
427 
428                 break;
429 
430             case Plexe::DRIVER:
431                 std::cerr << "Switching to normal driver behavior still not implemented in MSCFModel_CC\n";
432                 assert(false);
433                 break;
434 
435             default:
436                 std::cerr << "Invalid controller selected in MSCFModel_CC\n";
437                 assert(false);
438                 break;
439 
440         }
441 
442     }
443 
444     speed = MAX2(double(0), egoSpeed + ACCEL2SPEED(controllerAcceleration));
445 
446     return speed;
447 }
448 
449 double
_cc(const MSVehicle * veh,double egoSpeed,double desSpeed) const450 MSCFModel_CC::_cc(const MSVehicle* veh, double egoSpeed, double desSpeed) const {
451 
452     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
453     //Eq. 5.5 of the Rajamani book, with Ki = 0 and bounds on max and min acceleration
454     return std::min(myCcAccel, std::max(-myCcDecel, -vars->ccKp * (egoSpeed - desSpeed)));
455 
456 }
457 
458 double
_acc(const MSVehicle * veh,double egoSpeed,double predSpeed,double gap2pred,double headwayTime) const459 MSCFModel_CC::_acc(const MSVehicle* veh, double egoSpeed, double predSpeed, double gap2pred, double headwayTime) const {
460 
461     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
462     //Eq. 6.18 of the Rajamani book
463     return -1.0 / headwayTime * (egoSpeed - predSpeed + vars->accLambda * (-gap2pred + headwayTime * egoSpeed + 2));
464 
465 }
466 
467 double
_cacc(const MSVehicle * veh,double egoSpeed,double predSpeed,double predAcceleration,double gap2pred,double leaderSpeed,double leaderAcceleration,double spacing) const468 MSCFModel_CC::_cacc(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred, double leaderSpeed, double leaderAcceleration, double spacing) const {
469     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
470     //compute epsilon, i.e., the desired distance error
471     double epsilon = -gap2pred + spacing; //NOTICE: error (if any) should already be included in gap2pred
472     //compute epsilon_dot, i.e., the desired speed error
473     double epsilon_dot = egoSpeed - predSpeed;
474     //Eq. 7.39 of the Rajamani book
475     return vars->caccAlpha1 * predAcceleration + vars->caccAlpha2 * leaderAcceleration +
476            vars->caccAlpha3 * epsilon_dot + vars->caccAlpha4 * (egoSpeed - leaderSpeed) + vars->caccAlpha5 * epsilon;
477 }
478 
479 
480 double
_ploeg(const MSVehicle * veh,double egoSpeed,double predSpeed,double predAcceleration,double gap2pred) const481 MSCFModel_CC::_ploeg(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred) const {
482     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
483     return (1 / vars->ploegH * (
484                 -vars->controllerAcceleration +
485                 vars->ploegKp * (gap2pred - (2 + vars->ploegH * egoSpeed)) +
486                 vars->ploegKd * (predSpeed - egoSpeed - vars->ploegH * veh->getAcceleration()) +
487                 predAcceleration
488             )) * TS ;
489 }
490 
491 double
d_i_j(const struct Plexe::VEHICLE_DATA * vehicles,const double h[MAX_N_CARS],int i,int j) const492 MSCFModel_CC::d_i_j(const struct Plexe::VEHICLE_DATA* vehicles, const double h[MAX_N_CARS], int i, int j) const {
493 
494     int k, min_i, max_i;
495     double d = 0;
496     //compute indexes of the summation
497     if (j < i) {
498         min_i = j;
499         max_i = i - 1;
500     } else {
501         min_i = i;
502         max_i = j - 1;
503     }
504     //compute distance
505     for (k = min_i; k <= max_i; k++) {
506         d += h[k] * vehicles[0].speed + vehicles[k].length + 15;
507     }
508 
509     if (j < i) {
510         return d;
511     } else {
512         return -d;
513     }
514 
515 }
516 
517 double
_consensus(const MSVehicle * veh,double egoSpeed,Position egoPosition,double time) const518 MSCFModel_CC::_consensus(const MSVehicle* veh, double egoSpeed, Position egoPosition, double time) const {
519     //TODO: this controller, by using real GPS coordinates, does only work
520     //when vehicles are traveling west-to-east on a straight line, basically
521     //on the X axis. This needs to be fixed to consider direction as well
522     CC_VehicleVariables* vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
523     int index = vars->position;
524     int nCars = vars->nCars;
525     struct Plexe::VEHICLE_DATA* vehicles = vars->vehicles;
526 
527     //loop variable
528     int j;
529     //control input
530     double u_i = 0;
531     //actual distance term
532     double actualDistance = 0;
533     //desired distance term
534     double desiredDistance = 0;
535     //speed error term
536     double speedError = 0;
537     //degree of agent i
538     double d_i = 0;
539 
540     //compensate my position: compute prediction of what will be my position at time of actuation
541     Position egoVelocity = veh->getVelocityVector();
542     egoPosition.set(egoPosition.x() + egoVelocity.x() * STEPS2TIME(DELTA_T),
543                     egoPosition.y() + egoVelocity.y() * STEPS2TIME(DELTA_T));
544     vehicles[index].speed = egoSpeed;
545     vehicles[index].positionX = egoPosition.x();
546     vehicles[index].positionY = egoPosition.y();
547 
548     //check that data from all vehicles have been received. the control
549     //law might actually need a subset of all the data, but d_i_j needs
550     //the lengths of all vehicles. uninitialized values might cause problems
551     if (vars->nInitialized != vars->nCars - 1) {
552         return 0;
553     }
554 
555     //compute speed error.
556     speedError = -vars->b[index] * (egoSpeed - vehicles[0].speed);
557 
558     //compute desired distance term
559     for (j = 0; j < nCars; j++) {
560         if (j == index) {
561             continue;
562         }
563         d_i += vars->L[index][j];
564         desiredDistance -= vars->K[index][j] * vars->L[index][j] * d_i_j(vehicles, vars->h, index, j);
565     }
566     desiredDistance = desiredDistance / d_i;
567 
568     //compute actual distance term
569     for (j = 0; j < nCars; j++) {
570         if (j == index) {
571             continue;
572         }
573         //distance error for consensus with GPS equipped
574         Position otherPosition;
575         double dt = time - vehicles[j].time;
576         //predict the position of the other vehicle
577         otherPosition.setx(vehicles[j].positionX + dt * vehicles[j].speedX);
578         otherPosition.sety(vehicles[j].positionY + dt * vehicles[j].speedY);
579         double distance = egoPosition.distanceTo2D(otherPosition) * sgn(j - index);
580         actualDistance -= vars->K[index][j] * vars->L[index][j] * distance;
581     }
582 
583     actualDistance = actualDistance / (d_i);
584 
585     //original paper formula
586     u_i = (speedError + desiredDistance + actualDistance) / 1000;
587 
588     return u_i;
589 }
590 
591 double
_flatbed(const MSVehicle * veh,double egoAcceleration,double egoSpeed,double predSpeed,double gap2pred,double leaderSpeed) const592 MSCFModel_CC::_flatbed(const MSVehicle* veh, double egoAcceleration, double egoSpeed, double predSpeed,
593                        double gap2pred, double leaderSpeed) const {
594     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
595     return (
596                -vars->flatbedKa * egoAcceleration +
597                vars->flatbedKv * (predSpeed - egoSpeed) +
598                vars->flatbedKp * (gap2pred - vars->flatbedD - vars->flatbedH * (egoSpeed - leaderSpeed))
599            );
600 }
601 
602 double
getCACCConstantSpacing(const MSVehicle * veh) const603 MSCFModel_CC::getCACCConstantSpacing(const MSVehicle* veh) const {
604     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
605     return vars->caccSpacing;
606 }
607 
608 void
getVehicleInformation(const MSVehicle * veh,double & speed,double & acceleration,double & controllerAcceleration,Position & position,double & time) const609 MSCFModel_CC::getVehicleInformation(const MSVehicle* veh, double& speed, double& acceleration, double& controllerAcceleration, Position& position, double& time) const {
610     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
611     speed = veh->getSpeed();
612     acceleration = veh->getAcceleration();
613     controllerAcceleration = vars->controllerAcceleration;
614     position = veh->getPosition();
615     time = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep());
616 }
617 
setParameter(MSVehicle * veh,const std::string & key,const std::string & value) const618 void MSCFModel_CC::setParameter(MSVehicle* veh, const std::string& key, const std::string& value) const {
619     // vehicle variables used to set the parameter
620     CC_VehicleVariables* vars;
621 
622     ParBuffer buf(value);
623 
624     vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
625     try {
626         if (key.compare(PAR_LEADER_SPEED_AND_ACCELERATION) == 0) {
627             double x, y, vx, vy;
628             buf >> vars->leaderSpeed >> vars->leaderAcceleration >> x >> y >> vars->leaderDataReadTime
629                 >> vars->leaderControllerAcceleration >> vx >> vy >> vars->leaderAngle;
630             vars->leaderPosition = Position(x, y);
631             vars->leaderVelocity = Position(vx, vy);
632             vars->leaderInitialized = true;
633             if (vars->frontInitialized) {
634                 vars->caccInitialized = true;
635             }
636             return;
637         }
638         if (key.compare(PAR_PRECEDING_SPEED_AND_ACCELERATION) == 0) {
639             double x, y, vx, vy;
640             buf >> vars->frontSpeed >> vars->frontAcceleration >> x >> y >> vars->frontDataReadTime
641                 >> vars->frontControllerAcceleration >> vx >> vy >> vars->frontAngle;
642             vars->frontPosition = Position(x, y);
643             vars->frontVelocity = Position(vx, vy);
644             vars->frontInitialized = true;
645             if (vars->leaderInitialized) {
646                 vars->caccInitialized = true;
647             }
648             return;
649         }
650         if (key.compare(CC_PAR_VEHICLE_DATA) == 0) {
651             struct Plexe::VEHICLE_DATA vehicle;
652             buf >> vehicle.index >> vehicle.speed >> vehicle.acceleration >>
653                 vehicle.positionX >> vehicle.positionY >> vehicle.time >>
654                 vehicle.length >> vehicle.u >> vehicle.speedX >>
655                 vehicle.speedY >> vehicle.angle;
656             //if the index is larger than the number of cars, simply ignore the data
657             if (vehicle.index >= vars->nCars || vehicle.index == -1) {
658                 return;
659             }
660             vars->vehicles[vehicle.index] = vehicle;
661             if (!vars->initialized[vehicle.index] && vehicle.index != vars->position) {
662                 vars->nInitialized++;
663             }
664             vars->initialized[vehicle.index] = true;
665             return;
666         }
667         if (key.compare(PAR_LEADER_FAKE_DATA) == 0) {
668             buf >> vars->fakeData.leaderSpeed >> vars->fakeData.leaderAcceleration
669                 >> vars->fakeData.leaderControllerAcceleration;
670             if (buf.last_empty()) {
671                 vars->useControllerAcceleration = false;
672             }
673             return;
674         }
675         if (key.compare(PAR_FRONT_FAKE_DATA) == 0) {
676             buf >> vars->fakeData.frontSpeed >> vars->fakeData.frontAcceleration >> vars->fakeData.frontDistance
677                 >> vars->fakeData.frontControllerAcceleration;
678             if (buf.last_empty()) {
679                 vars->useControllerAcceleration = false;
680             }
681             return;
682         }
683         if (key.compare(CC_PAR_VEHICLE_POSITION) == 0) {
684             vars->position = StringUtils::toInt(value.c_str());
685             return;
686         }
687         if (key.compare(CC_PAR_PLATOON_SIZE) == 0) {
688             vars->nCars = StringUtils::toInt(value.c_str());
689             // given that we have a static matrix, check that we're not
690             // setting a number of cars larger than the size of that matrix
691             if (vars->nCars > MAX_N_CARS) {
692                 vars->nCars = MAX_N_CARS;
693                 std::stringstream warn;
694                 warn << "MSCFModel_CC: setting a number of cars of " << vars->nCars << " out of a maximum of " << MAX_N_CARS <<
695                      ". The CONSENSUS controller will not work properly if chosen. If you are using a different controller " <<
696                      "you can ignore this warning";
697                 WRITE_WARNING(warn.str());
698             }
699             return;
700         }
701         if (key.compare(PAR_ADD_MEMBER) == 0) {
702             std::string id;
703             int position;
704             buf >> id >> position;
705             vars->members[position] = id;
706             return;
707         }
708         if (key.compare(PAR_REMOVE_MEMBER) == 0) {
709             for (auto item = vars->members.begin(); item != vars->members.end(); item++)
710                 if (item->second.compare(value) == 0) {
711                     vars->members.erase(item);
712                     break;
713                 }
714             return;
715         }
716         if (key.compare(PAR_ENABLE_AUTO_LANE_CHANGE) == 0) {
717             vars->autoLaneChange = StringUtils::toInt(value.c_str()) == 1;
718             return;
719         }
720         if (key.compare(CC_PAR_CACC_XI) == 0) {
721             vars->caccXi = StringUtils::toDouble(value.c_str());
722             recomputeParameters(veh);
723             return;
724         }
725         if (key.compare(CC_PAR_CACC_OMEGA_N) == 0) {
726             vars->caccOmegaN = StringUtils::toDouble(value.c_str());
727             recomputeParameters(veh);
728             return;
729         }
730         if (key.compare(CC_PAR_CACC_C1) == 0) {
731             vars->caccC1 = StringUtils::toDouble(value.c_str());
732             recomputeParameters(veh);
733             return;
734         }
735         if (key.compare(CC_PAR_ENGINE_TAU) == 0) {
736             vars->engineTau = StringUtils::toDouble(value.c_str());
737             vars->engine->setParameter(FOLM_PAR_TAU, vars->engineTau);
738             recomputeParameters(veh);
739             vars->engine->setParameter(FOLM_PAR_TAU, vars->engineTau);
740         }
741         if (key.compare(CC_PAR_UMIN) == 0) {
742             vars->uMin = StringUtils::toDouble(value.c_str());
743             return;
744         }
745         if (key.compare(CC_PAR_UMAX) == 0) {
746             vars->uMax = StringUtils::toDouble(value.c_str());
747             return;
748         }
749         if (key.compare(CC_PAR_PLOEG_H) == 0) {
750             vars->ploegH = StringUtils::toDouble(value.c_str());
751             return;
752         }
753         if (key.compare(CC_PAR_PLOEG_KP) == 0) {
754             vars->ploegKp = StringUtils::toDouble(value.c_str());
755             return;
756         }
757         if (key.compare(CC_PAR_PLOEG_KD) == 0) {
758             vars->ploegKd = StringUtils::toDouble(value.c_str());
759             return;
760         }
761         if (key.compare(CC_PAR_FLATBED_KA) == 0) {
762             vars->flatbedKa = StringUtils::toDouble(value.c_str());
763             return;
764         }
765         if (key.compare(CC_PAR_FLATBED_KV) == 0) {
766             vars->flatbedKv = StringUtils::toDouble(value.c_str());
767             return;
768         }
769         if (key.compare(CC_PAR_FLATBED_KP) == 0) {
770             vars->flatbedKp = StringUtils::toDouble(value.c_str());
771             return;
772         }
773         if (key.compare(CC_PAR_FLATBED_H) == 0) {
774             vars->flatbedH = StringUtils::toDouble(value.c_str());
775             return;
776         }
777         if (key.compare(CC_PAR_FLATBED_D) == 0) {
778             vars->flatbedD = StringUtils::toDouble(value.c_str());
779             return;
780         }
781         if (key.compare(CC_PAR_VEHICLE_ENGINE_MODEL) == 0) {
782             if (vars->engine) {
783                 delete vars->engine;
784             }
785             int engineModel = StringUtils::toInt(value.c_str());;
786             switch (engineModel) {
787                 case CC_ENGINE_MODEL_REALISTIC: {
788                     vars->engine = new RealisticEngineModel();
789                     vars->engine->setParameter(ENGINE_PAR_DT, TS);
790                     veh->getInfluencer().setSpeedMode(0);
791                     vars->engineModel = CC_ENGINE_MODEL_REALISTIC;
792                     break;
793                 }
794                 case CC_ENGINE_MODEL_FOLM:
795                 default: {
796                     vars->engine = new FirstOrderLagModel();
797                     vars->engine->setParameter(FOLM_PAR_DT, TS);
798                     vars->engine->setParameter(FOLM_PAR_TAU, vars->engineTau);
799                     vars->engineModel = CC_ENGINE_MODEL_FOLM;
800                     break;
801                 }
802             }
803             vars->engine->setMaximumAcceleration(myAccel);
804             vars->engine->setMaximumDeceleration(myDecel);
805             return;
806         }
807         if (key.compare(CC_PAR_VEHICLE_MODEL) == 0) {
808             vars->engine->setParameter(ENGINE_PAR_VEHICLE, value);
809             return;
810         }
811         if (key.compare(CC_PAR_VEHICLES_FILE) == 0) {
812             vars->engine->setParameter(ENGINE_PAR_XMLFILE, value);
813             return;
814         }
815         if (key.compare(PAR_CACC_SPACING) == 0) {
816             vars->caccSpacing = StringUtils::toDouble(value.c_str());
817             return;
818         }
819         if (key.compare(PAR_FIXED_ACCELERATION) == 0) {
820             buf >> vars->useFixedAcceleration >> vars->fixedAcceleration;
821             return;
822         }
823         if (key.compare(PAR_CC_DESIRED_SPEED) == 0) {
824             vars->ccDesiredSpeed = StringUtils::toDouble(value.c_str());
825             return;
826         }
827         if (key.compare(PAR_ACTIVE_CONTROLLER) == 0) {
828             vars->activeController = (enum Plexe::ACTIVE_CONTROLLER) StringUtils::toInt(value.c_str());
829             return;
830         }
831         if (key.compare(PAR_ACC_HEADWAY_TIME) == 0) {
832             vars->accHeadwayTime = StringUtils::toDouble(value.c_str());
833             return;
834         }
835         if (key.compare(PAR_USE_CONTROLLER_ACCELERATION) == 0) {
836             vars->useControllerAcceleration = StringUtils::toInt(value.c_str()) != 0;
837             return;
838         }
839         if (key.compare(PAR_USE_AUTO_FEEDING) == 0) {
840             int af;
841             std::string leaderId, frontId;
842             buf >> af;
843             vars->autoFeed = af == 1;
844             if (vars->autoFeed) {
845                 vars->usePrediction = false;
846                 buf >> leaderId;
847                 if (buf.last_empty()) {
848                     throw InvalidArgument("Trying to enable auto feeding without providing leader vehicle id");
849                 }
850                 vars->leaderVehicle = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(leaderId));
851                 if (vars->leaderVehicle == 0) {
852                     throw libsumo::TraCIException("Vehicle '" + leaderId + "' is not known");
853                 }
854                 buf >> frontId;
855                 if (buf.last_empty()) {
856                     throw InvalidArgument("Trying to enable auto feeding without providing front vehicle id");
857                 }
858                 vars->frontVehicle = dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(frontId));
859                 if (vars->frontVehicle == 0) {
860                     throw libsumo::TraCIException("Vehicle '" + frontId + "' is not known");
861                 }
862                 vars->leaderInitialized = true;
863                 vars->frontInitialized = true;
864                 vars->caccInitialized = true;
865             }
866             return;
867         }
868         if (key.compare(PAR_USE_PREDICTION) == 0) {
869             vars->usePrediction = StringUtils::toInt(value.c_str()) == 1;
870             return;
871         }
872     } catch (NumberFormatException&) {
873         throw InvalidArgument("Invalid value '" + value + "' for parameter '" + key + "' for vehicle '" + veh->getID() + "'");
874     }
875 
876 }
877 
getParameter(const MSVehicle * veh,const std::string & key) const878 std::string MSCFModel_CC::getParameter(const MSVehicle* veh, const std::string& key) const {
879     // vehicle variables used to set the parameter
880     CC_VehicleVariables* vars;
881     ParBuffer buf;
882 
883     vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
884     if (key.compare(PAR_SPEED_AND_ACCELERATION) == 0) {
885         Position velocity = veh->getVelocityVector();
886         buf << veh->getSpeed() << veh->getAcceleration() <<
887             vars->controllerAcceleration << veh->getPosition().x() <<
888             veh->getPosition().y() <<
889             STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) <<
890             velocity.x() << velocity.y() << veh->getAngle();
891         return buf.str();
892     }
893     if (key.compare(PAR_CRASHED) == 0) {
894         return vars->crashed ? "1" : "0";
895     }
896     if (key.compare(PAR_RADAR_DATA) == 0) {
897         double distance, relSpeed;
898         getRadarMeasurements(veh, distance, relSpeed);
899         buf << distance << relSpeed;
900         return buf.str();
901     }
902     if (key.compare(PAR_LANES_COUNT) == 0) {
903         buf << veh->getLane()->getEdge().getLanes().size();
904         return buf.str();
905     }
906     if (key.compare(PAR_DISTANCE_TO_END) == 0) {
907         //route of the vehicle
908         const MSRoute* route;
909         //edge the vehicle is currently traveling on
910         const MSEdge* currentEdge;
911         //last edge of the route of this vehicle
912         const MSEdge* lastEdge;
913         //current position of the vehicle on the edge its traveling in
914         double positionOnEdge;
915         //distance to trip end using
916         double distanceToEnd;
917 
918         route = &veh->getRoute();
919         currentEdge = veh->getEdge();
920         lastEdge = route->getEdges().back();
921         positionOnEdge = veh->getPositionOnLane();
922         distanceToEnd = route->getDistanceBetween(positionOnEdge, lastEdge->getLanes()[0]->getLength(), currentEdge, lastEdge);
923 
924         buf << distanceToEnd;
925         return buf.str();
926     }
927     if (key.compare(PAR_DISTANCE_FROM_BEGIN) == 0) {
928         //route of the vehicle
929         const MSRoute* route;
930         //edge the vehicle is currently traveling on
931         const MSEdge* currentEdge;
932         //last edge of the route of this vehicle
933         const MSEdge* firstEdge;
934         //current position of the vehicle on the edge its traveling in
935         double positionOnEdge;
936         //distance to trip end using
937         double distanceFromBegin;
938 
939         route = &veh->getRoute();
940         currentEdge = veh->getEdge();
941         firstEdge = route->getEdges().front();
942         positionOnEdge = veh->getPositionOnLane();
943         distanceFromBegin = route->getDistanceBetween(0, positionOnEdge, firstEdge, currentEdge);
944 
945         buf << distanceFromBegin;
946         return buf.str();
947     }
948     if (key.compare(PAR_CC_DESIRED_SPEED) == 0) {
949         buf << (double)vars->ccDesiredSpeed;
950         return buf.str();
951     }
952     if (key.compare(PAR_ACTIVE_CONTROLLER) == 0) {
953         buf << (int)vars->activeController;
954         return buf.str();
955     }
956     if (key.compare(PAR_ACC_HEADWAY_TIME) == 0) {
957         buf << (double)vars->accHeadwayTime;
958         return buf.str();
959     }
960     if (key.compare(PAR_ACC_ACCELERATION) == 0) {
961         buf << getACCAcceleration(veh);
962         return buf.str();
963     }
964     if (key.compare(PAR_CACC_SPACING) == 0) {
965         buf << vars->caccSpacing;
966         return buf.str();
967     }
968     if (key.find(CC_PAR_VEHICLE_DATA) == 0) {
969         ParBuffer inBuf(key);
970         int index;
971         inBuf >> index;
972         struct Plexe::VEHICLE_DATA vehicle;
973         if (index >= vars->nCars || index < 0) {
974             vehicle.index = -1;
975         } else {
976             vehicle = vars->vehicles[index];
977         }
978         buf << vehicle.index << vehicle.speed << vehicle.acceleration <<
979             vehicle.positionX << vehicle.positionY << vehicle.time <<
980             vehicle.length << vehicle.u << vehicle.speedX <<
981             vehicle.speedY << vehicle.angle;
982         return buf.str();
983     }
984     if (key.compare(PAR_ENGINE_DATA) == 0) {
985         int gear;
986         double rpm;
987         RealisticEngineModel* engine = dynamic_cast<RealisticEngineModel*>(vars->engine);
988         if (engine) {
989             engine->getEngineData(veh->getSpeed(), gear, rpm);
990         } else {
991             gear = -1;
992             rpm = 0;
993         }
994         buf << (gear + 1) << rpm;
995         return buf.str();
996     }
997     return "";
998 }
999 
recomputeParameters(const MSVehicle * veh) const1000 void MSCFModel_CC::recomputeParameters(const MSVehicle* veh) const {
1001     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1002     vars->caccAlpha1 = 1 - vars->caccC1;
1003     vars->caccAlpha2 = vars->caccC1;
1004     vars->caccAlpha3 = -(2 * vars->caccXi - vars->caccC1 * (vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1))) * vars->caccOmegaN;
1005     vars->caccAlpha4 = -(vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1)) * vars->caccOmegaN * vars->caccC1;
1006     vars->caccAlpha5 = -vars->caccOmegaN * vars->caccOmegaN;
1007 }
1008 
resetConsensus(const MSVehicle * veh) const1009 void MSCFModel_CC::resetConsensus(const MSVehicle* veh) const {
1010     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1011     for (int i = 0; i < MAX_N_CARS; i++) {
1012         vars->initialized[i] = false;
1013         vars->nInitialized = 0;
1014     }
1015 }
1016 
switchOnACC(const MSVehicle * veh,double ccDesiredSpeed) const1017 void MSCFModel_CC::switchOnACC(const MSVehicle* veh, double ccDesiredSpeed)  const {
1018     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1019     vars->ccDesiredSpeed = ccDesiredSpeed;
1020     vars->activeController = Plexe::ACC;
1021 }
1022 
getActiveController(const MSVehicle * veh) const1023 enum Plexe::ACTIVE_CONTROLLER MSCFModel_CC::getActiveController(const MSVehicle* veh) const {
1024     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1025     return vars->activeController;
1026 }
1027 
getRadarMeasurements(const MSVehicle * veh,double & distance,double & relativeSpeed) const1028 void MSCFModel_CC::getRadarMeasurements(const MSVehicle* veh, double& distance, double& relativeSpeed) const {
1029     std::pair<std::string, double> l = libsumo::Vehicle::getLeader(veh->getID(), 250);
1030     if (l.second < 0) {
1031         distance = -1;
1032         relativeSpeed = 0;
1033     } else {
1034         distance = l.second;
1035         SUMOVehicle* leader = MSNet::getInstance()->getVehicleControl().getVehicle(l.first);
1036         relativeSpeed = leader->getSpeed() - veh->getSpeed();
1037     }
1038 }
1039 
setCrashed(const MSVehicle * veh,bool crashed,bool victim) const1040 void MSCFModel_CC::setCrashed(const MSVehicle* veh, bool crashed, bool victim) const {
1041     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1042     if (victim) {
1043         vars->crashedVictim = crashed;
1044     } else {
1045         vars->crashed = crashed;
1046     }
1047 }
1048 
getACCAcceleration(const MSVehicle * veh) const1049 double MSCFModel_CC::getACCAcceleration(const MSVehicle* veh) const {
1050     CC_VehicleVariables* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
1051     double distance, relSpeed;
1052     getRadarMeasurements(veh, distance, relSpeed);
1053     if (distance < 0) {
1054         return 0;
1055     } else {
1056         return _acc(veh, veh->getSpeed(), relSpeed + veh->getSpeed(), distance, vars->accHeadwayTime);
1057     }
1058 }
1059 
getMyLanesCount() const1060 int MSCFModel_CC::getMyLanesCount() const {
1061     return myLanesCount;
1062 }
1063 
1064 MSCFModel*
duplicate(const MSVehicleType * vtype) const1065 MSCFModel_CC::duplicate(const MSVehicleType* vtype) const {
1066     return new MSCFModel_CC(vtype);
1067 }
1068