fix #5772

eclipse-sumo · Jun 26, 2019 · c784098 · c784098
1 parent c9989ba
commit c784098
Show file tree

Hide file tree

Showing 4 changed files with 233 additions and 6 deletions.
diff --git a/src/traci_testclient/TraCITestClient.cpp b/src/traci_testclient/TraCITestClient.cpp
@@ -897,6 +897,28 @@ TraCITestClient::testAPI() {
         answerLog << "      vehicle=" << it->first << " pos=" << it->second[libsumo::VAR_LANEPOSITION]->getString() << "\n";
     }
 
+    answerLog << "    subscribe to vehicles around vehicle '1':\n";
+    std::vector<int> vars3;
+    vars3.push_back(libsumo::VAR_SPEED);
+    vehicle.subscribeContext("1", libsumo::CMD_GET_VEHICLE_VARIABLE, 1000, vars3, 0, 100);
+    //vehicle.addSubscriptionFilterTurn();
+    //vehicle.addSubscriptionFilterDownstreamDistance(1000);
+    //vehicle.addSubscriptionFilterUpstreamDistance(1000);
+    //vehicle.addSubscriptionFilterVClass(std::vector<std::string>({"passenger"}));
+    //vehicle.addSubscriptionFilterLanes(std::vector<int>({0, 1, 2}));
+    //vehicle.addSubscriptionFilterLeadFollow(std::vector<int>({0, 1, 2}));
+    //vehicle.addSubscriptionFilterLanes(std::vector<int>({0}));
+    //vehicle.addSubscriptionFilterLeadFollow(std::vector<int>({0}));
+    //vehicle.addSubscriptionFilterCFManeuver();
+    vehicle.addSubscriptionFilterLCManeuver(1);
+
+    simulationStep();
+    answerLog << "    context subscription results:\n";
+    libsumo::SubscriptionResults result5 = vehicle.getContextSubscriptionResults("1");
+    for (auto item : result5) {
+        answerLog << "      vehicle=" << item.first << "\n";
+    }
+
     // person
     answerLog << "  person:\n";
     person.setWidth("p0", 1);

diff --git a/src/utils/traci/TraCIAPI.cpp b/src/utils/traci/TraCIAPI.cpp
@@ -180,6 +180,29 @@ TraCIAPI::createCommand(int cmdID, int varID, const std::string& objID, tcpip::S
 }
 
 
