37 #define sgn(x) ((x > 0) - (x < 0))
66 throw ProcessError(
TL(
"The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute"));
114 std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->
getID(), left ? +1 : -1);
118 auto followers = libsumo::Vehicle::getNeighbors(veh->
getID(), left ? 0b100 : 0b101);
119 auto leaders = libsumo::Vehicle::getNeighbors(veh->
getID(), left ? 0b110 : 0b111);
120 bool noNeighbors = followers.empty() && leaders.empty();
121 if (!(state.second &
LCA_BLOCKED) && noNeighbors) {
123 for (
auto m = vars->
members.begin(); m != vars->
members.end(); m++) {
124 const std::pair<int, int> mState = libsumo::Vehicle::getLaneChangeState(m->second, left ? +1 : -1);
125 followers = libsumo::Vehicle::getNeighbors(m->second, left ? 0b100 : 0b101);
126 leaders = libsumo::Vehicle::getNeighbors(m->second, left ? 0b110 : 0b111);
127 noNeighbors = followers.empty() && leaders.empty();
142 for (
auto & member : vars->
members) {
143 libsumo::Vehicle::changeLane(member.second, veh->
getLaneIndex() + direction, 0);
150 std::pair<int, int> state = libsumo::Vehicle::getLaneChangeState(veh->
getID(), +1);
151 int traciState = state.first;
158 state = libsumo::Vehicle::getLaneChangeState(veh->
getID(), -1);
159 traciState = state.first;
171 if (currentLane == vars->platoonFixedLane) {
173 vars->platoonFixedLane = -1;
176 bool left = currentLane < vars->platoonFixedLane;
185 double controllerAcceleration;
187 double engineAcceleration;
197 if (s.pars.collision) {
217 controllerAcceleration = std::min(vars->
uMax, std::max(vars->
uMin, controllerAcceleration));
237 return _v(veh, gap2pred, speed, predSpeed);
260 if (gap2pred == -1) {
261 gap2pred = std::numeric_limits<double>().max();
263 return _v(veh, gap2pred, speed, speed + relSpeed);
272 double gap2pred, relSpeed;
274 if (gap2pred == -1) {
275 gap2pred = std::numeric_limits<double>().max();
277 return _v(veh, gap2pred, speed, speed + relSpeed);
326 double ccAcceleration;
328 double accAcceleration;
330 double caccAcceleration;
332 double predAcceleration, leaderAcceleration, leaderSpeed;
346 if (gap2pred > 250 || ccAcceleration < accAcceleration) {
347 controllerAcceleration = ccAcceleration;
349 controllerAcceleration = accAcceleration;
375 controllerAcceleration =
_cacc(veh, egoSpeed, predSpeed, predAcceleration, gap2pred, leaderSpeed, leaderAcceleration, vars->
caccSpacing);
379 controllerAcceleration = 0;
404 controllerAcceleration = std::min(ccAcceleration, caccAcceleration);
425 controllerAcceleration = 0;
454 controllerAcceleration = 0;
460 std::cerr <<
"Switching to normal driver behavior still not implemented in MSCFModel_CC\n";
465 std::cerr <<
"Invalid controller selected in MSCFModel_CC\n";
473 speed =
MAX2(
double(0), egoSpeed +
ACCEL2SPEED(controllerAcceleration));
492 return -1.0 / headwayTime * (egoSpeed - predSpeed + vars->
accLambda * (-gap2pred + headwayTime * egoSpeed + 2));
497 MSCFModel_CC::_cacc(
const MSVehicle* veh,
double egoSpeed,
double predSpeed,
double predAcceleration,
double gap2pred,
double leaderSpeed,
double leaderAcceleration,
double spacing)
const {
500 double epsilon = -gap2pred + spacing;
502 double epsilon_dot = egoSpeed - predSpeed;
512 return (1 / vars->
ploegH * (
534 for (k = min_i; k <= max_i; k++) {
535 d += h[k] * vehicles[0].
speed + vehicles[k].
length + 15;
553 int nCars = vars->
nCars;
561 double actualDistance = 0;
563 double desiredDistance = 0;
565 double speedError = 0;
585 speedError = -vars->
b[
index] * (egoSpeed - vehicles[0].
speed);
588 for (j = 0; j < nCars; j++) {
595 desiredDistance = desiredDistance / d_i;
598 for (j = 0; j < nCars; j++) {
604 double dt =
time - vehicles[j].
time;
609 actualDistance -= vars->
K[
index][j] * vars->
L[
index][j] * distance;
612 actualDistance = actualDistance / (d_i);
615 u_i = (speedError + desiredDistance + actualDistance) / 1000;
622 double gap2pred,
double leaderSpeed)
const {
626 vars->
flatbedKv * (predSpeed - egoSpeed) +
722 std::stringstream warn;
723 warn <<
"MSCFModel_CC: setting a number of cars of " << vars->
nCars <<
" out of a maximum of " <<
MAX_N_CARS <<
724 ". The CONSENSUS controller will not work properly if chosen. If you are using a different controller " <<
725 "you can ignore this warning";
733 buf >>
id >> position;
738 for (
auto item = vars->
members.begin(); item != vars->
members.end(); item++)
739 if (item->second.compare(value) == 0) {
825 switch (engineModel) {
880 std::string leaderId, frontId;
887 throw InvalidArgument(
"Trying to enable auto feeding without providing leader vehicle id");
895 throw InvalidArgument(
"Trying to enable auto feeding without providing front vehicle id");
912 throw InvalidArgument(
"Invalid value '" + value +
"' for parameter '" + key +
"' for vehicle '" + veh->
getID() +
"'");
929 velocity.
x() << velocity.
y() << veh->
getAngle();
933 return vars->
crashed ?
"1" :
"0";
936 double distance, relSpeed;
938 buf << distance << relSpeed;
949 const MSEdge* currentEdge;
953 double positionOnEdge;
955 double distanceToEnd;
959 lastEdge = route->getEdges().back();
961 distanceToEnd = route->getDistanceBetween(positionOnEdge, lastEdge->
getLanes()[0]->getLength(), currentEdge, lastEdge);
963 buf << distanceToEnd;
970 const MSEdge* currentEdge;
974 double positionOnEdge;
976 double distanceFromBegin;
980 firstEdge = route->getEdges().front();
982 distanceFromBegin = route->getDistanceBetween(0, positionOnEdge, firstEdge, currentEdge);
984 buf << distanceFromBegin;
1033 buf << (gear + 1) << rpm;
1068 std::pair<std::string, double> l = libsumo::Vehicle::getLeader(veh->
getID(), 250);
1073 distance = l.second;
1081 double distance, relSpeed;
#define ENGINE_PAR_XMLFILE
#define PAR_FIXED_ACCELERATION
#define PAR_REMOVE_MEMBER
#define CC_PAR_CACC_OMEGA_N
#define CC_PAR_FLATBED_KP
#define PAR_FRONT_FAKE_DATA
#define PAR_SPEED_AND_ACCELERATION
#define CC_ENGINE_MODEL_REALISTIC
#define PAR_PLATOON_FIXED_LANE
#define CC_PAR_FLATBED_KV
#define PAR_USE_PREDICTION
#define PAR_DISTANCE_TO_END
#define CC_ENGINE_MODEL_FOLM
#define CC_PAR_VEHICLE_MODEL
#define PAR_PRECEDING_SPEED_AND_ACCELERATION
#define PAR_ACTIVE_CONTROLLER
#define PAR_LEADER_SPEED_AND_ACCELERATION
#define CC_PAR_PLATOON_SIZE
#define CC_PAR_VEHICLE_POSITION
#define CC_PAR_FLATBED_KA
#define CC_PAR_VEHICLE_ENGINE_MODEL
#define PAR_USE_AUTO_FEEDING
#define PAR_CC_DESIRED_SPEED
#define CC_PAR_VEHICLES_FILE
#define PAR_LEADER_FAKE_DATA
#define PAR_ACC_HEADWAY_TIME
#define PAR_DISTANCE_FROM_BEGIN
#define ENGINE_PAR_VEHICLE
#define CC_PAR_ENGINE_TAU
#define PAR_USE_CONTROLLER_ACCELERATION
#define PAR_ACC_ACCELERATION
#define CC_PAR_VEHICLE_DATA
#define PAR_ENABLE_AUTO_LANE_CHANGE
#define WRITE_WARNING(msg)
std::shared_ptr< const MSRoute > ConstMSRoutePtr
@ LCA_KEEPRIGHT
The action is due to the default of keeping right "Rechtsfahrgebot".
@ LCA_BLOCKED
blocked in all directions
@ LCA_LEFT
Wants go to the left.
@ LCA_SPEEDGAIN
The action is due to the wish to be faster (tactical lc)
@ LCA_RIGHT
Wants go to the right.
@ SUMO_ATTR_CF_CC_FLATBED_KP
@ SUMO_ATTR_CF_CC_FLATBED_D
@ SUMO_ATTR_CF_CC_FLATBED_KA
@ SUMO_ATTR_CF_CC_PLOEG_KP
@ SUMO_ATTR_CF_CC_PLOEG_H
@ SUMO_ATTR_CF_CC_CCACCEL
@ SUMO_ATTR_CF_CC_PLOEG_KD
@ SUMO_ATTR_CF_CC_CCDECEL
@ SUMO_ATTR_CF_CC_FLATBED_H
@ SUMO_ATTR_CF_CC_LANES_COUNT
@ SUMO_ATTR_CF_CC_CONSTSPACING
@ SUMO_ATTR_CF_CC_FLATBED_KV
#define UNUSED_PARAMETER(x)
int nInitialized
count of initialized vehicles
int platoonFixedLane
whole platoon lane change (not automatic). -1 indicates no need to change lane (mechanism disabled)
bool autoFeed
determines whether CACC should automatically fetch data about other vehicles
struct FAKE_CONTROLLER_DATA fakeData
fake controller data.
MSVehicle * frontVehicle
front sumo id, used for auto feeding
double accHeadwayTime
headway time for ACC
bool initialized[MAX_N_CARS]
tells whether data about a certain vehicle has been initialized
double frontSpeed
current front vehicle speed
double K[MAX_N_CARS][MAX_N_CARS]
K matrix.
struct Plexe::VEHICLE_DATA vehicles[MAX_N_CARS]
data about vehicles in the platoon
int position
my position within the platoon (0 = first car)
double frontAcceleration
current front vehicle acceleration (used by CACC)
bool frontInitialized
@did we receive at least one packet?
double leaderDataReadTime
when leader data has been readed from GPS
bool usePrediction
enable/disable data prediction (interpolation) for missing data
std::map< int, std::string > members
list of members belonging to my platoon
double leaderControllerAcceleration
platoon's leader controller acceleration (used by CACC)
double controllerAcceleration
acceleration as computed by the controller, to be sent to other vehicles
int L[MAX_N_CARS][MAX_N_CARS]
L matrix.
double caccXi
controller related parameters
double frontControllerAcceleration
front vehicle controller acceleration (used by CACC)
double b[MAX_N_CARS]
vector of damping ratios b
GenericEngineModel * engine
engine model employed by this car
int nCars
number of cars in the platoon
double leaderSpeed
platoon's leader speed (used by CACC)
double leaderAngle
platoon's leader angle in radians
enum Plexe::ACTIVE_CONTROLLER activeController
currently active controller
bool leaderInitialized
@did we receive at least one packet?
double caccSpacing
fixed spacing for CACC
Position frontVelocity
front vehicle velocity vector
double ccDesiredSpeed
CC desired speed.
double h[MAX_N_CARS]
vector of time headways h
Position frontPosition
current front vehicle position
double leaderAcceleration
platoon's leader acceleration (used by CACC)
bool autoLaneChange
automatic whole platoon lane change
Position leaderVelocity
platoon's leader velocity vector
double frontAngle
front vehicle angle in radians
double frontDataReadTime
when front vehicle data has been readed from GPS
int engineModel
numeric value indicating the employed model
MSVehicle * leaderVehicle
leader vehicle, used for auto feeding
Position leaderPosition
platoon's leader position
bool useControllerAcceleration
determines whether PATH's CACC should use the real vehicle acceleration or the controller computed on...
void setMaximumAcceleration(double maxAcc)
virtual double getRealAcceleration(double speed_mps, double accel_mps2, double reqAccel_mps2, SUMOTime timeStep=0)=0
void setMaximumDeceleration(double maxDec)
virtual void setParameter(const std::string parameter, const std::string &value)=0
ConstMSRoutePtr getRoutePtr() const
Returns the current route.
void setChosenSpeedFactor(const double factor)
Returns the precomputed factor by which the driver wants to be faster than the speed limit.
const std::list< MSStop > & getStops() const
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
double _v(const MSVehicle *const veh, double gap2pred, double egoSpeed, double predSpeed) const
MSCFModel_CC(const MSVehicleType *vtype)
Constructor.
virtual std::string getParameter(const MSVehicle *veh, const std::string &key) const
set the information about a generic car. This method should be invoked by TraCI when a wireless messa...
virtual double insertionFollowSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const
Overload base MSCFModel::insertionFollowSpeed method to inject automated vehicles as soon as they are...
const double myPloegH
Ploeg's CACC parameters.
double getACCAcceleration(const MSVehicle *veh) const
returns the ACC computed acceleration when the faked CACC is controlling the car. This can be used to...
int getMyLanesCount() const
returns the number of lanes set in the configuration file
double _flatbed(const MSVehicle *veh, double egoAcceleration, double egoSpeed, double predSpeed, double gap2pred, double leaderSpeed) const
flatbed platoon towing model
void getVehicleInformation(const MSVehicle *veh, double &speed, double &acceleration, double &controllerAcceleration, Position &position, double &time) const
get the information about a vehicle. This can be used by TraCI in order to get speed and acceleration...
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
VehicleVariables * createVehicleVariables() const
Returns model specific values which are stored inside a vehicle and must be used with casting.
void getRadarMeasurements(const MSVehicle *veh, double &distance, double &relativeSpeed) const
return the data that is currently being measured by the radar
double _consensus(const MSVehicle *veh, double egoSpeed, Position egoPosition, double time) const
controller based on consensus strategy
double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed (no dawdling)
double _ploeg(const MSVehicle *veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred) const
controller for the Ploeg's CACC which computes the control input variation. Opposed to other controll...
void performPlatoonLaneChange(MSVehicle *const veh) const
void changeWholePlatoonLane(MSVehicle *const veh, int direction) const
const double myTau
engine time constant used for actuation lag
double d_i_j(const struct Plexe::VEHICLE_DATA *vehicles, const double h[MAX_N_CARS], int i, int j) const
computes the desired distance between vehicle i and vehicle j
bool isPlatoonLaneChangeSafe(MSVehicle *const veh, bool left) const
const double myOmegaN
design constant for CACC
double stopSpeed(const MSVehicle *const veh, const double speed, double gap2pred, double decel, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
const double myC1
design constant for CACC
const int myLanesCount
number of lanes in the highway, in the absence of on-/off-ramps. This is used to move to the correct ...
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
double _cacc(const MSVehicle *veh, double egoSpeed, double predSpeed, double predAcceleration, double gap2pred, double leaderSpeed, double leaderAcceleration, double spacing) const
controller for the CACC which computes the acceleration to be applied. the value needs to be passed t...
virtual void setParameter(MSVehicle *veh, const std::string &key, const std::string &value) const
try to set the given parameter for this carFollowingModel
const double myConstantSpacing
the constant gap for CACC
double getCACCConstantSpacing(const MSVehicle *veh) const
returns CACC desired constant spacing
~MSCFModel_CC()
Destructor.
void recomputeParameters(const MSVehicle *veh) const
Recomputes controller related parameters after setting them.
const double myXi
design constant for CACC
const double myFlatbedKa
flatbed CACC parameters
void switchOnACC(const MSVehicle *veh, double ccDesiredSpeed) const
switch on the ACC, so disabling the human driver car control
MSCFModel * myHumanDriver
the car following model which drives the car when automated cruising is disabled, i....
const double myLambda
design constant for ACC
void performAutoLaneChange(MSVehicle *const veh) const
const double myKp
design constant for CC
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
const double myCcAccel
The maximum acceleration that the CC can output.
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
const double myCcDecel
The maximum deceleration that the CC can output.
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
void resetConsensus(const MSVehicle *veh) const
Resets the consensus controller. In particular, sets the "initialized" vector all to false....
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
enum Plexe::ACTIVE_CONTROLLER getActiveController(const MSVehicle *veh) const
return the currently active controller
double _cc(const MSVehicle *veh, double egoSpeed, double desSpeed) const
controller for the CC which computes the acceleration to be applied. the value needs to be passed to ...
double _acc(const MSVehicle *veh, double egoSpeed, double predSpeed, double gap2pred, double headwayTime) const
controller for the ACC which computes the acceleration to be applied. the value needs to be passed to...
Krauss car-following model, with acceleration decrease and faster start.
The car-following model abstraction.
virtual double interactionGap(const MSVehicle *const veh, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed without a leader.
virtual double minNextSpeed(double speed, const MSVehicle *const veh=0) const
Returns the minimum speed given the current speed (depends on the numerical update scheme and its ste...
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
CalcReason
What the return value of stop/follow/free-Speed is used for.
double myDecel
The vehicle's maximum deceleration [m/s^2].
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double myAccel
The vehicle's maximum acceleration [m/s^2].
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0, const CalcReason usage=CalcReason::CURRENT) const =0
Computes the vehicle's follow speed (no dawdling)
double stopSpeed(const MSVehicle *const veh, const double speed, double gap, const CalcReason usage=CalcReason::CURRENT) const
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling)
A road/street connecting two junctions.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
MSEdge & getEdge() const
Returns the lane's edge.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
void setSpeedMode(int speedMode)
Sets speed-constraining behaviors.
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Representation of a vehicle in the micro simulation.
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
Influencer & getInfluencer()
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
double getSpeed() const
Returns the vehicle's current speed.
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
double getAngle() const
Returns the vehicle's direction in radians.
Position getVelocityVector() const
Returns the vehicle's direction in radians.
The car-following model and parameter.
const std::string & getID() const
Returns the id.
A point in 2D or 3D with translation and scaling methods.
void setx(double x)
set position x
void set(double x, double y)
set positions x and y
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
double x() const
Returns the x-position.
void sety(double y)
set position y
double y() const
Returns the y-position.
void getEngineData(double speed_mps, int &gear, double &rpm)
virtual double getSpeed() const =0
Returns the object's current speed.
Representation of a vehicle.
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter
static int toInt(const std::string &sData)
converts a string into the integer value described by it by calling the char-type converter,...
An error which allows to continue.
ACTIVE_CONTROLLER
Determines the currently active controller, i.e., ACC, CACC, or the driver. In future we might need t...
double leaderAcceleration
double leaderControllerAcceleration
double frontControllerAcceleration