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.h
15 : /// @author Michele Segata
16 : /// @date Wed, 18 Apr 2012
17 : ///
18 : // A series of automatic Cruise Controllers (CC, ACC, CACC)
19 : /****************************************************************************/
20 : #pragma once
21 : #include <config.h>
22 :
23 : #include "CC_Const.h"
24 : #include <microsim/cfmodels/MSCFModel.h>
25 : #include <microsim/MSLane.h>
26 : #include <microsim/MSVehicle.h>
27 : #include <microsim/MSVehicleType.h>
28 : #include <utils/xml/SUMOXMLDefinitions.h>
29 : #include <microsim/cfmodels/MSCFModel_Krauss.h>
30 : #include <string.h>
31 :
32 : #include <microsim/engine/GenericEngineModel.h>
33 : #include <microsim/engine/FirstOrderLagModel.h>
34 : #include <microsim/engine/RealisticEngineModel.h>
35 :
36 : #include "CC_VehicleVariables.h"
37 : #include <libsumo/Helper.h>
38 :
39 :
40 : // ===========================================================================
41 : // class definitions
42 : // ===========================================================================
43 : /** @class MSCFModel_CC
44 : * @brief A set of automatic Cruise Controllers, including classic Cruise
45 : * Control (CC), Adaptive Cruise Control (ACC) and Cooperative Adaptive Cruise
46 : * Control (CACC). Take as references the chapters 5, 6 and 7 of the Rajamani's
47 : * book "Vehicle dynamics and control" (2011).
48 : * This model is meant to be used for simulation of platooning systems in mixed
49 : * scenarios, so with automatic and driver controlled vehicles.
50 : * The platooning manager is a distributed application implemented for veins
51 : * (so for omnet++) supported by a 802.11p based communication protocol, which
52 : * will determine the actions to be performed (such as switching on the
53 : * automatic controller, or the lane to move to) and communicate them to this
54 : * car following models via TraCI
55 : * @see MSCFModel
56 : */
57 : class MSCFModel_CC : public MSCFModel {
58 : public:
59 :
60 : /** @brief Constructor
61 : * @param[in] accel The maximum acceleration that controllers can output (def. 1.5 m/s^2)
62 : * @param[in] decel The maximum deceleration that ACC and CACC controllers can output (def. 6 m/s^2)
63 : * @param[in] ccDecel The maximum deceleration that the CC can output (def. 1.5 m/s^2)
64 : * @param[in] headwayTime the headway gap for ACC (be aware of instabilities) (def. 1.5 s)
65 : * @param[in] constantSpacing the constant gap for CACC (def. 5 m)
66 : * @param[in] kp design constant for CC (def. 1)
67 : * @param[in] lambda design constant for ACC (def. 0.1)
68 : * @param[in] c1 design constant for CACC (def. 0.5)
69 : * @param[in] xi design constant for CACC (def. 1)
70 : * @param[in] omegaN design constant for CACC (def. 0.2)
71 : * @param[in] tau engine time constant used for actuation lag (def. 0.5 s)
72 : * @param[in] lanesCount number of lanes of the highway
73 : * @param[in] ccAccel the maximum acceleration the CC can apply
74 : */
75 : MSCFModel_CC(const MSVehicleType* vtype);
76 :
77 : /// @brief Destructor
78 : ~MSCFModel_CC();
79 :
80 : virtual double getSecureGap(const MSVehicle* const veh, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double leaderMaxDecel) const;
81 :
82 : /// @name Implementations of the MSCFModel interface
83 : /// @{
84 :
85 : /** @brief Applies interaction with stops and lane changing model influences
86 : * @param[in] veh The ego vehicle
87 : * @param[in] vPos The possible velocity
88 : * @return The velocity after applying interactions with stops and lane change model influences
89 : */
90 : virtual double finalizeSpeed(MSVehicle* const veh, double vPos) const;
91 :
92 :
93 : /** @brief Computes the vehicle's safe speed (no dawdling)
94 : * @param[in] veh The vehicle (EGO)
95 : * @param[in] speed The vehicle's speed
96 : * @param[in] gap2pred The (net) distance to the LEADER
97 : * @param[in] predSpeed The speed of LEADER
98 : * @return EGO's safe speed
99 : * @see MSCFModel::ffeV
100 : */
101 : double followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed,
102 : double predMaxDecel, const MSVehicle* const pred = 0, const CalcReason usage = CalcReason::CURRENT) const;
103 :
104 : /** @brief Overload base MSCFModel::insertionFollowSpeed method to inject
105 : * automated vehicles as soon as they are requested, without checking
106 : * for safe speed constraints
107 : *
108 : */
109 : virtual double insertionFollowSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred = 0) const;
110 :
111 :
112 : /** @brief Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
113 : * @param[in] veh The vehicle (EGO)
114 : * @param[in] gap2pred The (net) distance to the obstacle
115 : * @return EGO's safe speed for approaching a non-moving obstacle
116 : * @see MSCFModel::ffeS
117 : * @todo generic Interface, models can call for the values they need
118 : */
119 : double stopSpeed(const MSVehicle* const veh, const double speed, double gap2pred, double decel, const CalcReason usage = CalcReason::CURRENT) const;
120 :
121 : /** @brief Computes the vehicle's safe speed without a leader
122 : *
123 : * Returns the velocity of the vehicle in dependence to the length of the free street and the target
124 : * velocity at the end of the free range. If onInsertion is true, the vehicle may still brake
125 : * before the next movement.
126 : * @param[in] veh The vehicle (EGO)
127 : * @param[in] speed The vehicle's speed
128 : * @param[in] seen The look ahead distance
129 : * @param[in] maxSpeed The maximum allowed speed
130 : * @param[in] onInsertion whether speed at insertion is asked for
131 : * @return EGO's safe speed
132 : */
133 : virtual double freeSpeed(const MSVehicle* const veh, double speed, double seen,
134 : double maxSpeed, const bool onInsertion = false, const CalcReason usage = CalcReason::CURRENT) const;
135 :
136 : virtual double maxNextSpeed(double speed, const MSVehicle* const veh) const;
137 :
138 : virtual double minNextSpeed(double speed, const MSVehicle* const veh = 0) const;
139 :
140 :
141 : /** @brief Returns the maximum gap at which an interaction between both vehicles occurs
142 : *
143 : * "interaction" means that the LEADER influences EGO's speed.
144 : * @param[in] veh The EGO vehicle
145 : * @param[in] vL LEADER's speed
146 : * @return The interaction gap
147 : * @todo evaluate signature
148 : * @see MSCFModel::interactionGap
149 : */
150 : double interactionGap(const MSVehicle* const, double vL) const;
151 :
152 :
153 : /** @brief Returns the model's name
154 : * @return The model's name
155 : * @see MSCFModel::getModelName
156 : */
157 0 : int getModelID() const {
158 0 : return SUMO_TAG_CF_CC;
159 : }
160 : /// @}
161 :
162 :
163 :
164 : /** @brief Duplicates the car-following model
165 : * @param[in] vtype The vehicle type this model belongs to (1:1)
166 : * @return A duplicate of this car-following model
167 : */
168 : MSCFModel* duplicate(const MSVehicleType* vtype) const;
169 :
170 : VehicleVariables* createVehicleVariables() const;
171 :
172 : /**
173 : * @brief returns CACC desired constant spacing
174 : *
175 : * @param[in] veh the vehicle to get constant spacing of
176 : * @return spacing the spacing in meters
177 : */
178 : double getCACCConstantSpacing(const MSVehicle* veh) const;
179 :
180 : /**
181 : * @brief set the information about a generic car. This method should be invoked
182 : * by TraCI when a wireless message with such data is received. For testing, it might
183 : * be also invoked from SUMO source code
184 : *
185 : * @param[in] veh the vehicle for which the data must be saved
186 : * @param[in] speed the leader speed
187 : * @param[in] acceleration the leader acceleration
188 : * @param[in] position the position of the leader
189 : * @param[in] time the time at which this data was read from leader's sensors
190 : */
191 : // void setVehicleInformation(const MSVehicle* veh, double speed, double acceleration, Position position, double time) const;
192 :
193 : /**
194 : * @brief try to get the given parameter for this carFollowingModel
195 : *
196 : * @param[in] veh the vehicle from which the parameter must be retrieved
197 : * @param[in] key the key of the parameter
198 : * @return the value of the requested parameter
199 : */
200 : virtual std::string getParameter(const MSVehicle* veh, const std::string& key) const;
201 :
202 : /**
203 : * @brief try to set the given parameter for this carFollowingModel
204 : *
205 : * @param[in] veh the vehicle for which the parameter must be set
206 : * @param[in] key the key of the parameter
207 : * @param[in] value the value to be set for the given parameter
208 : */
209 : virtual void setParameter(MSVehicle* veh, const std::string& key, const std::string& value) const;
210 :
211 : /**
212 : * @brief get the information about a vehicle. This can be used by TraCI in order to
213 : * get speed and acceleration of the platoon leader before sending them to other
214 : * vehicles
215 : *
216 : * @param[in] veh the vehicle for which the data is requested
217 : * @param[out] speed where the speed is written
218 : * @param[out] acceleration where the acceleration is written
219 : * @param[out] controllerAcceleration the last acceleration value computed by
220 : * the controller will be written in this variable. This might be different from
221 : * acceleration because of actuation lag
222 : */
223 : void getVehicleInformation(const MSVehicle* veh, double& speed, double& acceleration, double& controllerAcceleration, Position& position, double& time) const;
224 :
225 : /**
226 : * @brief switch on the ACC, so disabling the human driver car control
227 : *
228 : * @param[in] veh the vehicle for which the ACC must be switched on
229 : * @param[in] ccDesiredSpeed the cruise control speed
230 : */
231 : void switchOnACC(const MSVehicle* veh, double ccDesiredSpeed) const;
232 :
233 : /**
234 : * @brief return the currently active controller
235 : *
236 : * @param[in] veh the vehicle for which the action is requested
237 : * @return the currently active controller
238 : */
239 : enum Plexe::ACTIVE_CONTROLLER getActiveController(const MSVehicle* veh) const;
240 :
241 : /**
242 : * @brief return the data that is currently being measured by the radar
243 : */
244 : void getRadarMeasurements(const MSVehicle* veh, double& distance, double& relativeSpeed) const;
245 :
246 : /**
247 : * @brief returns the ACC computed acceleration when the faked
248 : * CACC is controlling the car. This can be used to check for
249 : * vehicles in front
250 : */
251 : double getACCAcceleration(const MSVehicle* veh) const;
252 :
253 : /**
254 : * @brief computes whether a lane change for a whole platoon is safe or not.
255 : * This is done by checking the lane change state and neighbors of all vehicles in the platoon added through
256 : * the addPlatoonMember API. If the adjacent lane is free and there is enough safe gap for the platoon, the
257 : * method returns 0, otherwise it returns the blocking reason
258 : * @param veh vehicle for which the check should be made. If the method is invoked on a member, such member
259 : * invokes the same method recursively on its leader. The leader in turn asks all member for their status
260 : * @param left whether we want to check the left or the right lane
261 : * @return 0 if it is safe to change lane, the blocking reason otherwise
262 : */
263 : int isPlatoonLaneChangeSafe(const MSVehicle* veh, bool left) const;
264 :
265 : /**
266 : * @brief Sets the leader for a member of the platoon
267 : * @param veh platoon member
268 : * @param leader platoon leader
269 : * @param leaderId platoon leader id
270 : */
271 : void setLeader(MSVehicle* veh, MSVehicle* const leader, std::string leaderId) const;
272 :
273 : /**
274 : * @brief Returns whether a vehicle is a leader of a platoon or not. By default, a vehicle on its own using an ACC
275 : * is a leader of itself
276 : * @param veh vehicle to check
277 : * @return true if leader
278 : */
279 : bool isLeader(const MSVehicle* veh) const;
280 :
281 : /**
282 : * This method can be invoked by the lane change model to do a final check on the safety of a platoon lane change
283 : * maneuver. This method will cause the leader to check once again the safety status of all members. This might
284 : * have changed since the decision of changing lane because two platoons might have decided to change lane exactly
285 : * at the same time, when the lane was free for both, but changing lane would cause a side-to-side collision.
286 : * SUMO will call this method on one platoon which will answer "still safe" and cause SUMO to start moving the
287 : * vehicles on the adjacent lane. On the second platoon, calling this method will cause the platoon to find out
288 : * about the vehicles that have just been moved and indicate the lane as busy, aborting the lane change.
289 : * @param veh vehicle for which the check should be made. If the method is invoked on a member, such member
290 : * invokes the same method recursively on its leader. The leader in turn asks all member for their status
291 : * @param left whether we want to check the left or the right lane
292 : * @return 0 if it is safe to change lane, the blocking reason otherwise
293 : */
294 : int commitToLaneChange(const MSVehicle* veh, bool left) const;
295 :
296 : /**
297 : * Searches for a vehicle given its sumo id. Differently from libsumo's getVehicle, this call does not throw an
298 : * exception if the vehicle does not exist, so we can check for its existence
299 : * @param id sumo vehicle id
300 : * @return a pointer to the vehicle if found, nullptr otherwise
301 : */
302 : MSVehicle* findVehicle(std::string id) const;
303 :
304 : /**
305 : * @brief returns the number of lanes set in the configuration file
306 : */
307 : int getMyLanesCount() const;
308 :
309 : private:
310 :
311 : /**
312 : * @brief Recomputes controller related parameters after setting them
313 : */
314 : void recomputeParameters(const MSVehicle* veh) const;
315 :
316 : /**
317 : * @brief Resets the consensus controller. In particular, sets the
318 : * "initialized" vector all to false. This might be useful when changing
319 : * topology.
320 : */
321 : void resetConsensus(const MSVehicle* veh) const;
322 :
323 : private:
324 : /**
325 : * @brief Moves an entire platoon on an adjacent lane, calling changeLane() on all members
326 : * @param veh leader vehicle of the platoon
327 : * @param direction +1 for left adjacent lane, -1 for right adjacent lane
328 : */
329 : void changeWholePlatoonLane(MSVehicle* const veh, int direction) const;
330 :
331 : /**
332 : * @brief Check whether a platoon would gain speed by moving to the left or whether it should move to the right
333 : * after an overtake and, if safe, do so. This method is invoked when the platoon auto lane change mechanism is
334 : * enabled through the enableAutoLaneChanging API
335 : * @param veh leader vehicle of the platoon
336 : */
337 : void performAutoLaneChange(MSVehicle* const veh) const;
338 :
339 : /**
340 : * @brief If safe to do so, moves a platoon to a user-desired lane. If not safe, this method continues to try
341 : * at each simulation step. This method is invoked when the user requests a platoon to change lane through
342 : * the performPlatoonLaneChange API, or when invoking the old setFixedLane API on a single vehicle
343 : * @param veh leader vehicle of the platoon
344 : */
345 : void performPlatoonLaneChange(MSVehicle* const veh) const;
346 :
347 : double _v(const MSVehicle* const veh, double gap2pred, double egoSpeed, double predSpeed) const;
348 :
349 : /** @brief controller for the CC which computes the acceleration to be applied. the value needs to be passed to the actuator
350 : *
351 : * @param[in] egoSpeed vehicle current speed
352 : * @param[in] desSpeed vehicle desired speed
353 : * @return the acceleration to be given to the actuator
354 : */
355 : double _cc(const MSVehicle* veh, double egoSpeed, double desSpeed) const;
356 :
357 : /** @brief controller for the ACC which computes the acceleration to be applied. the value needs to be passed to the actuator
358 : *
359 : * @param[in] egoSpeed vehicle current speed
360 : * @param[in] desSpeed vehicle desired speed
361 : * @param[in] gap2pred the distance to preceding vehicle
362 : * @param[in] headwayTime the headway time ACC should maintain
363 : * @return the acceleration to be given to the actuator
364 : */
365 : double _acc(const MSVehicle* veh, double egoSpeed, double predSpeed, double gap2pred, double headwayTime) const;
366 :
367 : /** @brief controller for the CACC which computes the acceleration to be applied. the value needs to be passed to the actuator
368 : *
369 : * @param[in] egoSpeed vehicle current speed
370 : * @param[in] desSpeed vehicle desired speed
371 : * @param[in] predAcceleration acceleration of preceding vehicle
372 : * @param[in] gap2pred the distance to preceding vehicle
373 : * @param[in] leaderSpeed the speed of the platoon leader
374 : * @param[in] leaderAcceleration the acceleration of the platoon leader
375 : * @param[in] spacing the spacing to be kept
376 : * @return the acceleration to be given to the actuator
377 : */
378 : double _cacc(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred, double leaderSpeed, double leaderAcceleration, double spacing) const;
379 :
380 : /** @brief controller for the Ploeg's CACC which computes the control input variation.
381 : * Opposed to other controllers, this method returns a value which needs to be summed
382 : * to the previous desired acceleration.
383 : *
384 : * @param[in] egoSpeed vehicle current speed
385 : * @param[in] predSpeed the speed of the front vehicle
386 : * @param[in] predAcceleration acceleration of preceding vehicle
387 : * @param[in] gap2pred the distance to preceding vehicle
388 : * @return the variation of desired acceleration
389 : */
390 : double _ploeg(const MSVehicle* veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred) const;
391 :
392 : /** @brief controller based on consensus strategy
393 : *
394 : * @param[in] egoSpeed vehicle current speed
395 : * @param[in] egoPosition vehicle current position
396 : * @param[in] time current time
397 : * @return the acceleration to be given to the actuator
398 : */
399 : double _consensus(const MSVehicle* veh, double egoSpeed, Position egoPosition, double time) const;
400 :
401 : /** @brief computes the desired distance between vehicle i and vehicle j
402 : *
403 : * @param[in] vehicles data about all vehicles
404 : * @param[in] h vector of times headway
405 : * @param[in] i index of own vehicle
406 : * @param[in] j index of vehicle to compute distance from
407 : * @return the desired distance between vehicle i and j
408 : *
409 : */
410 : double d_i_j(const struct Plexe::VEHICLE_DATA* vehicles, const double h[MAX_N_CARS], int i, int j) const;
411 :
412 : /** @brief flatbed platoon towing model
413 : *
414 : * @param[in] egoAcceleration vehicle current acceleration
415 : * @param[in] egoSpeed vehicle current speed
416 : * @param[in] predSpeed front vehicle speed
417 : * @param[in] gap2pred distance to front vehicle
418 : * @param[in] leaderSpeed speed of leading vehicle
419 : */
420 : double _flatbed(const MSVehicle* veh, double egoAcceleration, double egoSpeed, double predSpeed,
421 : double gap2pred, double leaderSpeed) const;
422 :
423 :
424 : private:
425 :
426 : /// @brief the car following model which drives the car when automated cruising is disabled, i.e., the human driver
427 : MSCFModel* myHumanDriver;
428 :
429 : /// @brief The maximum deceleration that the CC can output
430 : const double myCcDecel;
431 :
432 : /// @brief The maximum acceleration that the CC can output
433 : const double myCcAccel;
434 :
435 : /// @brief the constant gap for CACC
436 : const double myConstantSpacing;
437 :
438 : /// @brief design constant for CC
439 : const double myKp;
440 :
441 : /// @brief design constant for ACC
442 : const double myLambda;
443 :
444 : /// @brief design constant for CACC
445 : const double myC1;
446 :
447 : /// @brief design constant for CACC
448 : const double myXi;
449 :
450 : /// @brief design constant for CACC
451 : const double myOmegaN;
452 :
453 : /// @brief engine time constant used for actuation lag
454 : const double myTau;
455 :
456 : /// @brief number of lanes in the highway, in the absence of on-/off-ramps. This is used
457 : /// to move to the correct lane even when a lane is added for on-/off-ramps
458 : const int myLanesCount;
459 :
460 : /// @brief Ploeg's CACC parameters
461 : const double myPloegH;
462 : const double myPloegKp;
463 : const double myPloegKd;
464 :
465 : /// @brief flatbed CACC parameters
466 : const double myFlatbedKa;
467 : const double myFlatbedKv;
468 : const double myFlatbedKp;
469 : const double myFlatbedH;
470 : const double myFlatbedD;
471 :
472 : private:
473 : /// @brief Invalidated assignment operator.
474 : MSCFModel_CC& operator=(const MSCFModel_CC&) = delete;
475 : };
|
