Merging Plexe platooning models into official SUMO release

CMakeLists.txt

@@ -62,7 +62,7 @@ endif ()
 # special debug flags
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
 if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
-    set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -stdlib=libstdc++ -fsanitize=undefined,address,integer,unsigned-integer-overflow -fno-omit-frame-pointer -fsanitize-blacklist=${CMAKE_SOURCE_DIR}/build/clang_sanitize_blacklist.txt")
+    set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined,address,integer,unsigned-integer-overflow -fno-omit-frame-pointer -fsanitize-blacklist=${CMAKE_SOURCE_DIR}/build/clang_sanitize_blacklist.txt")
 endif ()
 
 # we need to build position independent code when generating a shared library

src/gui/TraCIServerAPI_GUI.cpp

@@ -48,7 +48,8 @@ TraCIServerAPI_GUI::processGet(TraCIServer& server, tcpip::Storage& inputStorage
     std::string id = inputStorage.readString();
     // check variable
     if (variable != TRACI_ID_LIST && variable != VAR_VIEW_ZOOM && variable != VAR_VIEW_OFFSET
-            && variable != VAR_VIEW_SCHEMA && variable != VAR_VIEW_BOUNDARY && variable != VAR_HAS_VIEW) {
+            && variable != VAR_VIEW_SCHEMA && variable != VAR_VIEW_BOUNDARY && variable != VAR_HAS_VIEW
+            && variable != VAR_TRACK_VEHICLE) {
         return server.writeErrorStatusCmd(CMD_GET_GUI_VARIABLE, "Get GUI Variable: unsupported variable " + toHex(variable, 2) + " specified", outputStorage);
     }
     // begin response building
@@ -91,10 +92,30 @@ TraCIServerAPI_GUI::processGet(TraCIServer& server, tcpip::Storage& inputStorage
                 tempMsg.writeDouble(b.ymax());
                 break;
             }
-            case VAR_HAS_VIEW:
+            case VAR_HAS_VIEW: {
                 tempMsg.writeUnsignedByte(TYPE_INTEGER);
                 tempMsg.writeInt(v != nullptr ? 1 : 0);
                 break;
+            }
+            case VAR_TRACK_VEHICLE: {
+                GUIVehicle* gv = 0;
+                std::string id;
+                GUIGlID gid = v->getTrackedID();
+                if (gid != GUIGlObject::INVALID_ID) {
+                    gv = static_cast<GUIVehicle*>(GUIGlObjectStorage::gIDStorage.getObjectBlocking(gid));
+                }
+                if (gv == 0) {
+                    id = "";
+                } else {
+                    id = gv->getID();
+                }
+                tempMsg.writeUnsignedByte(TYPE_STRING);
+                tempMsg.writeString(id);
+                if (gid != GUIGlObject::INVALID_ID) {
+                    GUIGlObjectStorage::gIDStorage.unblockObject(gid);
+                }
+                break;
+            }
             default:
                 break;
         }

src/libsumo/Vehicle.cpp

@@ -613,6 +613,13 @@ Vehicle::getParameter(const std::string& vehicleID, const std::string& key) {
         } catch (InvalidArgument& e) {
             throw TraCIException("Vehicle '" + vehicleID + "' does not support laneChangeModel parameter '" + key + "' (" + e.what() + ").");
         }
