46 #define DEBUG_CACC_INSERTION_FOLLOW_SPEED 0 47 #define DEBUG_CACC_SECURE_GAP 0 48 #define DEBUG_COND (veh->isSelected()) 56 #define DEFAULT_SC_GAIN_CACC -0.4 57 #define DEFAULT_GCC_GAIN_GAP_CACC 0.005 58 #define DEFAULT_GCC_GAIN_GAP_DOT_CACC 0.05 59 #define DEFAULT_GC_GAIN_GAP_CACC 0.45 60 #define DEFAULT_GC_GAIN_GAP_DOT_CACC 0.0125 61 #define DEFAULT_CA_GAIN_GAP_CACC 0.45 62 #define DEFAULT_CA_GAIN_GAP_DOT_CACC 0.05 63 #define DEFAULT_HEADWAYTIME_ACC 1.0 66 #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0 93 const double vCACC =
_v(veh, pred, gap2pred, speed, predSpeed, desSpeed,
true);
100 <<
" v=" << speed <<
" vL=" << predSpeed <<
" gap=" << gap2pred
101 <<
" predDecel=" << predMaxDecel <<
" vCACC=" << vCACC <<
" vSafe=" << vSafe <<
"\n";
107 if (vSafe + speedOverride < vCACC) {
110 std::cout <<
"Apply Safe speed, override=" << speedOverride <<
"\n";
113 return vSafe + speedOverride;
136 std::cout <<
SIMTIME <<
"MSCFModel_ACC::getSecureGap speed=" << speed <<
" leaderSpeed=" << leaderSpeed <<
" desSpacing=" << desSpacing <<
" desSpacingACC=" << desSpacingACC <<
"\n";
143 #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1 145 std::cout <<
"MSCFModel_ACC::insertionFollowSpeed(), speed=" << speed <<
" gap2pred=" << gap2pred <<
" predSpeed=" << predSpeed <<
"\n";
150 const int max_iter = 50;
152 const double tol = 0.1;
153 double damping = 0.8;
156 while (n_iter < max_iter) {
158 const double vCACC =
_v(veh, pred, gap2pred, res, predSpeed, speed,
true);
160 const double a =
MIN2(vCACC, vSafe) - res;
161 res = res + damping * a;
162 #if DEBUG_CACC_INSERTION_FOLLOW_SPEED == 1 164 std::cout <<
" n_iter=" << n_iter <<
" vSafe=" << vSafe <<
" vCACC=" << vCACC <<
" a=" << a <<
" damping=" << damping <<
" res=" << res << std::endl;
195 const double speed,
const double predSpeed,
const double desSpeed,
double vErr,
198 double newSpeed = 0.0;
200 if (pred !=
nullptr) {
203 newSpeed =
acc_CFM.
_v(veh, gap2pred, speed, predSpeed, desSpeed,
true);
206 std::cout <<
" acc control mode" << std::endl;
214 std::cout <<
" CACC control mode" << std::endl;
219 double spacingErr = gap - desSpacing;
221 double spacingErr1 = predSpeed - speed +
myHeadwayTime * accel;
223 if ((spacingErr > 0 && spacingErr < 0.2) && (vErr < 0.1)) {
228 std::cout <<
" applying gap control" << std::endl;
232 }
else if (spacingErr < 0) {
237 std::cout <<
" applying collision avoidance err=" << spacingErr <<
" err1=" << spacingErr1 <<
"\n";
245 std::cout <<
" applying gap closing" << std::endl;
255 std::cout <<
" no leader" << std::endl;
267 const double predSpeed,
const double desSpeed,
const bool )
const {
268 double newSpeed = 0.0;
272 std::cout <<
SIMTIME <<
" MSCFModel_CACC::_v() for veh '" << veh->
getID()
273 <<
" gap=" << gap2pred <<
" speed=" << speed <<
" predSpeed=" << predSpeed
274 <<
" desSpeed=" << desSpeed << std::endl;
279 double vErr = speed - desSpeed;
280 int setControlMode = 0;
287 double time_gap = gap2pred / speed;
291 std::cout <<
" applying speedControl" << std::endl;
297 if (setControlMode) {
300 }
else if (time_gap < 1.5) {
304 std::cout <<
" speedGapControl" << std::endl;
307 newSpeed =
speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred);
309 if (setControlMode) {
319 std::cout <<
" applying speedControl (previous)" << std::endl;
326 std::cout <<
" previous speedGapControl (previous)" << std::endl;
329 newSpeed =
speedGapControl(veh, gap2pred, speed, predSpeed, desSpeed, vErr, pred);
335 <<
" newSpeed=" << newSpeed << std::endl;
339 return MAX2(0., newSpeed);
#define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD
double myGapControlGainSpeed
double _v(const MSVehicle *const veh, const MSVehicle *const pred, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap=true) const
double insertionFollowSpeed(const MSVehicle *const v, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's acceptable speed at insertion.
double maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion=false) const
Returns the maximum safe velocity for following the given leader.
Representation of a vehicle in the micro simulation.
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
MSLane * getLane() const
Returns the lane the vehicle is on.
The car-following model abstraction.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
int CACC_ControlMode
The vehicle's CACC precious time step gap error.
#define DEFAULT_GC_GAIN_GAP_CACC
#define UNUSED_PARAMETER(x)
The car-following model and parameter.
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
double myCollisionAvoidanceGainGapDot
double myGapClosingControlGainGapDot
double speedSpeedContol(const double speed, double vErr) const
#define DEFAULT_GCC_GAIN_GAP_CACC
const MSCFModel & getCarFollowModel() const
Returns the vehicle's car following model definition.
double getActionStepLengthSecs() const
Returns the vehicle's action step length in secs, i.e. the interval between two action points...
virtual int getModelID() const =0
Returns the model's ID; the XML-Tag number is used.
double speedGapControl(const MSVehicle *const veh, const double gap2pred, const double speed, const double predSpeed, const double desSpeed, double vErr, const MSVehicle *const pred) const
virtual double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the minimum gap to reserve if the leader is braking at maximum (>=0)
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
virtual void setHeadwayTime(double headwayTime)
Sets a new value for desired headway [s].
The ACC car-following model.
#define DEBUG_CACC_SECURE_GAP
#define DEFAULT_SC_GAIN_CACC
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
double myCollisionAvoidanceGainGap
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Computes the vehicle's safe speed (no dawdling)
double getMinGap() const
Get the free space in front of vehicles of this class.
MSCFModel_CACC(const MSVehicleType *vtype)
Constructor.
double maximumSafeStopSpeed(double gap, double currentSpeed, bool onInsertion=false, double headway=-1) const
Returns the maximum next velocity for stopping within gap.
const SUMOVTypeParameter & getParameter() const
double getCFParam(const SumoXMLAttr attr, const double defaultValue) const
Returns the named value from the map, or the default if it is not contained there.
#define DEFAULT_GC_GAIN_GAP_DOT_CACC
double getSecureGap(const double speed, const double leaderSpeed, const double leaderMaxDecel) const
Returns the a gap such that the gap mode acceleration of the follower is zero.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
~MSCFModel_CACC()
Destructor.
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
#define DEFAULT_CA_GAIN_GAP_DOT_CACC
double myGapControlGainGap
#define DEFAULT_CA_GAIN_GAP_CACC
double myCollisionMinGapFactor
The factor of minGap that must be maintained to avoid a collision event.
#define DEFAULT_HEADWAYTIME_ACC
double _v(const MSVehicle *const veh, const double gap2pred, const double mySpeed, const double predSpeed, const double desSpeed, const bool respectMinGap=true) const
double myGapClosingControlGainGap
double myHeadwayTime
The driver's desired time headway (aka reaction time tau) [s].
double mySpeedControlGain
const std::string & getID() const
Returns the name of the vehicle.
double getVehicleMaxSpeed(const SUMOTrafficObject *const veh) const
Returns the lane's maximum speed, given a vehicle's speed limit adaptation.
double myGapControlGainSpace
double myGapControlGainGapDot
double stopSpeed(const MSVehicle *const veh, const double speed, double gap2pred) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
#define DEFAULT_GCC_GAIN_GAP_DOT_CACC