36 #define sgn(x) ((x > 0) - (x < 0)) 64 std::cerr <<
"The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute\n";
65 WRITE_ERROR(
"The number of lanes needs to be specified in the attributes of carFollowing-CC with the \"lanesCount\" attribute");
117 int traciState = state.first;
123 for (
auto m = vars->
members.begin(); m != vars->
members.end(); m++) {
132 for (
auto m = vars->
members.begin(); m != vars->
members.end(); m++) {
140 traciState = state.first;
146 for (
auto m = vars->
members.begin(); m != vars->
members.end(); m++) {
155 for (
auto m = vars->
members.begin(); m != vars->
members.end(); m++) {
169 double controllerAcceleration;
171 double engineAcceleration;
180 std::list<MSVehicle::Stop> stops = veh->
getMyStops();
197 controllerAcceleration = std::min(vars->
uMax, std::max(vars->
uMin, controllerAcceleration));
217 return _v(veh, gap2pred, speed, predSpeed);
238 double gap2pred, relSpeed;
240 if (gap2pred == -1) {
241 gap2pred = std::numeric_limits<double>().max();
243 return _v(veh, gap2pred, speed, speed + relSpeed);
252 double gap2pred, relSpeed;
254 if (gap2pred == -1) {
255 gap2pred = std::numeric_limits<double>().max();
257 return _v(veh, gap2pred, speed, speed + relSpeed);
306 double ccAcceleration;
308 double accAcceleration;
310 double caccAcceleration;
312 double predAcceleration, leaderAcceleration, leaderSpeed;
326 if (gap2pred > 250 || ccAcceleration < accAcceleration) {
327 controllerAcceleration = ccAcceleration;
329 controllerAcceleration = accAcceleration;
355 controllerAcceleration =
_cacc(veh, egoSpeed, predSpeed, predAcceleration, gap2pred, leaderSpeed, leaderAcceleration, vars->
caccSpacing);
359 controllerAcceleration = 0;
384 controllerAcceleration = std::min(ccAcceleration, caccAcceleration);
405 controllerAcceleration = 0;
434 controllerAcceleration = 0;
440 std::cerr <<
"Switching to normal driver behavior still not implemented in MSCFModel_CC\n";
445 std::cerr <<
"Invalid controller selected in MSCFModel_CC\n";
453 speed =
MAX2(
double(0), egoSpeed +
ACCEL2SPEED(controllerAcceleration));
472 return -1.0 / headwayTime * (egoSpeed - predSpeed + vars->
accLambda * (-gap2pred + headwayTime * egoSpeed + 2));
477 MSCFModel_CC::_cacc(
const MSVehicle* veh,
double egoSpeed,
double predSpeed,
double predAcceleration,
double gap2pred,
double leaderSpeed,
double leaderAcceleration,
double spacing)
const {
480 double epsilon = -gap2pred + spacing;
482 double epsilon_dot = egoSpeed - predSpeed;
492 return (1 / vars->
ploegH * (
514 for (k = min_i; k <= max_i; k++) {
515 d += h[k] * vehicles[0].
speed + vehicles[k].
length + 15;
533 int nCars = vars->
nCars;
541 double actualDistance = 0;
543 double desiredDistance = 0;
545 double speedError = 0;
565 speedError = -vars->
b[
index] * (egoSpeed - vehicles[0].
speed);
568 for (j = 0; j < nCars; j++) {
573 desiredDistance -= vars->
K[
index][j] * vars->
L[
index][j] *
d_i_j(vehicles, vars->
h, index, j);
575 desiredDistance = desiredDistance / d_i;
578 for (j = 0; j < nCars; j++) {
584 double dt = time - vehicles[j].
time;
588 double distance = egoPosition.
distanceTo2D(otherPosition) *
sgn(j - index);
589 actualDistance -= vars->
K[
index][j] * vars->
L[
index][j] * distance;
592 actualDistance = actualDistance / (d_i);
595 u_i = (speedError + desiredDistance + actualDistance) / 1000;
602 double gap2pred,
double leaderSpeed)
const {
606 vars->
flatbedKv * (predSpeed - egoSpeed) +
702 std::stringstream warn;
703 warn <<
"MSCFModel_CC: setting a number of cars of " << vars->
nCars <<
" out of a maximum of " <<
MAX_N_CARS <<
704 ". The CONSENSUS controller will not work properly if chosen. If you are using a different controller " <<
705 "you can ignore this warning";
713 buf >>
id >> position;
718 for (
auto item = vars->
members.begin(); item != vars->
members.end(); item++)
719 if (item->second.compare(value) == 0) {
795 switch (engineModel) {
850 std::string leaderId, frontId;
857 throw InvalidArgument(
"Trying to enable auto feeding without providing leader vehicle id");
865 throw InvalidArgument(
"Trying to enable auto feeding without providing front vehicle id");
882 throw InvalidArgument(
"Invalid value '" + value +
"' for parameter '" + key +
"' for vehicle '" + veh->
getID() +
"'");
899 velocity.
x() << velocity.
y() << veh->
getAngle();
903 return vars->
crashed ?
"1" :
"0";
906 double distance, relSpeed;
908 buf << distance << relSpeed;
919 const MSEdge* currentEdge;
923 double positionOnEdge;
925 double distanceToEnd;
929 lastEdge = route->
getEdges().back();
933 buf << distanceToEnd;
940 const MSEdge* currentEdge;
944 double positionOnEdge;
946 double distanceFromBegin;
950 firstEdge = route->
getEdges().front();
952 distanceFromBegin = route->
getDistanceBetween(0, positionOnEdge, firstEdge, currentEdge);
954 buf << distanceFromBegin;
982 if (index >= vars->
nCars || index < 0) {
1003 buf << (gear + 1) << rpm;
1043 distance = l.second;
1051 double distance, relSpeed;
bool initialized[MAX_N_CARS]
tells whether data about a certain vehicle has been initialized
#define ENGINE_PAR_VEHICLE
#define CC_PAR_CACC_OMEGA_N
#define PAR_ACC_HEADWAY_TIME
#define PAR_CC_DESIRED_SPEED
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false) const
Computes the vehicle's safe speed without a leader.
double frontControllerAcceleration
#define CC_ENGINE_MODEL_REALISTIC
MSEdge & getEdge() const
Returns the lane's edge.
int engineModel
numeric value indicating the employed model
const double myXi
design constant for CACC
Representation of a vehicle in the micro simulation.
MSCFModel::VehicleVariables * getCarFollowVariables() const
Returns the vehicle's car following model variables.
#define PAR_DISTANCE_FROM_BEGIN
bool frontInitialized
we receive at least one packet?
~MSCFModel_CC()
Destructor.
double getAngle() const
Returns the vehicle's direction in radians.
The action is due to the default of keeping right "Rechtsfahrgebot".
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences.
const ConstMSEdgeVector & getEdges() const
#define CC_PAR_FLATBED_KP
#define PAR_PRECEDING_SPEED_AND_ACCELERATION
#define PAR_ACTIVE_CONTROLLER
struct FAKE_CONTROLLER_DATA fakeData
fake controller data.
Position getVelocityVector() const
Returns the vehicle's direction in radians.
#define PAR_USE_PREDICTION
virtual double freeSpeed(const MSVehicle *const veh, double speed, double seen, double maxSpeed, const bool onInsertion=false) const
Computes the vehicle's safe speed without a leader.
MSLane * getLane() const
Returns the lane the vehicle is on.
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...
double ccDesiredSpeed
CC desired speed.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
std::list< Stop > getMyStops()
virtual double maxNextSpeed(double speed, const MSVehicle *const veh) const
Returns the maximum speed given the current speed.
#define PAR_USE_AUTO_FEEDING
double y() const
Returns the y-position.
The car-following model abstraction.
double b[MAX_N_CARS]
vector of damping ratios b
double frontSpeed
current front vehicle speed
double leaderControllerAcceleration
double getPositionOnLane() const
Get the vehicle's position along the lane.
GenericEngineModel * engine
engine model employed by this car
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
virtual void setParameter(MSVehicle *veh, const std::string &key, const std::string &value) const
try to set the given parameter for this carFollowingModel
#define CC_PAR_PLATOON_SIZE
double x() const
Returns the x-position.
#define CC_PAR_FLATBED_KA
double leaderAngle
platoon's leader angle in radians
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
double myAccel
The vehicle's maximum acceleration [m/s^2].
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
MSVehicle * leaderVehicle
leader vehicle, used for auto feeding
Position leaderVelocity
platoon's leader velocity vector
const MSRoute & getRoute() const
Returns the current route.
Position getPosition(const double offset=0) const
Return current position (x/y, cartesian)
virtual void setParameter(const std::string parameter, const std::string &value)=0
double getACCAcceleration(const MSVehicle *veh) const
returns the ACC computed acceleration when the faked CACC is controlling the car. This can be used to...
#define CC_PAR_VEHICLE_MODEL
double leaderAcceleration
platoon's leader acceleration (used by CACC)
void set(double x, double y)
set positions x and y
double frontAngle
front vehicle angle in radians
double accHeadwayTime
headway time for ACC
The action is due to the wish to be faster (tactical lc)
MSCFModel * myHumanDriver
the car following model which drives the car when automated cruising is disabled, i...
const double myC1
design constant for CACC
#define UNUSED_PARAMETER(x)
virtual double finalizeSpeed(MSVehicle *const veh, double vPos) const
Applies interaction with stops and lane changing model influences. Called at most once per simulation...
#define WRITE_WARNING(msg)
The car-following model and parameter.
double _consensus(const MSVehicle *veh, double egoSpeed, Position egoPosition, double time) const
controller based on consensus strategy
double caccSpacing
fixed spacing for CACC
static std::pair< int, int > getLaneChangeState(const std::string &vehicleID, int direction)
double _v(const MSVehicle *const veh, double gap2pred, double egoSpeed, double predSpeed) const
double getMaxAccel() const
Get the vehicle type's maximum acceleration [m/s^2].
double interactionGap(const MSVehicle *const, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
#define CC_PAR_ENGINE_TAU
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 ...
A road/street connecting two junctions.
int nInitialized
count of initialized vehicles
#define PAR_FRONT_FAKE_DATA
const double myOmegaN
design constant for CACC
void recomputeParameters(const MSVehicle *veh) const
Recomputes controller related parameters after setting them.
void setSpeedMode(int speedMode)
Sets speed-constraining behaviors.
void switchOnACC(const MSVehicle *veh, double ccDesiredSpeed) const
switch on the ACC, so disabling the human driver car control
double K[MAX_N_CARS][MAX_N_CARS]
K matrix.
blocked in all directions
double frontAcceleration
current front vehicle acceleration (used by CACC)
#define PAR_FIXED_ACCELERATION
Position frontPosition
current front vehicle position
bool useControllerAcceleration
determines whether PATH's CACC should use the real vehicle acceleration or the controller computed on...
int nCars
number of cars in the platoon
Representation of a vehicle.
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...
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
A point in 2D or 3D with translation and scaling methods.
#define CC_PAR_VEHICLE_ENGINE_MODEL
void setx(double x)
set position x
MSCFModel_CC(const MSVehicleType *vtype)
Constructor.
#define CC_PAR_VEHICLES_FILE
#define PAR_LEADER_FAKE_DATA
#define PAR_USE_CONTROLLER_ACCELERATION
const MSEdge * getEdge() const
Returns the edge the vehicle is currently at.
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Position leaderPosition
platoon's leader position
const double myLambda
design constant for ACC
VehicleVariables * createVehicleVariables() const
Returns model specific values which are stored inside a vehicle and must be used with casting...
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...
MSCFModel * duplicate(const MSVehicleType *vtype) const
Duplicates the car-following model.
double controllerAcceleration
acceleration as computed by the controller, to be sent to other vehicles
std::map< int, std::string > members
list of members belonging to my platoon
double getSpeedLimit() const
Returns the lane's maximum allowed speed.
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...
#define ENGINE_PAR_XMLFILE
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
double leaderSpeed
platoon's leader speed (used by CACC)
virtual double interactionGap(const MSVehicle *const veh, double vL) const
Returns the maximum gap at which an interaction between both vehicles occurs.
enum Plexe::ACTIVE_CONTROLLER activeController
currently active controller
int getMyLanesCount() const
returns the number of lanes set in the configuration file
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 _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...
static void changeLane(const std::string &vehicleID, int laneIndex, double duration)
double frontDataReadTime
when front vehicle data has been readed from GPS
Position frontVelocity
front vehicle velocity vector
double leaderControllerAcceleration
platoon's leader controller acceleration (used by CACC)
static int toInt(const std::string &sData)
converts a string into the integer value described by it by calling the char-type converter...
void setChosenSpeedFactor(const double factor)
Returns the precomputed factor by which the driver wants to be faster than the speed limit...
double _flatbed(const MSVehicle *veh, double egoAcceleration, double egoSpeed, double predSpeed, double gap2pred, double leaderSpeed) const
flatbed platoon towing model
#define PAR_LEADER_SPEED_AND_ACCELERATION
void performAutoLaneChange(MSVehicle *const veh) const
void setMaximumDeceleration(double maxAcceleration_mpsps)
virtual double getRealAcceleration(double speed_mps, double accel_mps2, double reqAccel_mps2, SUMOTime timeStep=0)=0
double frontControllerAcceleration
front vehicle controller acceleration (used by CACC)
double myDecel
The vehicle's maximum deceleration [m/s^2].
#define CC_PAR_VEHICLE_DATA
MSVehicle * frontVehicle
front sumo id, used for auto feeding
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 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 myCcDecel
The maximum deceleration that the CC can output.
Influencer & getInfluencer()
Returns the velocity/lane influencer.
double processNextStop(double currentVelocity)
Processes stops, returns the velocity needed to reach the stop.
const double myFlatbedKa
flatbed CACC parameters
double caccXi
controller related parameters
#define CC_PAR_FLATBED_KV
void setMaximumAcceleration(double maxAcceleration_mpsps)
const double myConstantSpacing
the constant gap for CACC
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...
#define PAR_SPEED_AND_ACCELERATION
int position
my position within the platoon (0 = first car)
#define PAR_REMOVE_MEMBER
double getAcceleration() const
Returns the vehicle's acceleration in m/s (this is computed as the last step's mean acceleration in c...
void sety(double y)
set position y
static std::pair< std::string, double > getLeader(const std::string &vehicleID, double dist=0.)
double h[MAX_N_CARS]
vector of time headways h
enum Plexe::ACTIVE_CONTROLLER getActiveController(const MSVehicle *veh) const
return the currently active controller
struct Plexe::VEHICLE_DATA vehicles[MAX_N_CARS]
data about vehicles in the platoon
double getCACCConstantSpacing(const MSVehicle *veh) const
returns CACC desired constant spacing
bool autoLaneChange
automatic whole platoon lane change
#define PAR_ENABLE_AUTO_LANE_CHANGE
bool leaderInitialized
we receive at least one packet?
const double myTau
engine time constant used for actuation lag
int L[MAX_N_CARS][MAX_N_CARS]
L matrix.
const double myPloegH
Ploeg's CACC parameters.
ACTIVE_CONTROLLER
Determines the currently active controller, i.e., ACC, CACC, or the driver. In future we might need t...
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...
void getEngineData(double speed_mps, int &gear, double &rpm)
void resetConsensus(const MSVehicle *veh) const
Resets the consensus controller. In particular, sets the "initialized" vector all to false...
const double myCcAccel
The maximum acceleration that the CC can output.
#define PAR_DISTANCE_TO_END
#define CC_ENGINE_MODEL_FOLM
#define PAR_ACC_ACCELERATION
#define CC_PAR_VEHICLE_POSITION
const double myKp
design constant for CC
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
double getSpeed() const
Returns the vehicle's current speed.
double leaderAcceleration
Krauss car-following model, with acceleration decrease and faster start.
virtual double followSpeed(const MSVehicle *const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle *const pred=0) const =0
Computes the vehicle's follow speed (no dawdling)
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) ...
const std::string & getID() const
Returns the name of the vehicle.
virtual double getSpeed() const =0
Returns the vehicle's current speed.
bool autoFeed
determines whether CACC should automatically fetch data about other vehicles
double leaderDataReadTime
when leader data has been readed from GPS
void getRadarMeasurements(const MSVehicle *veh, double &distance, double &relativeSpeed) const
return the data that is currently being measured by the radar
virtual double stopSpeed(const MSVehicle *const veh, const double speed, double gap) const =0
Computes the vehicle's safe speed for approaching a non-moving obstacle (no dawdling) ...
bool usePrediction
enable/disable data prediction (interpolation) for missing data
double getDistanceBetween(double fromPos, double toPos, const MSEdge *fromEdge, const MSEdge *toEdge, bool includeInternal=true, int routePosition=0) const
Compute the distance between 2 given edges on this route, including the length of internal lanes...