Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
MSCFModel_CC.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
18// A series of automatic Cruise Controllers (CC, ACC, CACC)
19/****************************************************************************/
20#include <config.h>
21
22#include <algorithm>
26#include <microsim/MSVehicle.h>
28#include <microsim/MSNet.h>
29#include <microsim/MSEdge.h>
30#include <microsim/MSStop.h>
32#include <libsumo/Vehicle.h>
33#include <libsumo/TraCIDefs.h>
34#include "MSCFModel_CC.h"
35
36#ifndef sgn
37#define sgn(x) ((x > 0) - (x < 0))
38#endif
39
40
41// ===========================================================================
42// method definitions
43// ===========================================================================
45 myCcDecel(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CCDECEL, 1.5)),
46 myCcAccel(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CCACCEL, 1.5)),
47 myConstantSpacing(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_CONSTSPACING, 5.0)),
48 myKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_KP, 1.0)),
49 myLambda(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_LAMBDA, 0.1)),
50 myC1(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_C1, 0.5)),
51 myXi(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_XI, 1.0)),
52 myOmegaN(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_OMEGAN, 0.2)),
53 myTau(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_TAU, 0.5)),
54 myLanesCount((int)vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_LANES_COUNT, -1)),
55 myPloegH(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_H, 0.5)),
56 myPloegKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_KP, 0.2)),
57 myPloegKd(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_PLOEG_KD, 0.7)),
58 myFlatbedKa(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KA, 2.4)),
59 myFlatbedKv(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KV, 0.6)),
60 myFlatbedKp(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_KP, 12.0)),
61 myFlatbedH(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_H, 4.0)),
62 myFlatbedD(vtype->getParameter().getCFParam(SUMO_ATTR_CF_CC_FLATBED_D, 5.0)) {
63
64 //if the lanes count has not been specified in the attributes of the model, lane changing cannot properly work
65 if (myLanesCount == -1) {
66 throw ProcessError(TL("The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute"));
67 }
68
69 //instantiate the driver model. For now, use Krauss as default, then needs to be parameterized
71
72}
73
75
79 vars->ccKp = myKp;
80 vars->accLambda = myLambda;
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();
108 return (VehicleVariables*)vars;
109}
110
111void
112MSCFModel_CC::setLeader(MSVehicle* veh, MSVehicle* const leader, std::string leaderId) const {
113 auto* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
114 vars->leaderVehicle = leader;
115 vars->leaderVehicleId = leaderId;
116 if (leader != nullptr) {
117 vars->isLeader = false;
118 } else
119 // if we are removing our leader, then this vehicle must become a leader of itself until being member of another platoon
120 {
121 vars->isLeader = true;
122 }
123}
124
125int
128 if (!vars->isLeader) {
129 // before asking the leader, be sure it is still in the simulation
130 if (findVehicle(vars->leaderVehicleId)) {
131 return isPlatoonLaneChangeSafe(vars->leaderVehicle, left);
132 } else {
133 return LCA_BLOCKED;
134 }
135 }
136 int result = 0;
137 std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->getID(), left ? +1 : -1);
138 // bit 1: query lateral direction (left:0, right:1)
139 // bit 2: query longitudinal direction (followers:0, leaders:1)
140 // bit 3: blocking (return all:0, return only blockers:1)
141 auto followers = libsumo::Vehicle::getNeighbors(veh->getID(), left ? 0b100 : 0b101);
142 auto leaders = libsumo::Vehicle::getNeighbors(veh->getID(), left ? 0b110 : 0b111);
143 bool noNeighbors = followers.empty() && leaders.empty();
144 if (!(state.second & LCA_BLOCKED) && noNeighbors) {
145 // leader is not blocked. check all the members
146 for (auto m = vars->members.begin(); m != vars->members.end(); m++) {
147 const std::pair<int, int> mState = libsumo::Vehicle::getLaneChangeState(m->second, left ? +1 : -1);
148 followers = libsumo::Vehicle::getNeighbors(m->second, left ? 0b100 : 0b101);
149 leaders = libsumo::Vehicle::getNeighbors(m->second, left ? 0b110 : 0b111);
150 noNeighbors = followers.empty() && leaders.empty();
151 if (mState.second & LCA_BLOCKED || !noNeighbors) {
152 if (mState.second & LCA_BLOCKED) {
153 result = mState.second;
154 } else {
155 if (!followers.empty()) {
157 }
158 if (!leaders.empty()) {
160 }
161 }
162 break;
163 }
164 }
165 } else {
166 if (state.second & LCA_BLOCKED) {
167 result = state.second;
168 } else {
169 if (!followers.empty()) {
171 }
172 if (!leaders.empty()) {
174 }
175 }
176 }
177 return result;
178}
179
180void
181MSCFModel_CC::changeWholePlatoonLane(MSVehicle* const veh, int direction) const {
183 libsumo::Vehicle::changeLane(veh->getID(), veh->getLaneIndex() + direction, 0);
184 for (auto& member : vars->members) {
185 libsumo::Vehicle::changeLane(member.second, veh->getLaneIndex() + direction, 0);
186 }
187}
188
189void
191 // check for left lane change
192 std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->getID(), +1);
193 int traciState = state.first;
194 if (traciState & LCA_LEFT && traciState & LCA_SPEEDGAIN) {
195 // we can gain by moving left. check that all vehicles can move left
196 if (isPlatoonLaneChangeSafe(veh, true) == 0) {
197 changeWholePlatoonLane(veh, +1);
198 }
199 }
200 // check for right lane change
201 state = libsumo::Vehicle::getLaneChangeState(veh->getID(), -1);
202 traciState = state.first;
203 if (traciState & LCA_RIGHT && traciState & LCA_KEEPRIGHT) {
204 // we should move back right. check that all vehicles can move right
205 if (isPlatoonLaneChangeSafe(veh, false) == 0) {
206 changeWholePlatoonLane(veh, -1);
207 }
208 }
209}
210
211void
213 auto* vars = (CC_VehicleVariables*) veh->getCarFollowVariables();
214 int currentLane = veh->getLaneIndex();
215 if (currentLane == vars->platoonFixedLane) {
216 // platoon has already reached the desired lane
217 vars->platoonFixedLane = -1;
218 return;
219 }
220 bool left = currentLane < vars->platoonFixedLane;
221 if (isPlatoonLaneChangeSafe(veh, left) == 0) {
222 changeWholePlatoonLane(veh, left ? +1 : -1);
223 }
224}
225
226double
227MSCFModel_CC::getSecureGap(const MSVehicle* const veh, const MSVehicle* const pred, const double speed, const double leaderSpeed, const double leaderMaxDecel) const {
229
230 const double tolerance = 0.8;
231 switch (vars->activeController) {
232 case Plexe::CACC:
234 return vars->caccSpacing * tolerance;
235 case Plexe::ACC:
236 return (vars->accHeadwayTime * speed + 2) * tolerance;
237 case Plexe::PLOEG:
238 return (vars->ploegH * speed + 2) * tolerance;
239 case Plexe::CONSENSUS:
240 return d_i_j(vars->vehicles, vars->h, 1, 0) * tolerance;
241 case Plexe::FLATBED:
242 return (vars->flatbedD - vars->flatbedH * (speed - leaderSpeed)) * tolerance;
243 case Plexe::DRIVER:
244 return myHumanDriver->getSecureGap(veh, pred, speed, leaderSpeed, leaderMaxDecel);
245 default:
246 throw InvalidArgument("Unsupported activeController" + toString(vars->activeController));
247 }
248}
249
250int
251MSCFModel_CC::commitToLaneChange(const MSVehicle* veh, bool left) const {
252 auto vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
253 if (isLeader(veh)) {
255 if (vars->laneChangeCommitTime == timestep) {
256 if (vars->commitToLaneChange) {
257 return 0;
258 } else {
259 return vars->noCommitReason;
260 }
261 } else {
262 int blocked = isPlatoonLaneChangeSafe(veh, left);
263 if (blocked == 0) {
264 vars->commitToLaneChange = true;
265 vars->laneChangeCommitTime = timestep;
266 }
267 return blocked;
268 }
269 } else {
270 // before asking the leader, be sure it is still in the simulation
271 if (findVehicle(vars->leaderVehicleId)) {
272 return commitToLaneChange(vars->leaderVehicle, left);
273 } else {
274 return LCA_BLOCKED;
275 }
276 }
277}
278
280MSCFModel_CC::findVehicle(std::string id) const {
281 return dynamic_cast<MSVehicle*>(MSNet::getInstance()->getVehicleControl().getVehicle(id));
282}
283
284double
285MSCFModel_CC::finalizeSpeed(MSVehicle* const veh, double vPos) const {
286 double vNext;
287 //acceleration computed by the controller
288 double controllerAcceleration;
289 //acceleration after engine actuation
290 double engineAcceleration;
291
293
294 //call processNextStop() to ensure vehicle removal in case of crash
295 veh->processNextStop(vPos);
296
297 //check whether the vehicle has collided and set the flag in case
298 if (!vars->crashed) {
299 for (const MSStop& s : veh->getStops()) {
300 if (s.pars.collision) {
301 vars->crashed = true;
302 }
303 }
304 }
305
306 if (vars->activeController != Plexe::DRIVER && !vars->useFixedAcceleration) {
308 }
309
310 if (vars->platoonFixedLane >= 0) {
312 }
313
314 if (vars->autoLaneChange) {
316 }
317
318 if (vars->activeController != Plexe::DRIVER) {
319 controllerAcceleration = SPEED2ACCEL(vPos - veh->getSpeed());
320 controllerAcceleration = std::min(vars->uMax, std::max(vars->uMin, controllerAcceleration));
321 //compute the actual acceleration applied by the engine
322 engineAcceleration = vars->engine->getRealAcceleration(veh->getSpeed(), veh->getAcceleration(), controllerAcceleration, MSNet::getInstance()->getCurrentTimeStep());
323 vNext = MAX2(double(0), veh->getSpeed() + ACCEL2SPEED(engineAcceleration));
324 vars->controllerAcceleration = controllerAcceleration;
325 } else {
326 vNext = myHumanDriver->finalizeSpeed(veh, vPos);
327 }
328
329 return vNext;
330}
331
332
333double
334MSCFModel_CC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred, const CalcReason usage) const {
335
336 UNUSED_PARAMETER(pred);
338
339 if (vars->activeController != Plexe::DRIVER) {
340 return _v(veh, gap2pred, speed, predSpeed);
341 } else {
342 return myHumanDriver->followSpeed(veh, speed, gap2pred, predSpeed, predMaxDecel, pred, usage);
343 }
344}
345
346double
347MSCFModel_CC::insertionFollowSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
348 UNUSED_PARAMETER(veh);
349 UNUSED_PARAMETER(gap2pred);
350 UNUSED_PARAMETER(predSpeed);
351 UNUSED_PARAMETER(predMaxDecel);
352 //by returning speed + 1, we tell sumo that "speed" is always a safe speed
353 return speed + 1;
354}
355
356double
357MSCFModel_CC::stopSpeed(const MSVehicle* const veh, double speed, double gap2pred, double decel, const CalcReason usage) const {
358
360 if (vars->activeController != Plexe::DRIVER) {
361 double relSpeed;
362 getRadarMeasurements(veh, gap2pred, relSpeed);
363 if (gap2pred == -1) {
364 gap2pred = std::numeric_limits<double>().max();
365 }
366 return _v(veh, gap2pred, speed, speed + relSpeed);
367 } else {
368 return myHumanDriver->stopSpeed(veh, speed, gap2pred, decel, usage);
369 }
370}
371
372double MSCFModel_CC::freeSpeed(const MSVehicle* const veh, double speed, double seen, double maxSpeed, const bool onInsertion, const CalcReason usage) const {
374 if (vars->activeController != Plexe::DRIVER) {
375 double gap2pred, relSpeed;
376 getRadarMeasurements(veh, gap2pred, relSpeed);
377 if (gap2pred == -1) {
378 gap2pred = std::numeric_limits<double>().max();
379 }
380 return _v(veh, gap2pred, speed, speed + relSpeed);
381 } else {
382 return MSCFModel::freeSpeed(veh, speed, seen, maxSpeed, onInsertion, usage);
383 }
384}
385
386double
387MSCFModel_CC::interactionGap(const MSVehicle* const veh, double vL) const {
388
390 if (vars->activeController != Plexe::DRIVER) {
391 //maximum radar range is CC is enabled
392 return 250;
393 } else {
394 return myHumanDriver->interactionGap(veh, vL);
395 }
396
397}
398
399double
400MSCFModel_CC::maxNextSpeed(double speed, const MSVehicle* const veh) const {
402 if (vars->engineModel == CC_ENGINE_MODEL_FOLM) {
403 return speed + (double) ACCEL2SPEED(getMaxAccel());
404 } else {
405 return speed + (double) ACCEL2SPEED(20);
406 }
407}
408
409double
410MSCFModel_CC::minNextSpeed(double speed, const MSVehicle* const veh) const {
412 if (vars->engineModel == CC_ENGINE_MODEL_FOLM) {
413 return MSCFModel::minNextSpeed(speed, veh);
414 } else {
415 return MAX2((double)0, speed - (double) ACCEL2SPEED(20));
416 }
417}
418
419double
420MSCFModel_CC::_v(const MSVehicle* const veh, double gap2pred, double egoSpeed, double predSpeed) const {
421
423
424 //acceleration computed by the controller
425 double controllerAcceleration = vars->fixedAcceleration;
426 //speed computed by the model
427 double speed;
428 //acceleration computed by the Cruise Control
429 double ccAcceleration;
430 //acceleration computed by the Adaptive Cruise Control
431 double accAcceleration;
432 //acceleration computed by the Cooperative Adaptive Cruise Control
433 double caccAcceleration;
434 //variables needed by CACC
435 double predAcceleration, leaderAcceleration, leaderSpeed;
436 //dummy variables used for auto feeding
437 Position pos;
438 double time;
439 const double currentTime = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep() + DELTA_T);
440
441 if (vars->crashed) {
442 return 0;
443 }
444 if (vars->autoFeed) {
445 if (!findVehicle(vars->leaderVehicleId) || !findVehicle(vars->frontVehicleId)) {
446 // either the leader or the front vehicle have left the simulation. Disable auto feed
447 vars->autoFeed = false;
448 vars->leaderVehicle = nullptr;
449 vars->frontVehicle = nullptr;
450 }
451 }
452 if (vars->activeController == Plexe::DRIVER || !vars->useFixedAcceleration) {
453 switch (vars->activeController) {
454 case Plexe::ACC:
455 ccAcceleration = _cc(veh, egoSpeed, vars->ccDesiredSpeed);
456 accAcceleration = _acc(veh, egoSpeed, predSpeed, gap2pred, vars->accHeadwayTime);
457 if (gap2pred > 250 || ccAcceleration < accAcceleration) {
458 controllerAcceleration = ccAcceleration;
459 } else {
460 controllerAcceleration = accAcceleration;
461 }
462 break;
463
464 case Plexe::CACC:
465 if (vars->autoFeed) {
468 }
469
470 if (vars->useControllerAcceleration) {
471 predAcceleration = vars->frontControllerAcceleration;
472 leaderAcceleration = vars->leaderControllerAcceleration;
473 } else {
474 predAcceleration = vars->frontAcceleration;
475 leaderAcceleration = vars->leaderAcceleration;
476 }
477 //overwrite pred speed using data obtained through wireless communication
478 predSpeed = vars->frontSpeed;
479 leaderSpeed = vars->leaderSpeed;
480 if (vars->usePrediction) {
481 predSpeed += (currentTime - vars->frontDataReadTime) * vars->frontAcceleration;
482 leaderSpeed += (currentTime - vars->leaderDataReadTime) * vars->leaderAcceleration;
483 }
484
485 if (vars->caccInitialized) {
486 controllerAcceleration = _cacc(veh, egoSpeed, predSpeed, predAcceleration, gap2pred, leaderSpeed, leaderAcceleration, vars->caccSpacing);
487 } else
488 //do not let CACC take decisions until at least one packet has been received
489 {
490 controllerAcceleration = 0;
491 }
492
493 break;
494
496
497 if (vars->autoFeed) {
501 }
502
503 if (vars->useControllerAcceleration) {
504 predAcceleration = vars->fakeData.frontControllerAcceleration;
505 leaderAcceleration = vars->fakeData.leaderControllerAcceleration;
506 } else {
507 predAcceleration = vars->fakeData.frontAcceleration;
508 leaderAcceleration = vars->fakeData.leaderAcceleration;
509 }
510 ccAcceleration = _cc(veh, egoSpeed, vars->ccDesiredSpeed);
511 caccAcceleration = _cacc(veh, egoSpeed, vars->fakeData.frontSpeed, predAcceleration, vars->fakeData.frontDistance, vars->fakeData.leaderSpeed, leaderAcceleration, vars->caccSpacing);
512 //faked CACC can be used to get closer to a platoon for joining
513 //using the minimum acceleration ensures that we do not exceed
514 //the CC desired speed
515 controllerAcceleration = std::min(ccAcceleration, caccAcceleration);
516
517 break;
518
519 case Plexe::PLOEG:
520
521 if (vars->autoFeed) {
523 }
524
525 if (vars->useControllerAcceleration) {
526 predAcceleration = vars->frontControllerAcceleration;
527 } else {
528 predAcceleration = vars->frontAcceleration;
529 }
530 //check if we received at least one packet
531 if (vars->frontInitialized)
532 //ploeg's controller computes \dot{u}_i, so we need to sum such value to the previously computed u_i
533 {
534 controllerAcceleration = vars->controllerAcceleration + _ploeg(veh, egoSpeed, predSpeed, predAcceleration, gap2pred);
535 } else {
536 controllerAcceleration = 0;
537 }
538
539 break;
540
541 case Plexe::CONSENSUS:
542 controllerAcceleration = _consensus(veh, egoSpeed, veh->getPosition(), currentTime);
543 break;
544
545 case Plexe::FLATBED:
546
547 if (vars->autoFeed) {
550 }
551
552 //overwrite pred speed using data obtained through wireless communication
553 predSpeed = vars->frontSpeed;
554 leaderSpeed = vars->leaderSpeed;
555 if (vars->usePrediction) {
556 predSpeed += (currentTime - vars->frontDataReadTime) * vars->frontAcceleration;
557 leaderSpeed += (currentTime - vars->leaderDataReadTime) * vars->leaderAcceleration;
558 }
559
560 if (vars->caccInitialized) {
561 controllerAcceleration = _flatbed(veh, veh->getAcceleration(), egoSpeed, predSpeed, gap2pred, leaderSpeed);
562 } else
563 //do not let CACC take decisions until at least one packet has been received
564 {
565 controllerAcceleration = 0;
566 }
567
568 break;
569
570 case Plexe::DRIVER:
571 std::cerr << "Switching to normal driver behavior still not implemented in MSCFModel_CC\n";
572 assert(false);
573 break;
574
575 default:
576 std::cerr << "Invalid controller selected in MSCFModel_CC\n";
577 assert(false);
578 break;
579
580 }
581
582 }
583
584 speed = MAX2(double(0), egoSpeed + ACCEL2SPEED(controllerAcceleration));
585
586 return speed;
587}
588
589double
590MSCFModel_CC::_cc(const MSVehicle* veh, double egoSpeed, double desSpeed) const {
591
593 //Eq. 5.5 of the Rajamani book, with Ki = 0 and bounds on max and min acceleration
594 return std::min(myCcAccel, std::max(-myCcDecel, -vars->ccKp * (egoSpeed - desSpeed)));
595
596}
597
598double
599MSCFModel_CC::_acc(const MSVehicle* veh, double egoSpeed, double predSpeed, double gap2pred, double headwayTime) const {
600
602 //Eq. 6.18 of the Rajamani book
603 return -1.0 / headwayTime * (egoSpeed - predSpeed + vars->accLambda * (-gap2pred + headwayTime * egoSpeed + 2));
604
605}
606
607double
608MSCFModel_CC::_cacc(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred, double leaderSpeed, double leaderAcceleration, double spacing) const {
610 //compute epsilon, i.e., the desired distance error
611 double epsilon = -gap2pred + spacing; //NOTICE: error (if any) should already be included in gap2pred
612 //compute epsilon_dot, i.e., the desired speed error
613 double epsilon_dot = egoSpeed - predSpeed;
614 //Eq. 7.39 of the Rajamani book
615 return vars->caccAlpha1 * predAcceleration + vars->caccAlpha2 * leaderAcceleration +
616 vars->caccAlpha3 * epsilon_dot + vars->caccAlpha4 * (egoSpeed - leaderSpeed) + vars->caccAlpha5 * epsilon;
617}
618
619
620double
621MSCFModel_CC::_ploeg(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred) const {
623 return (1 / vars->ploegH * (
625 vars->ploegKp * (gap2pred - (2 + vars->ploegH * egoSpeed)) +
626 vars->ploegKd * (predSpeed - egoSpeed - vars->ploegH * veh->getAcceleration()) +
627 predAcceleration
628 )) * TS ;
629}
630
631double
632MSCFModel_CC::d_i_j(const struct Plexe::VEHICLE_DATA* vehicles, const double h[MAX_N_CARS], int i, int j) const {
633
634 int k, min_i, max_i;
635 double d = 0;
636 //compute indexes of the summation
637 if (j < i) {
638 min_i = j;
639 max_i = i - 1;
640 } else {
641 min_i = i;
642 max_i = j - 1;
643 }
644 //compute distance
645 for (k = min_i; k <= max_i; k++) {
646 d += h[k] * vehicles[0].speed + vehicles[k].length + 15;
647 }
648
649 if (j < i) {
650 return d;
651 } else {
652 return -d;
653 }
654
655}
656
657double
658MSCFModel_CC::_consensus(const MSVehicle* veh, double egoSpeed, Position egoPosition, double time) const {
659 //TODO: this controller, by using real GPS coordinates, does only work
660 //when vehicles are traveling west-to-east on a straight line, basically
661 //on the X axis. This needs to be fixed to consider direction as well
663 int index = vars->position;
664 int nCars = vars->nCars;
665 struct Plexe::VEHICLE_DATA* vehicles = vars->vehicles;
666
667 //loop variable
668 int j;
669 //control input
670 double u_i = 0;
671 //actual distance term
672 double actualDistance = 0;
673 //desired distance term
674 double desiredDistance = 0;
675 //speed error term
676 double speedError = 0;
677 //degree of agent i
678 double d_i = 0;
679
680 //compensate my position: compute prediction of what will be my position at time of actuation
681 Position egoVelocity = veh->getVelocityVector();
682 egoPosition.set(egoPosition.x() + egoVelocity.x() * STEPS2TIME(DELTA_T),
683 egoPosition.y() + egoVelocity.y() * STEPS2TIME(DELTA_T));
684 vehicles[index].speed = egoSpeed;
685 vehicles[index].positionX = egoPosition.x();
686 vehicles[index].positionY = egoPosition.y();
687
688 //check that data from all vehicles have been received. the control
689 //law might actually need a subset of all the data, but d_i_j needs
690 //the lengths of all vehicles. uninitialized values might cause problems
691 if (vars->nInitialized != vars->nCars - 1) {
692 return 0;
693 }
694
695 //compute speed error.
696 speedError = -vars->b[index] * (egoSpeed - vehicles[0].speed);
697
698 //compute desired distance term
699 for (j = 0; j < nCars; j++) {
700 if (j == index) {
701 continue;
702 }
703 d_i += vars->L[index][j];
704 desiredDistance -= vars->K[index][j] * vars->L[index][j] * d_i_j(vehicles, vars->h, index, j);
705 }
706 desiredDistance = desiredDistance / d_i;
707
708 //compute actual distance term
709 for (j = 0; j < nCars; j++) {
710 if (j == index) {
711 continue;
712 }
713 //distance error for consensus with GPS equipped
714 Position otherPosition;
715 double dt = time - vehicles[j].time;
716 //predict the position of the other vehicle
717 otherPosition.setx(vehicles[j].positionX + dt * vehicles[j].speedX);
718 otherPosition.sety(vehicles[j].positionY + dt * vehicles[j].speedY);
719 double distance = egoPosition.distanceTo2D(otherPosition) * sgn(j - index);
720 actualDistance -= vars->K[index][j] * vars->L[index][j] * distance;
721 }
722
723 actualDistance = actualDistance / (d_i);
724
725 //original paper formula
726 u_i = (speedError + desiredDistance + actualDistance) / 1000;
727
728 return u_i;
729}
730
731double
732MSCFModel_CC::_flatbed(const MSVehicle* veh, double egoAcceleration, double egoSpeed, double predSpeed,
733 double gap2pred, double leaderSpeed) const {
735 return (
736 -vars->flatbedKa * egoAcceleration +
737 vars->flatbedKv * (predSpeed - egoSpeed) +
738 vars->flatbedKp * (gap2pred - vars->flatbedD - vars->flatbedH * (egoSpeed - leaderSpeed))
739 );
740}
741
742double
747
748void
749MSCFModel_CC::getVehicleInformation(const MSVehicle* veh, double& speed, double& acceleration, double& controllerAcceleration, Position& position, double& time) const {
751 speed = veh->getSpeed();
752 acceleration = veh->getAcceleration();
753 controllerAcceleration = vars->controllerAcceleration;
754 position = veh->getPosition();
755 time = STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep());
756}
757
758void MSCFModel_CC::setParameter(MSVehicle* veh, const std::string& key, const std::string& value) const {
759 // vehicle variables used to set the parameter
761
762 ParBuffer buf(value);
763
765 try {
766 if (key.compare(PAR_LEADER_SPEED_AND_ACCELERATION) == 0) {
767 double x, y, vx, vy;
768 buf >> vars->leaderSpeed >> vars->leaderAcceleration >> x >> y >> vars->leaderDataReadTime
769 >> vars->leaderControllerAcceleration >> vx >> vy >> vars->leaderAngle;
770 vars->leaderPosition = Position(x, y);
771 vars->leaderVelocity = Position(vx, vy);
772 vars->leaderInitialized = true;
773 if (vars->frontInitialized) {
774 vars->caccInitialized = true;
775 }
776 return;
777 }
778 if (key.compare(PAR_PRECEDING_SPEED_AND_ACCELERATION) == 0) {
779 double x, y, vx, vy;
780 buf >> vars->frontSpeed >> vars->frontAcceleration >> x >> y >> vars->frontDataReadTime
781 >> vars->frontControllerAcceleration >> vx >> vy >> vars->frontAngle;
782 vars->frontPosition = Position(x, y);
783 vars->frontVelocity = Position(vx, vy);
784 vars->frontInitialized = true;
785 if (vars->leaderInitialized) {
786 vars->caccInitialized = true;
787 }
788 return;
789 }
790 if (key.compare(CC_PAR_VEHICLE_DATA) == 0) {
791 struct Plexe::VEHICLE_DATA vehicle;
792 buf >> vehicle.index >> vehicle.speed >> vehicle.acceleration >>
793 vehicle.positionX >> vehicle.positionY >> vehicle.time >>
794 vehicle.length >> vehicle.u >> vehicle.speedX >>
795 vehicle.speedY >> vehicle.angle;
796 //if the index is larger than the number of cars, simply ignore the data
797 if (vehicle.index >= vars->nCars || vehicle.index == -1) {
798 return;
799 }
800 vars->vehicles[vehicle.index] = vehicle;
801 if (!vars->initialized[vehicle.index] && vehicle.index != vars->position) {
802 vars->nInitialized++;
803 }
804 vars->initialized[vehicle.index] = true;
805 return;
806 }
807 if (key.compare(PAR_LEADER_FAKE_DATA) == 0) {
808 buf >> vars->fakeData.leaderSpeed >> vars->fakeData.leaderAcceleration
810 if (buf.last_empty()) {
811 vars->useControllerAcceleration = false;
812 }
813 return;
814 }
815 if (key.compare(PAR_FRONT_FAKE_DATA) == 0) {
818 if (buf.last_empty()) {
819 vars->useControllerAcceleration = false;
820 }
821 return;
822 }
823 if (key.compare(CC_PAR_VEHICLE_POSITION) == 0) {
824 vars->position = StringUtils::toInt(value.c_str());
825 return;
826 }
827 if (key.compare(CC_PAR_PLATOON_SIZE) == 0) {
828 vars->nCars = StringUtils::toInt(value.c_str());
829 // given that we have a static matrix, check that we're not
830 // setting a number of cars larger than the size of that matrix
831 if (vars->nCars > MAX_N_CARS) {
832 vars->nCars = MAX_N_CARS;
833 std::stringstream warn;
834 warn << "MSCFModel_CC: setting a number of cars of " << vars->nCars << " out of a maximum of " << MAX_N_CARS <<
835 ". The CONSENSUS controller will not work properly if chosen. If you are using a different controller " <<
836 "you can ignore this warning";
837 WRITE_WARNING(warn.str());
838 }
839 return;
840 }
841 if (key.compare(PAR_ADD_MEMBER) == 0) {
842 std::string id;
843 int position;
844 buf >> id >> position;
845 vars->members[position] = id;
846
847 auto vehicle = findVehicle(id);
848 if (!vehicle) {
849 throw libsumo::TraCIException("Adding " + id + " as member but " + id + " does not exists");
850 }
851 auto cfm = dynamic_cast<const MSCFModel_CC*>(&vehicle->getVehicleType().getCarFollowModel());
852 if (!cfm) {
853 throw libsumo::TraCIException("Adding " + id + " as member but " + id + " is not using MSCFModel_CC");
854 }
855 cfm->setLeader(vehicle, veh, veh->getID());
856 vars->isLeader = true;
857 return;
858 }
859 if (key.compare(PAR_REMOVE_MEMBER) == 0) {
860 for (auto item = vars->members.begin(); item != vars->members.end(); item++)
861 if (item->second.compare(value) == 0) {
862 auto vehicle = findVehicle(value);
863 if (!vehicle) {
864 throw libsumo::TraCIException("Removing " + value + " from members but " + value + " does not exist");
865 }
866 auto cfm = dynamic_cast<const MSCFModel_CC*>(&vehicle->getVehicleType().getCarFollowModel());
867 if (!cfm) {
868 throw libsumo::TraCIException("Removing " + value + " from members but " + value + " is not using MSCFModel_CC");
869 }
870 cfm->setLeader(vehicle, nullptr, "");
871 vars->members.erase(item);
872 break;
873 }
874 return;
875 }
876 if (key.compare(PAR_ENABLE_AUTO_LANE_CHANGE) == 0) {
877 vars->autoLaneChange = StringUtils::toInt(value.c_str()) == 1;
878 // if the user enables automatic lane changing, any request for a fixed lane is deleted
879 if (vars->autoLaneChange) {
880 vars->platoonFixedLane = -1;
881 }
882 return;
883 }
884 if (key.compare(PAR_PLATOON_FIXED_LANE) == 0) {
885 vars->platoonFixedLane = StringUtils::toInt(value.c_str());
886 // asking a leader to change the lane for the whole platoon automatically disables auto lane change
887 if (vars->platoonFixedLane >= 0) {
888 vars->autoLaneChange = false;
889 }
890 return;
891 }
892 if (key.compare(CC_PAR_CACC_XI) == 0) {
893 vars->caccXi = StringUtils::toDouble(value.c_str());
895 return;
896 }
897 if (key.compare(CC_PAR_CACC_OMEGA_N) == 0) {
898 vars->caccOmegaN = StringUtils::toDouble(value.c_str());
900 return;
901 }
902 if (key.compare(CC_PAR_CACC_C1) == 0) {
903 vars->caccC1 = StringUtils::toDouble(value.c_str());
905 return;
906 }
907 if (key.compare(CC_PAR_ENGINE_TAU) == 0) {
908 vars->engineTau = StringUtils::toDouble(value.c_str());
912 }
913 if (key.compare(CC_PAR_UMIN) == 0) {
914 vars->uMin = StringUtils::toDouble(value.c_str());
915 return;
916 }
917 if (key.compare(CC_PAR_UMAX) == 0) {
918 vars->uMax = StringUtils::toDouble(value.c_str());
919 return;
920 }
921 if (key.compare(CC_PAR_PLOEG_H) == 0) {
922 vars->ploegH = StringUtils::toDouble(value.c_str());
923 return;
924 }
925 if (key.compare(CC_PAR_PLOEG_KP) == 0) {
926 vars->ploegKp = StringUtils::toDouble(value.c_str());
927 return;
928 }
929 if (key.compare(CC_PAR_PLOEG_KD) == 0) {
930 vars->ploegKd = StringUtils::toDouble(value.c_str());
931 return;
932 }
933 if (key.compare(CC_PAR_FLATBED_KA) == 0) {
934 vars->flatbedKa = StringUtils::toDouble(value.c_str());
935 return;
936 }
937 if (key.compare(CC_PAR_FLATBED_KV) == 0) {
938 vars->flatbedKv = StringUtils::toDouble(value.c_str());
939 return;
940 }
941 if (key.compare(CC_PAR_FLATBED_KP) == 0) {
942 vars->flatbedKp = StringUtils::toDouble(value.c_str());
943 return;
944 }
945 if (key.compare(CC_PAR_FLATBED_H) == 0) {
946 vars->flatbedH = StringUtils::toDouble(value.c_str());
947 return;
948 }
949 if (key.compare(CC_PAR_FLATBED_D) == 0) {
950 vars->flatbedD = StringUtils::toDouble(value.c_str());
951 return;
952 }
953 if (key.compare(CC_PAR_VEHICLE_ENGINE_MODEL) == 0) {
954 if (vars->engine) {
955 delete vars->engine;
956 }
957 int engineModel = StringUtils::toInt(value.c_str());
958 switch (engineModel) {
960 vars->engine = new RealisticEngineModel();
962 veh->getInfluencer().setSpeedMode(0);
964 break;
965 }
967 default: {
968 vars->engine = new FirstOrderLagModel();
972 break;
973 }
974 }
977 return;
978 }
979 if (key.compare(CC_PAR_VEHICLE_MODEL) == 0) {
981 return;
982 }
983 if (key.compare(CC_PAR_VEHICLES_FILE) == 0) {
985 return;
986 }
987 if (key.compare(PAR_CACC_SPACING) == 0) {
988 vars->caccSpacing = StringUtils::toDouble(value.c_str());
989 return;
990 }
991 if (key.compare(PAR_FIXED_ACCELERATION) == 0) {
992 buf >> vars->useFixedAcceleration >> vars->fixedAcceleration;
993 return;
994 }
995 if (key.compare(PAR_CC_DESIRED_SPEED) == 0) {
996 vars->ccDesiredSpeed = StringUtils::toDouble(value.c_str());
997 return;
998 }
999 if (key.compare(PAR_ACTIVE_CONTROLLER) == 0) {
1000 vars->activeController = (enum Plexe::ACTIVE_CONTROLLER) StringUtils::toInt(value.c_str());
1001 return;
1002 }
1003 if (key.compare(PAR_ACC_HEADWAY_TIME) == 0) {
1004 vars->accHeadwayTime = StringUtils::toDouble(value.c_str());
1005 return;
1006 }
1007 if (key.compare(PAR_USE_CONTROLLER_ACCELERATION) == 0) {
1008 vars->useControllerAcceleration = StringUtils::toInt(value.c_str()) != 0;
1009 return;
1010 }
1011 if (key.compare(PAR_USE_AUTO_FEEDING) == 0) {
1012 int af;
1013 std::string leaderId, frontId;
1014 buf >> af;
1015 vars->autoFeed = af == 1;
1016 if (vars->autoFeed) {
1017 vars->usePrediction = false;
1018 buf >> leaderId;
1019 if (buf.last_empty()) {
1020 throw InvalidArgument("Trying to enable auto feeding without providing leader vehicle id");
1021 }
1022 vars->leaderVehicleId = leaderId;
1023 vars->leaderVehicle = findVehicle(leaderId);
1024 if (!vars->leaderVehicle) {
1025 throw libsumo::TraCIException("Vehicle '" + leaderId + "' is not known");
1026 }
1027 buf >> frontId;
1028 if (buf.last_empty()) {
1029 throw InvalidArgument("Trying to enable auto feeding without providing front vehicle id");
1030 }
1031 vars->frontVehicleId = frontId;
1032 vars->frontVehicle = findVehicle(frontId);
1033 if (!vars->frontVehicle) {
1034 throw libsumo::TraCIException("Vehicle '" + frontId + "' is not known");
1035 }
1036 vars->leaderInitialized = true;
1037 vars->frontInitialized = true;
1038 vars->caccInitialized = true;
1039 }
1040 return;
1041 }
1042 if (key.compare(PAR_USE_PREDICTION) == 0) {
1043 vars->usePrediction = StringUtils::toInt(value.c_str()) == 1;
1044 return;
1045 }
1046 } catch (NumberFormatException&) {
1047 throw InvalidArgument("Invalid value '" + value + "' for parameter '" + key + "' for vehicle '" + veh->getID() + "'");
1048 }
1049
1050}
1051
1052std::string MSCFModel_CC::getParameter(const MSVehicle* veh, const std::string& key) const {
1053 // vehicle variables used to set the parameter
1054 CC_VehicleVariables* vars;
1055 ParBuffer buf;
1056
1058 if (key.compare(PAR_SPEED_AND_ACCELERATION) == 0) {
1059 Position velocity = veh->getVelocityVector();
1060 buf << veh->getSpeed() << veh->getAcceleration() <<
1061 vars->controllerAcceleration << veh->getPosition().x() <<
1062 veh->getPosition().y() <<
1063 STEPS2TIME(MSNet::getInstance()->getCurrentTimeStep()) <<
1064 velocity.x() << velocity.y() << veh->getAngle();
1065 return buf.str();
1066 }
1067 if (key.compare(PAR_CRASHED) == 0) {
1068 return vars->crashed ? "1" : "0";
1069 }
1070 if (key.compare(PAR_RADAR_DATA) == 0) {
1071 double distance, relSpeed;
1072 getRadarMeasurements(veh, distance, relSpeed);
1073 buf << distance << relSpeed;
1074 return buf.str();
1075 }
1076 if (key.compare(PAR_LANES_COUNT) == 0) {
1077 buf << veh->getLane()->getEdge().getLanes().size();
1078 return buf.str();
1079 }
1080 if (key.compare(PAR_DISTANCE_TO_END) == 0) {
1081 //route of the vehicle
1082 ConstMSRoutePtr route = veh->getRoutePtr();
1083 //last lane of the route of this vehicle
1084 const MSLane* lastLane = route->getEdges().back()->getLanes()[0];
1085 //distance to trip end
1086 buf << route->getDistanceBetween(veh->getPositionOnLane(), lastLane->getLength(), veh->getLane(), lastLane);
1087 return buf.str();
1088 }
1089 if (key.compare(PAR_DISTANCE_FROM_BEGIN) == 0) {
1090 //route of the vehicle
1091 ConstMSRoutePtr route = veh->getRoutePtr();
1092 //first lane of the route of this vehicle
1093 const MSLane* firstLane = route->getEdges().front()->getLanes()[0];
1094 //distance to trip start
1095 buf << route->getDistanceBetween(0., veh->getPositionOnLane(), firstLane, veh->getLane());
1096 return buf.str();
1097 }
1098 if (key.compare(PAR_CC_DESIRED_SPEED) == 0) {
1099 buf << (double)vars->ccDesiredSpeed;
1100 return buf.str();
1101 }
1102 if (key.compare(PAR_ACTIVE_CONTROLLER) == 0) {
1103 buf << (int)vars->activeController;
1104 return buf.str();
1105 }
1106 if (key.compare(PAR_ACC_HEADWAY_TIME) == 0) {
1107 buf << (double)vars->accHeadwayTime;
1108 return buf.str();
1109 }
1110 if (key.compare(PAR_ACC_ACCELERATION) == 0) {
1111 buf << getACCAcceleration(veh);
1112 return buf.str();
1113 }
1114 if (key.compare(PAR_CACC_SPACING) == 0) {
1115 buf << vars->caccSpacing;
1116 return buf.str();
1117 }
1118 if (key.find(CC_PAR_VEHICLE_DATA) == 0) {
1119 ParBuffer inBuf(key);
1120 int index;
1121 inBuf >> index;
1122 struct Plexe::VEHICLE_DATA vehicle;
1123 if (index >= vars->nCars || index < 0) {
1124 vehicle.index = -1;
1125 } else {
1126 vehicle = vars->vehicles[index];
1127 }
1128 buf << vehicle.index << vehicle.speed << vehicle.acceleration <<
1129 vehicle.positionX << vehicle.positionY << vehicle.time <<
1130 vehicle.length << vehicle.u << vehicle.speedX <<
1131 vehicle.speedY << vehicle.angle;
1132 return buf.str();
1133 }
1134 if (key.compare(PAR_ENGINE_DATA) == 0) {
1135 int gear;
1136 double rpm;
1137 RealisticEngineModel* engine = dynamic_cast<RealisticEngineModel*>(vars->engine);
1138 if (engine) {
1139 engine->getEngineData(veh->getSpeed(), gear, rpm);
1140 } else {
1141 gear = -1;
1142 rpm = 0;
1143 }
1144 buf << (gear + 1) << rpm;
1145 return buf.str();
1146 }
1147 return "";
1148}
1149
1152 vars->caccAlpha1 = 1 - vars->caccC1;
1153 vars->caccAlpha2 = vars->caccC1;
1154 vars->caccAlpha3 = -(2 * vars->caccXi - vars->caccC1 * (vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1))) * vars->caccOmegaN;
1155 vars->caccAlpha4 = -(vars->caccXi + sqrt(vars->caccXi * vars->caccXi - 1)) * vars->caccOmegaN * vars->caccC1;
1156 vars->caccAlpha5 = -vars->caccOmegaN * vars->caccOmegaN;
1157}
1158
1161 for (int i = 0; i < MAX_N_CARS; i++) {
1162 vars->initialized[i] = false;
1163 vars->nInitialized = 0;
1164 }
1165}
1166
1167void MSCFModel_CC::switchOnACC(const MSVehicle* veh, double ccDesiredSpeed) const {
1169 vars->ccDesiredSpeed = ccDesiredSpeed;
1171}
1172
1177
1178void MSCFModel_CC::getRadarMeasurements(const MSVehicle* veh, double& distance, double& relativeSpeed) const {
1179 std::pair<std::string, double> l = libsumo::Vehicle::getLeader(veh->getID(), 250);
1180 if (l.second < 0) {
1181 distance = -1;
1182 relativeSpeed = 0;
1183 } else {
1184 distance = l.second;
1185 SUMOVehicle* leader = findVehicle(l.first);
1186 relativeSpeed = leader->getSpeed() - veh->getSpeed();
1187 }
1188}
1189
1192 double distance, relSpeed;
1193 getRadarMeasurements(veh, distance, relSpeed);
1194 if (distance < 0) {
1195 return 0;
1196 } else {
1197 return _acc(veh, veh->getSpeed(), relSpeed + veh->getSpeed(), distance, vars->accHeadwayTime);
1198 }
1199}
1200
1202 return myLanesCount;
1203}
1204
1205MSCFModel*
1207 return new MSCFModel_CC(vtype);
1208}
1209
1210bool
1212 auto vars = (CC_VehicleVariables*)veh->getCarFollowVariables();
1213 return vars->isLeader;
1214}
#define ENGINE_PAR_XMLFILE
Definition CC_Const.h:87
#define PAR_RADAR_DATA
Definition CC_Const.h:149
#define PAR_FIXED_ACCELERATION
Definition CC_Const.h:128
#define PAR_REMOVE_MEMBER
Definition CC_Const.h:180
#define CC_PAR_CACC_OMEGA_N
Definition CC_Const.h:96
#define CC_PAR_FLATBED_KP
Definition CC_Const.h:109
#define PAR_CACC_SPACING
Definition CC_Const.h:119
#define CC_PAR_PLOEG_KP
Definition CC_Const.h:104
#define PAR_FRONT_FAKE_DATA
Definition CC_Const.h:155
#define PAR_SPEED_AND_ACCELERATION
Definition CC_Const.h:131
#define CC_ENGINE_MODEL_REALISTIC
Definition CC_Const.h:80
#define MAX_N_CARS
Definition CC_Const.h:77
#define PAR_PLATOON_FIXED_LANE
Definition CC_Const.h:186
#define CC_PAR_FLATBED_KV
Definition CC_Const.h:108
#define PAR_USE_PREDICTION
Definition CC_Const.h:176
#define PAR_DISTANCE_TO_END
Definition CC_Const.h:158
#define CC_ENGINE_MODEL_FOLM
Definition CC_Const.h:79
#define CC_PAR_VEHICLE_MODEL
Definition CC_Const.h:115
#define CC_PAR_PLOEG_H
Definition CC_Const.h:103
#define PAR_PRECEDING_SPEED_AND_ACCELERATION
Definition CC_Const.h:164
#define PAR_ACTIVE_CONTROLLER
Definition CC_Const.h:146
#define CC_PAR_CACC_XI
Definition CC_Const.h:95
#define PAR_LEADER_SPEED_AND_ACCELERATION
Definition CC_Const.h:134
#define CC_PAR_PLATOON_SIZE
Definition CC_Const.h:92
#define CC_PAR_UMIN
Definition CC_Const.h:100
#define CC_PAR_VEHICLE_POSITION
Definition CC_Const.h:91
#define CC_PAR_FLATBED_KA
Definition CC_Const.h:107
#define PAR_LANES_COUNT
Definition CC_Const.h:140
#define CC_PAR_VEHICLE_ENGINE_MODEL
Definition CC_Const.h:113
#define PAR_USE_AUTO_FEEDING
Definition CC_Const.h:173
#define CC_PAR_FLATBED_D
Definition CC_Const.h:111
#define PAR_CC_DESIRED_SPEED
Definition CC_Const.h:143
#define CC_PAR_PLOEG_KD
Definition CC_Const.h:105
#define CC_PAR_VEHICLES_FILE
Definition CC_Const.h:116
#define CC_PAR_UMAX
Definition CC_Const.h:101
#define CC_PAR_CACC_C1
Definition CC_Const.h:97
#define FOLM_PAR_DT
Definition CC_Const.h:84
#define PAR_LEADER_FAKE_DATA
Definition CC_Const.h:154
#define FOLM_PAR_TAU
Definition CC_Const.h:83
#define CC_PAR_FLATBED_H
Definition CC_Const.h:110
#define PAR_ADD_MEMBER
Definition CC_Const.h:179
#define PAR_ACC_HEADWAY_TIME
Definition CC_Const.h:167
#define PAR_DISTANCE_FROM_BEGIN
Definition CC_Const.h:161
#define ENGINE_PAR_VEHICLE
Definition CC_Const.h:86
#define CC_PAR_ENGINE_TAU
Definition CC_Const.h:98
#define ENGINE_PAR_DT
Definition CC_Const.h:88
#define PAR_USE_CONTROLLER_ACCELERATION
Definition CC_Const.h:137
#define PAR_ACC_ACCELERATION
Definition CC_Const.h:122
#define PAR_ENGINE_DATA
Definition CC_Const.h:170
#define CC_PAR_VEHICLE_DATA
Definition CC_Const.h:90
#define PAR_ENABLE_AUTO_LANE_CHANGE
Definition CC_Const.h:183
#define PAR_CRASHED
Definition CC_Const.h:125
long long int SUMOTime
Definition GUI.h:36
#define sgn(x)
#define WRITE_WARNING(msg)
Definition MsgHandler.h:295
#define TL(string)
Definition MsgHandler.h:315
std::shared_ptr< const MSRoute > ConstMSRoutePtr
Definition Route.h:32
SUMOTime DELTA_T
Definition SUMOTime.cpp:38
#define STEPS2TIME(x)
Definition SUMOTime.h:55
#define ACCEL2SPEED(x)
Definition SUMOTime.h:51
#define TS
Definition SUMOTime.h:42
#define SPEED2ACCEL(x)
Definition SUMOTime.h:53
@ LCA_KEEPRIGHT
The action is due to the default of keeping right "Rechtsfahrgebot".
@ LCA_BLOCKED
blocked in all directions
@ LCA_BLOCKED_BY_RIGHT_LEADER
The vehicle is blocked by right leader.
@ LCA_BLOCKED_BY_LEFT_FOLLOWER
The vehicle is blocked by left follower.
@ LCA_LEFT
Wants go to the left.
@ LCA_BLOCKED_BY_RIGHT_FOLLOWER
The vehicle is blocked by right follower.
@ LCA_SPEEDGAIN
The action is due to the wish to be faster (tactical lc)
@ LCA_RIGHT
Wants go to the right.
@ LCA_BLOCKED_BY_LEFT_LEADER
@ SUMO_ATTR_CF_CC_FLATBED_KP
@ SUMO_ATTR_CF_CC_LAMBDA
@ SUMO_ATTR_CF_CC_FLATBED_D
@ SUMO_ATTR_CF_CC_FLATBED_KA
@ SUMO_ATTR_CF_CC_PLOEG_KP
@ SUMO_ATTR_CF_CC_PLOEG_H
@ SUMO_ATTR_CF_CC_OMEGAN
@ SUMO_ATTR_CF_CC_C1
@ SUMO_ATTR_CF_CC_CCACCEL
@ SUMO_ATTR_CF_CC_PLOEG_KD
@ SUMO_ATTR_CF_CC_TAU
@ SUMO_ATTR_CF_CC_XI
@ SUMO_ATTR_CF_CC_CCDECEL
@ SUMO_ATTR_CF_CC_FLATBED_H
@ SUMO_ATTR_CF_CC_LANES_COUNT
@ SUMO_ATTR_CF_CC_CONSTSPACING
@ SUMO_ATTR_CF_CC_KP
@ SUMO_ATTR_CF_CC_FLATBED_KV
#define UNUSED_PARAMETER(x)
Definition StdDefs.h:30
T MAX2(T a, T b)
Definition StdDefs.h:82
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition ToString.h:46
int nInitialized
count of initialized vehicles
int platoonFixedLane
whole platoon lane change (not automatic). -1 indicates no need to change lane (mechanism disabled)
bool autoFeed
determines whether CACC should automatically fetch data about other vehicles
struct FAKE_CONTROLLER_DATA fakeData
fake controller data.
MSVehicle * frontVehicle
front sumo id, used for auto feeding
double accHeadwayTime
headway time for ACC
bool initialized[MAX_N_CARS]
tells whether data about a certain vehicle has been initialized
double frontSpeed
current front vehicle speed
double K[MAX_N_CARS][MAX_N_CARS]
K matrix.
struct Plexe::VEHICLE_DATA vehicles[MAX_N_CARS]
data about vehicles in the platoon
int position
my position within the platoon (0 = first car)
double frontAcceleration
current front vehicle acceleration (used by CACC)
bool frontInitialized
@did we receive at least one packet?
double leaderDataReadTime
when leader data has been readed from GPS
bool usePrediction
enable/disable data prediction (interpolation) for missing data
std::map< int, std::string > members
list of members belonging to my platoon
double leaderControllerAcceleration
platoon's leader controller acceleration (used by CACC)
double controllerAcceleration
acceleration as computed by the controller, to be sent to other vehicles
int L[MAX_N_CARS][MAX_N_CARS]
L matrix.
double caccXi
controller related parameters
double frontControllerAcceleration
front vehicle controller acceleration (used by CACC)
double b[MAX_N_CARS]
vector of damping ratios b
GenericEngineModel * engine
engine model employed by this car
int nCars
number of cars in the platoon
double leaderSpeed
platoon's leader speed (used by CACC)
double leaderAngle
platoon's leader angle in radians
enum Plexe::ACTIVE_CONTROLLER activeController
currently active controller
bool leaderInitialized
@did we receive at least one packet?
double caccSpacing
fixed spacing for CACC
Position frontVelocity
front vehicle velocity vector
double uMin
limits for u
double ccDesiredSpeed
CC desired speed.
double h[MAX_N_CARS]
vector of time headways h
std::string frontVehicleId
sumo id of the front vehicle
Position frontPosition
current front vehicle position
bool isLeader
whether this vehicle is leader of a platoon or not. This is mainly used by the lane change logic....
double leaderAcceleration
platoon's leader acceleration (used by CACC)
std::string leaderVehicleId
sumo id of the leader vehicle
bool autoLaneChange
automatic whole platoon lane change
Position leaderVelocity
platoon's leader velocity vector
double frontAngle
front vehicle angle in radians
double frontDataReadTime
when front vehicle data has been readed from GPS
int engineModel
numeric value indicating the employed model
MSVehicle * leaderVehicle
leader vehicle, used for auto feeding
Position leaderPosition
platoon's leader position
bool useControllerAcceleration
determines whether PATH's CACC should use the real vehicle acceleration or the controller computed on...
void setMaximumAcceleration(double maxAcc)
virtual double getRealAcceleration(double speed_mps, double accel_mps2, double reqAccel_mps2, SUMOTime timeStep=0)=0
void setMaximumDeceleration(double maxDec)
virtual void setParameter(const std::string parameter, const std::string &value)=0
ConstMSRoutePtr getRoutePtr() const
Returns the current route.
void setChosenSpeedFactor(const double factor)
Returns the precomputed factor by which the driver wants to be faster than the speed limit.
const std::list< MSStop > & getStops() const
A set of automatic Cruise Controllers, including classic Cruise Control (CC), Adaptive Cruise Control...
double _v(const MSVehicle *const veh, double gap2pred, double egoSpeed, double predSpeed) const
MSCFModel_CC(const MSVehicleType *vtype)
Constructor.
virtual std::string getParameter(const MSVehicle *veh, const std::string &key) const
set the information about a generic car. This method should be invoked by TraCI when a wireless messa...
virtual double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Overload base MSCFModel::insertionFollowSpeed method to inject automated vehicles as soon as they are...
const double myPloegKd
const double myFlatbedH
const double myFlatbedKp
const double myPloegH
Ploeg's CACC parameters.
double getACCAcceleration(const MSVehicle *veh) const
returns the ACC computed acceleration when the faked CACC is controlling the car. This can be used to...
bool isLeader(const MSVehicle *veh) const
Returns whether a vehicle is a leader of a platoon or not. By default, a vehicle on its own using an ...
int getMyLanesCount() const
returns the number of lanes set in the configuration file
double _flatbed(const MSVehicle *veh, double egoAcceleration, double egoSpeed, double predSpeed, double gap2pred, double leaderSpeed) const
flatbed platoon towing model
void getVehicleInformation(const MSVehicle *veh, double &speed, double &acceleration, double &controllerAcceleration, Position &position, double &time) const
get the information about a vehicle. This can be used by TraCI in order to get speed and acceleration...
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
virtual double getSecureGap(const MSVehicle *const veh, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
VehicleVariables * createVehicleVariables() const
Returns model specific values which are stored inside a vehicle and must be used with casting.
MSVehicle * findVehicle(std::string id) const
void getRadarMeasurements(const MSVehicle *veh, double &distance, double &relativeSpeed) const
return the data that is currently being measured by the radar
double _consensus(const MSVehicle *veh, double egoSpeed, Position egoPosition, double time) const
controller based on consensus strategy
const double myPloegKp
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed (no dawdling)
double _ploeg(const MSVehicle *veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred) const
controller for the Ploeg's CACC which computes the control input variation. Opposed to other controll...
void performPlatoonLaneChange(MSVehicle *const veh) const
If safe to do so, moves a platoon to a user-desired lane. If not safe, this method continues to try a...
void changeWholePlatoonLane(MSVehicle *const veh, int direction) const
Moves an entire platoon on an adjacent lane, calling changeLane() on all members.
const double myTau
engine time constant used for actuation lag
double d_i_j(const struct Plexe::VEHICLE_DATA *vehicles, const double h[MAX_N_CARS], int i, int j) const
computes the desired distance between vehicle i and vehicle j
const double myOmegaN
design constant for CACC
double stopSpeed(const MSVehicle *const veh, const double speed, double gap2pred, double decel, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
const double myC1
design constant for CACC
const int myLanesCount
number of lanes in the highway, in the absence of on-/off-ramps. This is used to move to the correct ...
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
double _cacc(const MSVehicle *veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred, double leaderSpeed, double leaderAcceleration, double spacing) const
controller for the CACC which computes the acceleration to be applied. the value needs to be passed t...
virtual void setParameter(MSVehicle *veh, const std::string &key, const std::string &value) const
try to set the given parameter for this carFollowingModel
const double myConstantSpacing
the constant gap for CACC
double getCACCConstantSpacing(const MSVehicle *veh) const
returns CACC desired constant spacing
~MSCFModel_CC()
Destructor.
const double myFlatbedKv
void recomputeParameters(const MSVehicle *veh) const
Recomputes controller related parameters after setting them.
const double myXi
design constant for CACC
const double myFlatbedKa
flatbed CACC parameters
void switchOnACC(const MSVehicle *veh, double ccDesiredSpeed) const
switch on the ACC, so disabling the human driver car control
int commitToLaneChange(const MSVehicle *veh, bool left) const
MSCFModel * myHumanDriver
the car following model which drives the car when automated cruising is disabled, i....
const double myLambda
design constant for ACC
void performAutoLaneChange(MSVehicle *const veh) const
Check whether a platoon would gain speed by moving to the left or whether it should move to the right...
void setLeader(MSVehicle *veh, MSVehicle *const leader, std::string leaderId) const
Sets the leader for a member of the platoon.
const double myKp
design constant for CC
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
const double myCcAccel
The maximum acceleration that the CC can output.
int isPlatoonLaneChangeSafe(const MSVehicle *veh, bool left) const
computes whether a lane change for a whole platoon is safe or not. This is done by checking the lane ...
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
const double myCcDecel
The maximum deceleration that the CC can output.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
const double myFlatbedD
void resetConsensus(const MSVehicle *veh) const
Resets the consensus controller. In particular, sets the "initialized" vector all to false....
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
enum Plexe::ACTIVE_CONTROLLER getActiveController(const MSVehicle *veh) const
return the currently active controller
double _cc(const MSVehicle *veh, double egoSpeed, double desSpeed) const
controller for the CC which computes the acceleration to be applied. the value needs to be passed to ...
double _acc(const MSVehicle *veh, double egoSpeed, double predSpeed, double gap2pred, double headwayTime) const
controller for the ACC which computes the acceleration to be applied. the value needs to be passed to...
Krauss car-following model, with acceleration decrease and faster start.
The car-following model abstraction.
Definition MSCFModel.h:55
virtual double interactionGap(const MSVehicle *const veh, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
CalcReason
What the return value of stop/follow/free-Speed is used for.
Definition MSCFModel.h:77
virtual double getSecureGap(const MSVehicle *const veh, const MSVehicle *const, const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
double myDecel
The vehicle's maximum deceleration [m/s^2].
Definition MSCFModel.h:701
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
Definition MSCFModel.h:256
double myAccel
The vehicle's maximum acceleration [m/s^2].
Definition MSCFModel.h:698
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
Definition MSCFModel.h:168
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition MSEdge.h:168
Representation of a lane in the micro simulation.
Definition MSLane.h:84
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
Definition MSLane.h:592
double getLength() const
Returns the lane's length.
Definition MSLane.h:606
MSEdge & getEdge() const
Returns the lane's edge.
Definition MSLane.h:764
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition MSNet.cpp:185
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition MSNet.h:320
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition MSNet.h:378
void setSpeedMode(int speedMode)
Sets speed-constraining behaviors.
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Representation of a vehicle in the micro simulation.
Definition MSVehicle.h:77
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Definition MSVehicle.h:514
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition MSVehicle.h:581
bool isLeader(const MSLink *link, const MSVehicle *veh, const double gap) const
whether the given vehicle must be followed at the given junction
Influencer & getInfluencer()
double getSpeed() const
Returns the vehicle's current speed.
Definition MSVehicle.h:490
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition MSVehicle.h:374
double getAngle() const
Returns the vehicle's direction in radians.
Definition MSVehicle.h:735
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
Definition MSVehicle.h:990
int getLaneIndex() const
Position getVelocityVector() const
Returns the vehicle's direction in radians.
Definition MSVehicle.h:743
The car-following model and parameter.
const std::string & getID() const
Returns the id.
Definition Named.h:74
std::string str() const
Definition ParBuffer.h:148
bool last_empty()
Definition ParBuffer.h:138
A point in 2D or 3D with translation and scaling methods.
Definition Position.h:37
void setx(double x)
set position x
Definition Position.h:70
void set(double x, double y)
set positions x and y
Definition Position.h:85
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition Position.h:276
double x() const
Returns the x-position.
Definition Position.h:55
void sety(double y)
set position y
Definition Position.h:75
double y() const
Returns the y-position.
Definition Position.h:60
void getEngineData(double speed_mps, int &gear, double &rpm)
virtual double getSpeed() const =0
Returns the object's current speed.
Representation of a vehicle.
Definition SUMOVehicle.h:62
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static int toInt(const std::string &sData)
converts a string into the integer value described by it by calling the char-type converter,...
An error which allows to continue.
Definition TraCIDefs.h:144
ACTIVE_CONTROLLER
Determines the currently active controller, i.e., ACC, CACC, or the driver. In future we might need t...
Definition CC_Const.h:49
@ DRIVER
Definition CC_Const.h:49
@ CONSENSUS
Definition CC_Const.h:49
@ FAKED_CACC
Definition CC_Const.h:49
@ CACC
Definition CC_Const.h:49
@ PLOEG
Definition CC_Const.h:49
@ ACC
Definition CC_Const.h:49
@ FLATBED
Definition CC_Const.h:49