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