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