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