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