+void
+TraCIAPI::createFilterCommand(int cmdID, int varID, tcpip::Storage* add) const {
+    myOutput.reset();
+    // command length
+    int length = 1 + 1 + 1;
+    if (add != nullptr) {
+        length += (int)add->size();
+    }
+    if (length <= 255) {
+        myOutput.writeUnsignedByte(length);
+    } else {
+        myOutput.writeUnsignedByte(0);
+        myOutput.writeInt(length + 4);
+    }
+    myOutput.writeUnsignedByte(cmdID);
+    myOutput.writeUnsignedByte(varID);
+    // additional values
+    if (add != nullptr) {
+        myOutput.writeStorage(*add);
+    }
+}
+
+
 void
 TraCIAPI::send_commandSubscribeObjectVariable(int domID, const std::string& objID, double beginTime, double endTime,
         const std::vector<int>& vars) const {
@@ -3119,6 +3142,133 @@ TraCIAPI::VehicleScope::setEmissionClass(const std::string& vehicleID, const std
     myParent.processSet(libsumo::CMD_SET_VEHICLE_VARIABLE);
 }
 
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterLanes(const std::vector<int>& lanes,
+        bool noOpposite, double downstreamDist, double upstreamDist) const {
+    addSubscriptionFilterByteList(libsumo::FILTER_TYPE_LANES, lanes);
+    if (noOpposite) {
+        addSubscriptionFilterNoOpposite();
+    }
+    if (downstreamDist >= 0) {
+        addSubscriptionFilterDownstreamDistance(downstreamDist);
+    }
+    if (upstreamDist >= 0) {
+        addSubscriptionFilterUpstreamDistance(upstreamDist);
+    }
+}
+
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterNoOpposite() const {
+    addSubscriptionFilterEmpty(libsumo::FILTER_TYPE_NOOPPOSITE);
+}
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterDownstreamDistance(double dist) const {
+    addSubscriptionFilterFloat(libsumo::FILTER_TYPE_DOWNSTREAM_DIST, dist);
+}
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterUpstreamDistance(double dist) const {
+    addSubscriptionFilterFloat(libsumo::FILTER_TYPE_UPSTREAM_DIST, dist);
+}
+
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterCFManeuver(double downstreamDist, double upstreamDist) const {
+    addSubscriptionFilterLeadFollow(std::vector<int>({0}));
+    if (downstreamDist >= 0) {
+        addSubscriptionFilterDownstreamDistance(downstreamDist);
+    }
+    if (upstreamDist >= 0) {
+        addSubscriptionFilterUpstreamDistance(upstreamDist);
+    }
+}
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterLCManeuver(int direction, bool noOpposite, double downstreamDist, double upstreamDist) const {
+    if (abs(direction) != 1) {
+        std::cerr << "Ignoring lane change subscription filter with non-neighboring lane offset direction " << direction << "\n";
+        return;
+    }
+    addSubscriptionFilterLeadFollow(std::vector<int>({0, direction}));
+    if (noOpposite) {
+        addSubscriptionFilterNoOpposite();
+    }
+    if (downstreamDist >= 0) {
+        addSubscriptionFilterDownstreamDistance(downstreamDist);
+    }
+    if (upstreamDist >= 0) {
+        addSubscriptionFilterUpstreamDistance(upstreamDist);
+    }
+}
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterLeadFollow(const std::vector<int>& lanes) const {
+    addSubscriptionFilterEmpty(libsumo::FILTER_TYPE_LEAD_FOLLOW);
+    addSubscriptionFilterByteList(libsumo::FILTER_TYPE_LANES, lanes);
+}
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterTurn(double downstreamDist, double upstreamDist) const {
+    addSubscriptionFilterEmpty(libsumo::FILTER_TYPE_TURN);
+    if (downstreamDist >= 0) {
+        addSubscriptionFilterDownstreamDistance(downstreamDist);
+    }
+    if (upstreamDist >= 0) {
+        addSubscriptionFilterUpstreamDistance(upstreamDist);
+    }
+}
+
+
+void 
+TraCIAPI::VehicleScope::addSubscriptionFilterVClass(const std::vector<std::string>& vClasses) const {
+    addSubscriptionFilterStringList(libsumo::FILTER_TYPE_VCLASS, vClasses);
+}
+
+
+void 
+TraCIAPI::VehicleScope::addSubscriptionFilterVType(const std::vector<std::string>& vTypes) const {
+    addSubscriptionFilterStringList(libsumo::FILTER_TYPE_VTYPE, vTypes);
+}
+
+
+void
+TraCIAPI::VehicleScope::addSubscriptionFilterEmpty(int filterType) const {
+    myParent.createFilterCommand(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, filterType);
+    myParent.processSet(libsumo::CMD_ADD_SUBSCRIPTION_FILTER);
+}
+
+void 
+TraCIAPI::VehicleScope::addSubscriptionFilterFloat(int filterType, double val) const {
+    tcpip::Storage content;
+    content.writeUnsignedByte(libsumo::TYPE_DOUBLE);
+    content.writeDouble(val);
+    myParent.createFilterCommand(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, filterType, &content);
+    myParent.processSet(libsumo::CMD_ADD_SUBSCRIPTION_FILTER);
+}
+
+
+void 
+TraCIAPI::VehicleScope::addSubscriptionFilterStringList(int filterType, const std::vector<std::string>& vals) const {
+    tcpip::Storage content;
+    content.writeUnsignedByte(libsumo::TYPE_STRINGLIST);
+    content.writeStringList(vals);
+    myParent.createFilterCommand(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, filterType, &content);
+    myParent.processSet(libsumo::CMD_ADD_SUBSCRIPTION_FILTER);
+}
+
+void 
+TraCIAPI::VehicleScope::addSubscriptionFilterByteList(int filterType, const std::vector<int>& vals) const {
+    tcpip::Storage content;
+    content.writeUnsignedByte(vals.size());
+    for (int i : vals)  {
+        content.writeByte(i);
+    }
+    myParent.createFilterCommand(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, filterType, &content);
+    myParent.processSet(libsumo::CMD_ADD_SUBSCRIPTION_FILTER);
+}
+
 
 // ---------------------------------------------------------------------------
 // // TraCIAPI::PersonScope-methods

diff --git a/src/utils/traci/TraCIAPI.h b/src/utils/traci/TraCIAPI.h
@@ -808,7 +808,58 @@ class TraCIAPI {
         void setMaxSpeed(const std::string& vehicleID, double speed) const;
         /// @}
 
+        /// @name subscription filtering
+        /* @brief Filters are added to the last modified vehicle context
+         *  subscription (call these fucntions right after subscribing) */
+        /// @{
+
+        /* @brief Adds a lane-filter, lanes is a list of relative lane indices (-1 -> right neighboring lane of the ego, 0 -> ego lane, etc.)
+         * noOpposite specifies whether vehicles on opposite direction lanes shall be returned
+         * downstreamDist and upstreamDist specify the range of the search for surrounding vehicles along the road net. */
+        void addSubscriptionFilterLanes(const std::vector<int>& lanes, 
+                bool noOpposite=false, double downstreamDist=-1, double upstreamDist=-1) const;
+
+        /* @brief Omits vehicles on other edges than the ego's */
+        void addSubscriptionFilterNoOpposite() const;
+
+        /* @brief Limits the downstream distance for resulting vehicles */
+        void addSubscriptionFilterDownstreamDistance(double dist) const;
+
+        /* @brief Limits the updstream distance for resulting vehicles */
+        void addSubscriptionFilterUpstreamDistance(double dist) const;
+
+        /* @brief Restricts vehicles returned by the last modified vehicle context subscription to leader and follower of the ego.
+         * downstreamDist and upstreamDist specify the range of the search for leader and follower along the road net. */
+        void addSubscriptionFilterCFManeuver(double downstreamDist=-1, double upstreamDist=-1) const;
+
+        /* @brief Restricts returned vehicles to neighbor and ego-lane leader
+         *  and follower of the ego in the given direction 
+         * noOpposite specifies whether vehicles on opposite direction lanes shall be returned
+         * downstreamDist and upstreamDist specify the range of the search for leader and follower along the road net.
+         * Combine with: distance filters; vClass/vType filter. */
+        void addSubscriptionFilterLCManeuver(int direction, bool noOpposite=false, double downstreamDist=-1, double upstreamDist=-1) const;
+
+        /* @brief Restricts returned vehicles to neighbor and ego-lane leader and follower of the ego.
+         * Combine with: lanes-filter to restrict to one direction; distance filters; vClass/vType filter. */
+        void addSubscriptionFilterLeadFollow(const std::vector<int>& lanes) const; 
+
+        /* @brief Restricts returned vehicles to foes on an upcoming junction */
+        void addSubscriptionFilterTurn(double downstreamDist=-1, double upstreamDist=-1) const; 
+
+        /* @brief Restricts returned vehicles to the given classes */
+        void addSubscriptionFilterVClass(const std::vector<std::string>& vClasses) const;
+
+        /* @brief Restricts returned vehicles to the given types */
+        void addSubscriptionFilterVType(const std::vector<std::string>& vTypes) const;
+
+        /// @}
+
     private:
+        void addSubscriptionFilterEmpty(int filterType) const;
+        void addSubscriptionFilterFloat(int filterType, double val) const;
+        void addSubscriptionFilterStringList(int filterType, const std::vector<std::string>& vals) const;
+        void addSubscriptionFilterByteList(int filterType, const std::vector<int>& vals) const;
+
         /// @brief invalidated copy constructor
         VehicleScope(const VehicleScope& src);
 
@@ -934,6 +985,7 @@ class TraCIAPI {
      * @param[in] add Optional additional parameter
      */
     void createCommand(int cmdID, int varID, const std::string& objID, tcpip::Storage* add = nullptr) const;
+    void createFilterCommand(int cmdID, int varID, tcpip::Storage* add = nullptr) const;
 
 
     /** @brief Sends a SubscribeVariable request

diff --git a/tests/traci/testAPI/testclient_out.traci b/tests/traci/testAPI/testclient_out.traci
@@ -1,4 +1,4 @@
-TraCITestClient output file. Date: Wed Jun 26 12:27:14 2019
+TraCITestClient output file. Date: Wed Jun 26 16:22:55 2019
 
 
 -> Command sent: <SetOrder>:
@@ -129,6 +129,9 @@ testAPI:
     context subscription results:
       vehicle=1 pos=11.4581
       vehicle=2 pos=16.3564
+    subscribe to vehicles around vehicle '1':
+    context subscription results:
+      vehicle=2
   person:
     getIDList: p0 pWaiting
     getRoadID: e_m3
@@ -139,15 +142,15 @@ testAPI:
     getRemainingStages: 1
     getVehicle: 
     getEdges: e_m3 e_m4
-    getPosition: TraCIPosition(1500,490.47,0)
-    getPosition3D: TraCIPosition(1500,490.47,0)
+    getPosition: TraCIPosition(1501.64,490.47,0)
+    getPosition3D: TraCIPosition(1501.64,490.47,0)
     getAngle: 90.00
     getSlope: 0.00
-    getLanePosition: 0.00
+    getLanePosition: 1.64
     getLength: 0.21
     getColor: TraCIColor(255,255,0,128)
     param: bar
-    getSpeed: 2.47
+    getSpeed: 2.57
     getRemainingStages: 4
     getRemainingStages: 3
     getRemainingStages: 1
@@ -161,7 +164,7 @@ testAPI:
     phase: 0
     phaseName: nameSetByTraCI
     phaseDuration: 12.00
-    nextSwitch: 24.00
+    nextSwitch: 25.00
     controlledLanes: e_vo0_0 e_vo0_1 e_vu0_0 e_vu0_1 e_m3_0 e_m3_1 e_m3_2
     controlledLinks:
       index=0 link=0 fromLane=e_vo0_0 viaLane=:n_m4_0_0 toLane=e_m4_1