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