+    } else if (StringUtils::startsWith(key, "carFollowModel.")) {
+        const std::string attrName = key.substr(15);
+        try {
+            return veh->getCarFollowModel().getParameter(veh, attrName);
+        } catch (InvalidArgument& e) {
+            throw TraCIException("Vehicle '" + vehicleID + "' does not support carFollowModel parameter '" + key + "' (" + e.what() + ").");
+        }
     } else if (StringUtils::startsWith(key, "has.") && StringUtils::endsWith(key, ".device")) {
         StringTokenizer tok(key, ".");
         if (tok.size() != 3) {
@@ -1549,6 +1556,13 @@ Vehicle::setParameter(const std::string& vehicleID, const std::string& key, cons
         } catch (InvalidArgument& e) {
             throw TraCIException("Vehicle '" + vehicleID + "' does not support laneChangeModel parameter '" + key + "' (" + e.what() + ").");
         }
+    } else if (StringUtils::startsWith(key, "carFollowModel.")) {
+        const std::string attrName = key.substr(15);
+        try {
+            veh->getCarFollowModel().setParameter(veh, attrName, value);
+        } catch (InvalidArgument& e) {
+            throw TraCIException("Vehicle '" + vehicleID + "' does not support carFollowModel parameter '" + key + "' (" + e.what() + ").");
+        }
     } else if (StringUtils::startsWith(key, "has.") && StringUtils::endsWith(key, ".device")) {
         StringTokenizer tok(key, ".");
         if (tok.size() != 3) {

src/microsim/MSLane.cpp

@@ -66,6 +66,7 @@
 #include "MSVehicleControl.h"
 #include "MSLeaderInfo.h"
 #include "MSVehicle.h"
+#include "cfmodels/MSCFModel_CC.h"
 
 //#define DEBUG_INSERTION
 //#define DEBUG_PLAN_MOVE
@@ -1505,6 +1506,14 @@ MSLane::handleCollisionBetween(SUMOTime timestep, const std::string& stage, MSVe
         std::swap(collider, victim);
     }
     std::string prefix = "Vehicle '" + collider->getID() + "'; " + collisionType + " with vehicle '" + victim->getID() ;
+    const MSCFModel_CC* model = dynamic_cast<const MSCFModel_CC*>(&collider->getCarFollowModel());
+    if (model) {
+        model->setCrashed(collider, true);
+    }
+    model = dynamic_cast<const MSCFModel_CC*>(&victim->getCarFollowModel());
+    if (model) {
+        model->setCrashed(victim, true, true);
+    }
     if (myCollisionStopTime > 0) {
         if (collider->collisionStopTime() >= 0 && victim->collisionStopTime() >= 0) {
             return;

src/microsim/MSVehicleType.cpp

@@ -47,6 +47,7 @@
 #include "cfmodels/MSCFModel_ACC.h"
 #include "cfmodels/MSCFModel_CACC.h"
 #include "MSVehicleControl.h"
+#include "cfmodels/MSCFModel_CC.h"
 #include "MSVehicleType.h"
 
 
@@ -331,6 +332,9 @@ MSVehicleType::build(SUMOVTypeParameter& from) {
         case SUMO_TAG_CF_CACC:
             vtype->myCarFollowModel = new MSCFModel_CACC(vtype);
             break;
+        case SUMO_TAG_CF_CC:
+            vtype->myCarFollowModel = new MSCFModel_CC(vtype);
+            break;
         case SUMO_TAG_CF_KRAUSS:
         default:
             vtype->myCarFollowModel = new MSCFModel_Krauss(vtype);

src/microsim/cfmodels/CC_Const.h

@@ -0,0 +1,184 @@
+/****************************************************************************/
+// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
+// Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
+// This program and the accompanying materials
+// are made available under the terms of the Eclipse Public License v2.0
+// which accompanies this distribution, and is available at
+// http://www.eclipse.org/legal/epl-v20.html
+// SPDX-License-Identifier: EPL-2.0
+/****************************************************************************/
+/// @file    CC_Const.h
+/// @author  Michele Segata
+/// @date    Fri, 11 Apr 2014
+/// @version $Id$
+///
+// File defining constants, structs, and enums for cruise controllers
+/****************************************************************************/
+#ifndef CC_CONST_H
+#define CC_CONST_H
+
+#include <string>
+#include <sstream>
+
+namespace Plexe {
+
+/**
+ * @brief action that might be requested by the platooning management
+ */
+enum PLATOONING_LANE_CHANGE_ACTION {
+    DRIVER_CHOICE = 0,        //the platooning management is not active, so just let the driver choose the lane
+    STAY_IN_CURRENT_LANE = 3, //the car is part of a platoon, so it has to stay on the dedicated platooning lane
+    MOVE_TO_FIXED_LANE = 4    //move the car to a specific lane
+};
+
+/**
+ * @brief TraCI modes for lane changing
+ */
+#define FIX_LC 0b1000000000
+#define DEFAULT_NOTRACI_LC 0b1010101010
+
+/** @enum ACTIVE_CONTROLLER
+ * @brief Determines the currently active controller, i.e., ACC, CACC, or the
+ * driver. In future we might need to switch off the automatic controller and
+ * leave the control to the mobility model which reproduces a human driver
+ */
+enum ACTIVE_CONTROLLER
+{DRIVER = 0, ACC = 1, CACC = 2, FAKED_CACC = 3, PLOEG = 4, CONSENSUS = 5, FLATBED = 6};
+
+/**
+ * @brief struct used as header for generic data passing to this model through
+ * traci
+ */
+struct CCDataHeader {
+    int type;    //type of message. indicates what comes after the header
+    int size;    //size of message. indicates how many bytes comes after the header
+};
+
+/**
+ * Struct defining data passed about a vehicle
+ */
+struct VEHICLE_DATA {
+    int index;           //position in the platoon (0 = first)
+    double speed;        //vehicle speed
+    double acceleration; //vehicle acceleration
+    double positionX;    //position of the vehicle in the simulation
+    double positionY;    //position of the vehicle in the simulation
+    double time;         //time at which such information was read from vehicle's sensors
+    double length;       //vehicle length
+    double u;            //controller acceleration
+    double speedX;       //vehicle speed on the X axis
+    double speedY;       //vehicle speed on the Y axis
+    double angle;        //vehicle angle in radians
+};
+
+#define MAX_N_CARS 8
+
+#define CC_ENGINE_MODEL_FOLM             0x00    //first order lag model
+#define CC_ENGINE_MODEL_REALISTIC        0x01    //the detailed and realistic engine model
+
+//parameter names for engine models
+#define FOLM_PAR_TAU                     "tau_s"
+#define FOLM_PAR_DT                      "dt_s"
+
+#define ENGINE_PAR_VEHICLE               "vehicle"
+#define ENGINE_PAR_XMLFILE               "xmlFile"
+#define ENGINE_PAR_DT                    "dt_s"
+
+#define CC_PAR_VEHICLE_DATA              "ccvd"   //data about a vehicle, like position, speed, acceleration, etc
+#define CC_PAR_VEHICLE_POSITION          "ccvp"   //position of the vehicle in the platoon (0 based)
+#define CC_PAR_PLATOON_SIZE              "ccps"   //number of cars in the platoon
+
+//set of controller-related constants
+#define CC_PAR_CACC_XI                   "ccxi"    //xi
+#define CC_PAR_CACC_OMEGA_N              "ccon"    //omega_n
+#define CC_PAR_CACC_C1                   "ccc1"    //C1
+#define CC_PAR_ENGINE_TAU                "cctau"   //engine time constant
+
+#define CC_PAR_UMIN                      "ccumin"  //lower saturation for u
+#define CC_PAR_UMAX                      "ccumax"  //upper saturation for u
+
+#define CC_PAR_PLOEG_H                   "ccph"    //time headway of ploeg's CACC
+#define CC_PAR_PLOEG_KP                  "ccpkp"   //kp parameter of ploeg's CACC
+#define CC_PAR_PLOEG_KD                  "ccpkd"   //kd parameter of ploeg's CACC
+
+#define CC_PAR_FLATBED_KA                "ccfka"   //ka parameter of flatbed CACC
+#define CC_PAR_FLATBED_KV                "ccfkv"   //kv parameter of flatbed CACC
+#define CC_PAR_FLATBED_KP                "ccfkp"   //kp parameter of flatbed CACC
+#define CC_PAR_FLATBED_H                 "ccfh"    //h parameter of flatbed CACC
+#define CC_PAR_FLATBED_D                 "ccfd"    //distance parameter of flatbed CACC
+
+#define CC_PAR_VEHICLE_ENGINE_MODEL      "ccem"    //set the engine model for a vehicle
+
+#define CC_PAR_VEHICLE_MODEL             "ccvm"    //set the vehicle model, i.e., engine characteristics
+#define CC_PAR_VEHICLES_FILE             "ccvf"    //set the location of the vehicle parameters file
+
+// set CACC constant spacing
+#define PAR_CACC_SPACING                 "ccsp"
+
+// get ACC computed acceleration when faked CACC controller is enabled
+#define PAR_ACC_ACCELERATION             "ccacc"
+
+// determine whether a vehicle has crashed or not
+#define PAR_CRASHED                      "cccr"
+
+// set a fixed acceleration to a vehicle controlled by CC/ACC/CACC
+#define PAR_FIXED_ACCELERATION           "ccfa"
+
+// get vehicle speed and acceleration, needed for example by the platoon leader (get: vehicle)
+#define PAR_SPEED_AND_ACCELERATION       "ccsa"
+
+// set speed and acceleration of the platoon leader
+#define PAR_LEADER_SPEED_AND_ACCELERATION "cclsa"
+
+// set whether CACCs should use real or controller acceleration
+#define PAR_USE_CONTROLLER_ACCELERATION "ccca"
+
+// get lane count for the street the vehicle is currently traveling
+#define PAR_LANES_COUNT                  "cclc"
+
+// set the cruise control desired speed
+#define PAR_CC_DESIRED_SPEED             "ccds"
+
+// set the currently active vehicle controller which can be either the driver, or the ACC or the CACC
+#define PAR_ACTIVE_CONTROLLER            "ccac"
+
+// get radar data from the car
+#define PAR_RADAR_DATA                   "ccrd"
+
+// communicate with the cruise control to give him fake indications. this can be useful when you want
+// to advance a vehicle to a certain position, for example, for joining a platoon. clearly the ACC
+// must always take into consideration both fake and real data
+#define PAR_LEADER_FAKE_DATA             "cclfd"
+#define PAR_FRONT_FAKE_DATA              "ccffd"
+
+// get the distance that a car has to travel until it reaches the end of its route
+#define PAR_DISTANCE_TO_END              "ccdte"
+
+// get the distance from the beginning of the route
+#define PAR_DISTANCE_FROM_BEGIN          "ccdfb"
+
+// set speed and acceleration of preceding vehicle
+#define PAR_PRECEDING_SPEED_AND_ACCELERATION "ccpsa"
+
+// set ACC headway time
+#define PAR_ACC_HEADWAY_TIME             "ccaht"
+
+// return engine information (for the realistic engine model)
+#define PAR_ENGINE_DATA                  "cced"
+
+// enabling/disabling auto feeding
+#define PAR_USE_AUTO_FEEDING             "ccaf"
+
+// enabling/disabling data prediction
+#define PAR_USE_PREDICTION               "ccup"
+
+// add/remove members from own platoon
+#define PAR_ADD_MEMBER                   "ccam"
+#define PAR_REMOVE_MEMBER                "ccrm"
+
+// let the leader automatically change lane for the whole platoon if there is a speed advantage
+#define PAR_ENABLE_AUTO_LANE_CHANGE      "ccalc"
+
+}
+
+#endif /* CC_CONST_H */

src/microsim/cfmodels/CC_VehicleVariables.cpp

@@ -0,0 +1,87 @@
+/****************************************************************************/
+// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
+// Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
+// This program and the accompanying materials
+// are made available under the terms of the Eclipse Public License v2.0
+// which accompanies this distribution, and is available at
+// http://www.eclipse.org/legal/epl-v20.html
+// SPDX-License-Identifier: EPL-2.0
+/****************************************************************************/
+/// @file    CC_VehicleVariables.cpp
+/// @author  Michele Segata
+/// @date    Mon, 7 Mar 2016
+/// @version $Id$
+///
+/****************************************************************************/
+#include "CC_VehicleVariables.h"
+
+//initialize default L and K matrices
+const int CC_VehicleVariables::defaultL[][MAX_N_CARS] = {
+    {0, 0, 0, 0, 0, 0, 0, 0},
+    {1, 0, 0, 0, 0, 0, 0, 0},
+    {1, 1, 0, 0, 0, 0, 0, 0},
+    {1, 0, 1, 0, 0, 0, 0, 0},
+    {1, 0, 0, 1, 0, 0, 0, 0},
+    {1, 0, 0, 0, 1, 0, 0, 0},
+    {1, 0, 0, 0, 0, 1, 0, 0},
+    {1, 0, 0, 0, 0, 0, 1, 0}
+};
+const double CC_VehicleVariables::defaultK[][MAX_N_CARS] = {
+    {0, 0, 0, 0, 0, 0, 0, 0},
+    {460, 0, 0, 0, 0, 0, 0, 0},
+    {80, 860, 0, 0, 0, 0, 0, 0},
+    {80, 0, 860, 0, 0, 0, 0, 0},
+    {80, 0, 0, 860, 0, 0, 0, 0},
+    {80, 0, 0, 0, 860, 0, 0, 0},
+    {80, 0, 0, 0, 0, 860, 0, 0},
+    {80, 0, 0, 0, 0, 0, 860, 0}
+};
+const double CC_VehicleVariables::defaultB[MAX_N_CARS] = {1800, 1800, 1800, 1800, 1800, 1800, 1800, 1800};
+const double CC_VehicleVariables::defaultH[MAX_N_CARS] = {0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8};
+
+CC_VehicleVariables::CC_VehicleVariables() :
+    controllerAcceleration(0), frontSpeed(0), frontAcceleration(0),
+    frontControllerAcceleration(0), frontDataReadTime(0), frontAngle(0), frontInitialized(false),
+    autoFeed(false), leaderVehicle(0), frontVehicle(0),
+    accHeadwayTime(1.5), accLambda(0.1),
+    useControllerAcceleration(true), leaderSpeed(0),
+    leaderAcceleration(0), leaderControllerAcceleration(0), leaderDataReadTime(0), leaderAngle(0),
+    leaderInitialized(false), caccInitialized(false),
+    useFixedAcceleration(0), fixedAcceleration(0),
+    crashed(false), crashedVictim(false),
+    ccDesiredSpeed(14), ccKp(1), activeController(Plexe::DRIVER),
+    nInitialized(0), position(-1), nCars(8),
+    caccXi(-1), caccOmegaN(-1), caccC1(-1), caccAlpha1(-1), caccAlpha2(-1),
+    caccAlpha3(-1), caccAlpha4(-1), caccAlpha5(-1), caccSpacing(5),
+    engineTau(0.5),
+    uMin(-1e6), uMax(1e6),
+    ploegH(0.5), ploegKp(0.2), ploegKd(0.7),
+    flatbedKa(2.4), flatbedKv(0.6), flatbedKp(12), flatbedD(5), flatbedH(4),
+    engine(0), engineModel(CC_ENGINE_MODEL_FOLM),
+    usePrediction(false),
+    autoLaneChange(false) {
+    fakeData.frontAcceleration = 0;
+    fakeData.frontControllerAcceleration = 0;
+    fakeData.frontDistance = 0;
+    fakeData.frontSpeed = 0;
+    fakeData.leaderAcceleration = 0;
+    fakeData.leaderControllerAcceleration = 0;
+    fakeData.leaderSpeed = 0;
+    leaderPosition.set(0, 0);
+    frontPosition.set(0, 0);
+    //init L, K, b, and h with default values
+    memcpy(L, defaultL, sizeof(int)*MAX_N_CARS * MAX_N_CARS);
+    memcpy(K, defaultK, sizeof(double)*MAX_N_CARS * MAX_N_CARS);
+    memcpy(b, defaultB, sizeof(double)*MAX_N_CARS);
+    memcpy(h, defaultH, sizeof(double)*MAX_N_CARS);
+    //no data about any vehicle has been set
+    for (int i = 0; i < MAX_N_CARS; i++) {
+        initialized[i] = false;
+    }
+}
+
+CC_VehicleVariables::~CC_VehicleVariables() {
+    if (engine) {
+        delete engine;
+    }
+}