Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2007-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file TraCIServer.cpp
15 : /// @author Axel Wegener
16 : /// @author Friedemann Wesner
17 : /// @author Christoph Sommer
18 : /// @author Jakob Erdmann
19 : /// @author Daniel Krajzewicz
20 : /// @author Thimor Bohn
21 : /// @author Tino Morenz
22 : /// @author Laura Bieker
23 : /// @author Michael Behrisch
24 : /// @author Mario Krumnow
25 : /// @author Leonhard Luecken
26 : /// @date 2007/10/24
27 : ///
28 : // TraCI server used to control sumo by a remote TraCI client (e.g., ns2)
29 : /****************************************************************************/
30 : #include <config.h>
31 :
32 : #ifdef HAVE_VERSION_H
33 : #include <version.h>
34 : #endif
35 :
36 : #include <string>
37 : #include <cmath>
38 : #include <map>
39 : #include <iostream>
40 : #include <algorithm>
41 : #include <foreign/tcpip/socket.h>
42 : #include <foreign/tcpip/storage.h>
43 : #include <utils/common/SUMOTime.h>
44 : #include <utils/router/DijkstraRouter.h>
45 : #include <utils/common/NamedObjectCont.h>
46 : #include <utils/common/RandHelper.h>
47 : #include <utils/common/MsgHandler.h>
48 : #include <utils/vehicle/SUMOVehicleParameter.h>
49 : #include <utils/shapes/PointOfInterest.h>
50 : #include <utils/shapes/ShapeContainer.h>
51 : #include <utils/xml/XMLSubSys.h>
52 : #include <libsumo/Helper.h>
53 : #include <microsim/MSNet.h>
54 : #include <microsim/MSVehicle.h>
55 : #include <microsim/MSEdge.h>
56 : #include <microsim/MSJunctionControl.h>
57 : #include <microsim/transportables/MSTransportableControl.h>
58 : #include <microsim/MSJunction.h>
59 : #include <microsim/MSEdgeControl.h>
60 : #include <microsim/MSLane.h>
61 : #include <microsim/MSGlobals.h>
62 : #include <microsim/traffic_lights/MSTLLogicControl.h>
63 : #include <libsumo/Simulation.h>
64 : #include <libsumo/Subscription.h>
65 : #include <libsumo/TraCIConstants.h>
66 : #include "TraCIServer.h"
67 : #include "TraCIServerAPI_InductionLoop.h"
68 : #include "TraCIServerAPI_Junction.h"
69 : #include "TraCIServerAPI_Lane.h"
70 : #include "TraCIServerAPI_MultiEntryExit.h"
71 : #include "TraCIServerAPI_LaneArea.h"
72 : #include "TraCIServerAPI_TrafficLight.h"
73 : #include "TraCIServerAPI_Vehicle.h"
74 : #include "TraCIServerAPI_VehicleType.h"
75 : #include "TraCIServerAPI_Route.h"
76 : #include "TraCIServerAPI_POI.h"
77 : #include "TraCIServerAPI_Polygon.h"
78 : #include "TraCIServerAPI_Edge.h"
79 : #include "TraCIServerAPI_Simulation.h"
80 : #include "TraCIServerAPI_Person.h"
81 : #include "TraCIServerAPI_Calibrator.h"
82 : #include "TraCIServerAPI_BusStop.h"
83 : #include "TraCIServerAPI_ParkingArea.h"
84 : #include "TraCIServerAPI_ChargingStation.h"
85 : #include "TraCIServerAPI_RouteProbe.h"
86 : #include "TraCIServerAPI_Rerouter.h"
87 : #include "TraCIServerAPI_VariableSpeedSign.h"
88 : #include "TraCIServerAPI_MeanData.h"
89 : #include "TraCIServerAPI_OverheadWire.h"
90 :
91 :
92 : // ===========================================================================
93 : // debug constants
94 : // ===========================================================================
95 : //#define DEBUG_MULTI_CLIENTS
96 : //#define DEBUG_SUBSCRIPTIONS
97 : //#define DEBUG_SUBSCRIPTION_FILTERS
98 : //#define DEBUG_RAW_INPUT
99 :
100 :
101 : // ===========================================================================
102 : // static member definitions
103 : // ===========================================================================
104 : TraCIServer* TraCIServer::myInstance = nullptr;
105 : bool TraCIServer::myDoCloseConnection = false;
106 :
107 :
108 : // ===========================================================================
109 : // method definitions
110 : // ===========================================================================
111 : void
112 18084621 : TraCIServer::initWrapper(const int domainID, const int variable, const std::string& objID) {
113 18084621 : myWrapperStorage.reset();
114 18084621 : myWrapperStorage.writeUnsignedByte(domainID);
115 18084621 : myWrapperStorage.writeUnsignedByte(variable);
116 18084621 : myWrapperStorage.writeString(objID);
117 18084621 : }
118 :
119 :
120 : bool
121 4427876 : TraCIServer::wrapDouble(const std::string& /* objID */, const int /* variable */, const double value) {
122 4427876 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLE);
123 4427876 : myWrapperStorage.writeDouble(value);
124 4427876 : return true;
125 : }
126 :
127 :
128 : bool
129 8200008 : TraCIServer::wrapInt(const std::string& /* objID */, const int /* variable */, const int value) {
130 8200008 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_INTEGER);
131 8200008 : myWrapperStorage.writeInt(value);
132 8200008 : return true;
133 : }
134 :
135 :
136 : bool
137 4191656 : TraCIServer::wrapString(const std::string& /* objID */, const int /* variable */, const std::string& value) {
138 4191656 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
139 4191656 : myWrapperStorage.writeString(value);
140 4191656 : return true;
141 : }
142 :
143 :
144 : bool
145 178837 : TraCIServer::wrapStringList(const std::string& /* objID */, const int /* variable */, const std::vector<std::string>& value) {
146 178837 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRINGLIST);
147 178837 : myWrapperStorage.writeStringList(value);
148 178837 : return true;
149 : }
150 :
151 :
152 : bool
153 6 : TraCIServer::wrapDoubleList(const std::string& /* objID */, const int /* variable */, const std::vector<double>& value) {
154 6 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLELIST);
155 6 : myWrapperStorage.writeDoubleList(value);
156 6 : return true;
157 : }
158 :
159 :
160 : bool
161 908437 : TraCIServer::wrapPosition(const std::string& /* objID */, const int variable, const libsumo::TraCIPosition& value) {
162 : const bool includeZ = variable == libsumo::VAR_POSITION3D;
163 1815642 : myWrapperStorage.writeUnsignedByte(includeZ ? libsumo::POSITION_3D : libsumo::POSITION_2D);
164 908437 : myWrapperStorage.writeDouble(value.x);
165 908437 : myWrapperStorage.writeDouble(value.y);
166 908437 : if (includeZ) {
167 1232 : myWrapperStorage.writeDouble(value.z);
168 : }
169 908437 : return true;
170 : }
171 :
172 :
173 : bool
174 12105 : TraCIServer::wrapPositionVector(const std::string& /* objID */, const int /* variable */, const libsumo::TraCIPositionVector& shape) {
175 12105 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_POLYGON);
176 12105 : if (shape.value.size() < 256) {
177 12102 : myWrapperStorage.writeUnsignedByte((int)shape.value.size());
178 : } else {
179 3 : myWrapperStorage.writeUnsignedByte(0);
180 3 : myWrapperStorage.writeInt((int)shape.value.size());
181 : }
182 43290 : for (const libsumo::TraCIPosition& pos : shape.value) {
183 31185 : myWrapperStorage.writeDouble(pos.x);
184 31185 : myWrapperStorage.writeDouble(pos.y);
185 : }
186 12105 : return true;
187 : }
188 :
189 :
190 : bool
191 710 : TraCIServer::wrapColor(const std::string& /* objID */, const int /* variable */, const libsumo::TraCIColor& value) {
192 710 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COLOR);
193 710 : myWrapperStorage.writeUnsignedByte(value.r);
194 710 : myWrapperStorage.writeUnsignedByte(value.g);
195 710 : myWrapperStorage.writeUnsignedByte(value.b);
196 710 : myWrapperStorage.writeUnsignedByte(value.a);
197 710 : return true;
198 : }
199 :
200 :
201 : bool
202 22393 : TraCIServer::wrapStringDoublePair(const std::string& /* objID */, const int /* variable */, const std::pair<std::string, double>& value) {
203 22393 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COMPOUND);
204 22393 : myWrapperStorage.writeInt(2);
205 22393 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
206 22393 : myWrapperStorage.writeString(value.first);
207 22393 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLE);
208 22393 : myWrapperStorage.writeDouble(value.second);
209 22393 : return true;
210 : }
211 :
212 :
213 : bool
214 92 : TraCIServer::wrapStringPair(const std::string& /* objID */, const int /* variable */, const std::pair<std::string, std::string>& value) {
215 92 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COMPOUND);
216 92 : myWrapperStorage.writeInt(2);
217 92 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
218 92 : myWrapperStorage.writeString(value.first);
219 92 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
220 92 : myWrapperStorage.writeString(value.second);
221 92 : return true;
222 : }
223 :
224 :
225 : tcpip::Storage&
226 18607701 : TraCIServer::getWrapperStorage() {
227 18607701 : return myWrapperStorage;
228 : }
229 :
230 :
231 :
232 2479 : TraCIServer::TraCIServer(const SUMOTime begin, const int port, const int numClients)
233 2479 : : myTargetTime(begin), myLastContextSubscription(nullptr) {
234 : #ifdef DEBUG_MULTI_CLIENTS
235 : std::cout << "Creating new TraCIServer for " << numClients << " clients on port " << port << "." << std::endl;
236 : #endif
237 2479 : myVehicleStateChanges[MSNet::VehicleState::BUILT] = std::vector<std::string>();
238 2479 : myVehicleStateChanges[MSNet::VehicleState::DEPARTED] = std::vector<std::string>();
239 2479 : myVehicleStateChanges[MSNet::VehicleState::STARTING_TELEPORT] = std::vector<std::string>();
240 2479 : myVehicleStateChanges[MSNet::VehicleState::ENDING_TELEPORT] = std::vector<std::string>();
241 2479 : myVehicleStateChanges[MSNet::VehicleState::ARRIVED] = std::vector<std::string>();
242 2479 : myVehicleStateChanges[MSNet::VehicleState::NEWROUTE] = std::vector<std::string>();
243 2479 : myVehicleStateChanges[MSNet::VehicleState::STARTING_PARKING] = std::vector<std::string>();
244 2479 : myVehicleStateChanges[MSNet::VehicleState::MANEUVERING] = std::vector<std::string>();
245 2479 : myVehicleStateChanges[MSNet::VehicleState::ENDING_PARKING] = std::vector<std::string>();
246 2479 : myVehicleStateChanges[MSNet::VehicleState::STARTING_STOP] = std::vector<std::string>();
247 2479 : myVehicleStateChanges[MSNet::VehicleState::ENDING_STOP] = std::vector<std::string>();
248 2479 : myVehicleStateChanges[MSNet::VehicleState::COLLISION] = std::vector<std::string>();
249 2479 : myVehicleStateChanges[MSNet::VehicleState::EMERGENCYSTOP] = std::vector<std::string>();
250 :
251 2479 : myTransportableStateChanges[MSNet::TransportableState::PERSON_DEPARTED] = std::vector<std::string>();
252 2479 : myTransportableStateChanges[MSNet::TransportableState::PERSON_ARRIVED] = std::vector<std::string>();
253 2479 : myTransportableStateChanges[MSNet::TransportableState::CONTAINER_DEPARTED] = std::vector<std::string>();
254 2479 : myTransportableStateChanges[MSNet::TransportableState::CONTAINER_ARRIVED] = std::vector<std::string>();
255 :
256 2479 : myExecutors[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = &TraCIServerAPI_InductionLoop::processGet;
257 2479 : myExecutors[libsumo::CMD_SET_INDUCTIONLOOP_VARIABLE] = &TraCIServerAPI_InductionLoop::processSet;
258 2479 : myExecutors[libsumo::CMD_GET_LANEAREA_VARIABLE] = &TraCIServerAPI_LaneArea::processGet;
259 2479 : myExecutors[libsumo::CMD_SET_LANEAREA_VARIABLE] = &TraCIServerAPI_LaneArea::processSet;
260 2479 : myExecutors[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = &TraCIServerAPI_MultiEntryExit::processGet;
261 2479 : myExecutors[libsumo::CMD_SET_MULTIENTRYEXIT_VARIABLE] = &TraCIServerAPI_MultiEntryExit::processSet;
262 :
263 2479 : myExecutors[libsumo::CMD_GET_TL_VARIABLE] = &TraCIServerAPI_TrafficLight::processGet;
264 2479 : myExecutors[libsumo::CMD_SET_TL_VARIABLE] = &TraCIServerAPI_TrafficLight::processSet;
265 2479 : myExecutors[libsumo::CMD_GET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processGet;
266 2479 : myExecutors[libsumo::CMD_SET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processSet;
267 2479 : myExecutors[libsumo::CMD_GET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processGet;
268 2479 : myExecutors[libsumo::CMD_SET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processSet;
269 2479 : myExecutors[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processGet;
270 2479 : myExecutors[libsumo::CMD_SET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processSet;
271 2479 : myExecutors[libsumo::CMD_GET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processGet;
272 2479 : myExecutors[libsumo::CMD_SET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processSet;
273 2479 : myExecutors[libsumo::CMD_GET_POI_VARIABLE] = &TraCIServerAPI_POI::processGet;
274 2479 : myExecutors[libsumo::CMD_SET_POI_VARIABLE] = &TraCIServerAPI_POI::processSet;
275 2479 : myExecutors[libsumo::CMD_GET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processGet;
276 2479 : myExecutors[libsumo::CMD_SET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processSet;
277 2479 : myExecutors[libsumo::CMD_GET_JUNCTION_VARIABLE] = &TraCIServerAPI_Junction::processGet;
278 2479 : myExecutors[libsumo::CMD_SET_JUNCTION_VARIABLE] = &TraCIServerAPI_Junction::processSet;
279 2479 : myExecutors[libsumo::CMD_GET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processGet;
280 2479 : myExecutors[libsumo::CMD_SET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processSet;
281 2479 : myExecutors[libsumo::CMD_GET_SIM_VARIABLE] = &TraCIServerAPI_Simulation::processGet;
282 2479 : myExecutors[libsumo::CMD_SET_SIM_VARIABLE] = &TraCIServerAPI_Simulation::processSet;
283 2479 : myExecutors[libsumo::CMD_GET_PERSON_VARIABLE] = &TraCIServerAPI_Person::processGet;
284 2479 : myExecutors[libsumo::CMD_SET_PERSON_VARIABLE] = &TraCIServerAPI_Person::processSet;
285 2479 : myExecutors[libsumo::CMD_GET_CALIBRATOR_VARIABLE] = &TraCIServerAPI_Calibrator::processGet;
286 2479 : myExecutors[libsumo::CMD_SET_CALIBRATOR_VARIABLE] = &TraCIServerAPI_Calibrator::processSet;
287 2479 : myExecutors[libsumo::CMD_GET_BUSSTOP_VARIABLE] = &TraCIServerAPI_BusStop::processGet;
288 2479 : myExecutors[libsumo::CMD_SET_BUSSTOP_VARIABLE] = &TraCIServerAPI_BusStop::processSet;
289 2479 : myExecutors[libsumo::CMD_GET_PARKINGAREA_VARIABLE] = &TraCIServerAPI_ParkingArea::processGet;
290 2479 : myExecutors[libsumo::CMD_SET_PARKINGAREA_VARIABLE] = &TraCIServerAPI_ParkingArea::processSet;
291 2479 : myExecutors[libsumo::CMD_GET_CHARGINGSTATION_VARIABLE] = &TraCIServerAPI_ChargingStation::processGet;
292 2479 : myExecutors[libsumo::CMD_SET_CHARGINGSTATION_VARIABLE] = &TraCIServerAPI_ChargingStation::processSet;
293 2479 : myExecutors[libsumo::CMD_GET_ROUTEPROBE_VARIABLE] = &TraCIServerAPI_RouteProbe::processGet;
294 2479 : myExecutors[libsumo::CMD_SET_ROUTEPROBE_VARIABLE] = &TraCIServerAPI_RouteProbe::processSet;
295 2479 : myExecutors[libsumo::CMD_GET_REROUTER_VARIABLE] = &TraCIServerAPI_Rerouter::processGet;
296 2479 : myExecutors[libsumo::CMD_SET_REROUTER_VARIABLE] = &TraCIServerAPI_Rerouter::processSet;
297 2479 : myExecutors[libsumo::CMD_GET_VARIABLESPEEDSIGN_VARIABLE] = &TraCIServerAPI_VariableSpeedSign::processGet;
298 2479 : myExecutors[libsumo::CMD_SET_VARIABLESPEEDSIGN_VARIABLE] = &TraCIServerAPI_VariableSpeedSign::processSet;
299 2479 : myExecutors[libsumo::CMD_GET_MEANDATA_VARIABLE] = &TraCIServerAPI_MeanData::processGet;
300 : //myExecutors[libsumo::CMD_SET_MEANDATA_VARIABLE] = &TraCIServerAPI_MeanData::processSet;
301 2479 : myExecutors[libsumo::CMD_GET_OVERHEADWIRE_VARIABLE] = &TraCIServerAPI_OverheadWire::processGet;
302 2479 : myExecutors[libsumo::CMD_SET_OVERHEADWIRE_VARIABLE] = &TraCIServerAPI_OverheadWire::processSet;
303 :
304 2479 : myParameterized.insert(std::make_pair(libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE, libsumo::VAR_LEADER));
305 2479 : myParameterized.insert(std::make_pair(libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE, libsumo::VAR_FOLLOWER));
306 2479 : myParameterized.insert(std::make_pair(0, libsumo::VAR_PARAMETER));
307 2479 : myParameterized.insert(std::make_pair(0, libsumo::VAR_PARAMETER_WITH_KEY));
308 :
309 2479 : myDoCloseConnection = false;
310 :
311 : // display warning if internal lanes are not used
312 : // TODO this may be redundant to the warning in NLBuilder::build
313 2479 : if (!MSGlobals::gUsingInternalLanes && !MSGlobals::gUseMesoSim) {
314 2 : WRITE_WARNING(TL("Starting TraCI without using internal lanes!"));
315 2 : MsgHandler::getWarningInstance()->inform("Vehicles will jump over junctions.", false);
316 12 : MsgHandler::getWarningInstance()->inform("Use without option --no-internal-links to avoid unexpected behavior", false);
317 : }
318 :
319 : try {
320 4958 : WRITE_MESSAGEF(TL("***Starting server on port % ***"), toString(port));
321 2479 : tcpip::Socket serverSocket(port);
322 2479 : if (numClients > 1) {
323 156 : WRITE_MESSAGEF(TL(" waiting for % clients..."), toString(numClients));
324 : }
325 5120 : while ((int)mySockets.size() < numClients) {
326 2645 : int index = (int)mySockets.size() + libsumo::MAX_ORDER + 1;
327 2645 : mySockets[index] = new SocketInfo(serverSocket.accept(true), begin);
328 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::BUILT] = std::vector<std::string>();
329 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::DEPARTED] = std::vector<std::string>();
330 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_TELEPORT] = std::vector<std::string>();
331 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_TELEPORT] = std::vector<std::string>();
332 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ARRIVED] = std::vector<std::string>();
333 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::NEWROUTE] = std::vector<std::string>();
334 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_PARKING] = std::vector<std::string>();
335 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::MANEUVERING] = std::vector<std::string>();
336 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_PARKING] = std::vector<std::string>();
337 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_STOP] = std::vector<std::string>();
338 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_STOP] = std::vector<std::string>();
339 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::COLLISION] = std::vector<std::string>();
340 2641 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::EMERGENCYSTOP] = std::vector<std::string>();
341 :
342 2641 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::PERSON_DEPARTED] = std::vector<std::string>();
343 2641 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::PERSON_ARRIVED] = std::vector<std::string>();
344 2641 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::CONTAINER_DEPARTED] = std::vector<std::string>();
345 2641 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::CONTAINER_ARRIVED] = std::vector<std::string>();
346 2641 : if (numClients > 1) {
347 488 : WRITE_MESSAGE(TL(" client connected"));
348 : }
349 : }
350 : // When got here, all clients have connected
351 2475 : if (numClients > 1) {
352 78 : checkClientOrdering();
353 : }
354 : // set myCurrentSocket != mySockets.end() to indicate that this is the first step in processCommands()
355 2471 : myCurrentSocket = mySockets.begin();
356 2487 : } catch (tcpip::SocketException& e) {
357 8 : throw ProcessError(e.what());
358 4 : }
359 2519 : }
360 :
361 :
362 4942 : TraCIServer::~TraCIServer() {
363 2502 : for (const auto& socket : mySockets) {
364 31 : delete socket.second;
365 : }
366 : // there is no point in calling cleanup() here, it does not free any pointers and will only modify members which get deleted anyway
367 7413 : }
368 :
369 :
370 : // ---------- Initialisation and Shutdown
371 : void
372 43013 : TraCIServer::openSocket(const std::map<int, CmdExecutor>& execs) {
373 126534 : if (myInstance == nullptr && !myDoCloseConnection && (OptionsCont::getOptions().getInt("remote-port") != 0)) {
374 7429 : myInstance = new TraCIServer(string2time(OptionsCont::getOptions().getString("begin")),
375 2479 : OptionsCont::getOptions().getInt("remote-port"),
376 9924 : OptionsCont::getOptions().getInt("num-clients"));
377 2657 : for (std::map<int, CmdExecutor>::const_iterator i = execs.begin(); i != execs.end(); ++i) {
378 186 : myInstance->myExecutors[i->first] = i->second;
379 : }
380 : }
381 43005 : if (myInstance != nullptr) {
382 : // maybe net was deleted and built again
383 2484 : MSNet::getInstance()->addVehicleStateListener(myInstance);
384 2484 : MSNet::getInstance()->addTransportableStateListener(myInstance);
385 2484 : myInstance->mySubscriptionCache.writeInt(0);
386 : }
387 43005 : }
388 :
389 :
390 : void
391 42589 : TraCIServer::close() {
392 42589 : if (myInstance == nullptr) {
393 : return;
394 : }
395 2471 : delete myInstance;
396 2471 : myInstance = nullptr;
397 2471 : myDoCloseConnection = true;
398 : }
399 :
400 :
401 : bool
402 101087013 : TraCIServer::wasClosed() {
403 101087013 : return myDoCloseConnection;
404 : }
405 :
406 :
407 : // ---------- Initialisation and Shutdown
408 :
409 :
410 : void
411 88203 : TraCIServer::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
412 88203 : if (!myDoCloseConnection) {
413 88203 : myVehicleStateChanges[to].push_back(vehicle->getID());
414 185436 : for (std::map<int, SocketInfo*>::iterator i = mySockets.begin(); i != mySockets.end(); ++i) {
415 97233 : i->second->vehicleStateChanges[to].push_back(vehicle->getID());
416 : }
417 : }
418 88203 : }
419 :
420 :
421 : void
422 1686 : TraCIServer::transportableStateChanged(const MSTransportable* const transportable, MSNet::TransportableState to, const std::string& /*info*/) {
423 1686 : if (!myDoCloseConnection) {
424 1686 : myTransportableStateChanges[to].push_back(transportable->getID());
425 3372 : for (std::map<int, SocketInfo*>::iterator i = mySockets.begin(); i != mySockets.end(); ++i) {
426 1686 : i->second->transportableStateChanges[to].push_back(transportable->getID());
427 : }
428 : }
429 1686 : }
430 :
431 :
432 : void
433 78 : TraCIServer::checkClientOrdering() {
434 : #ifdef DEBUG_MULTI_CLIENTS
435 : std::cout << "Checking client order requests." << std::endl;
436 : #endif
437 : // check for SET_ORDER commands queued by connected clients
438 : // In multiclient cas it is mandatory that SET_ORDER is sent as the first command (or directly after GET_VERSION)
439 78 : myCurrentSocket = mySockets.begin();
440 316 : while (myCurrentSocket != mySockets.end()) {
441 : #ifdef DEBUG_MULTI_CLIENTS
442 : std::cout << " Socket " << myCurrentSocket->second->socket << ":" << std::endl;
443 : #endif
444 : // bool clientUnordered = true;
445 : while (true) {
446 482 : myInputStorage.reset();
447 482 : myCurrentSocket->second->socket->receiveExact(myInputStorage);
448 : int commandStart, commandLength;
449 482 : int commandId = readCommandID(commandStart, commandLength);
450 : #ifdef DEBUG_MULTI_CLIENTS
451 : std::cout << " received command " << commandId << std::endl;
452 : #endif
453 : // Whether the received command is a permitted command for the initialization phase.
454 : // Currently, getVersion and setOrder are permitted.
455 482 : bool initCommand = commandId == libsumo::CMD_SETORDER || commandId == libsumo::CMD_GETVERSION;
456 482 : if (initCommand) {
457 : #ifdef DEBUG_MULTI_CLIENTS
458 : std::cout << " Init command. Sending response." << std::endl;
459 : #endif
460 : // reset input storage to initial state before reading the commandId
461 : // (ugly, but we can't just reset the store's iter_ from here)
462 : // Giving the commandId to dispatch command didn't work either
463 478 : tcpip::Storage tmp;
464 478 : tmp.writeStorage(myInputStorage);
465 478 : myInputStorage.reset();
466 : // we don't know whether the command was set with extended
467 : // length syntax or not so we hardcode the length here (#5037)
468 718 : myInputStorage.writeUnsignedByte(commandId == libsumo::CMD_SETORDER ? 6 : 2);
469 478 : myInputStorage.writeUnsignedByte(commandId);
470 478 : myInputStorage.writeStorage(tmp);
471 :
472 : // Handle initialization command completely
473 478 : dispatchCommand();
474 478 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
475 478 : myOutputStorage.reset();
476 478 : } else {
477 : #ifdef DEBUG_MULTI_CLIENTS
478 : std::cout << " Client " << myCurrentSocket->second->socket << " did not set order initially." << std::endl;
479 : #endif
480 8 : throw ProcessError(TL("Execution order (libsumo::CMD_SETORDER) was not set for all TraCI clients in pre-execution phase."));
481 : }
482 478 : if (commandId == libsumo::CMD_SETORDER) {
483 : // This is what we have waited for.
484 : break;
485 : }
486 240 : }
487 : ++myCurrentSocket;
488 : }
489 74 : }
490 :
491 :
492 : void
493 8402797 : TraCIServer::processReorderingRequests() {
494 : // Process reordering requests
495 8402797 : if (mySocketReorderRequests.size() > 0) {
496 : // process reordering requests
497 : std::map<int, SocketInfo*>::const_iterator i = mySocketReorderRequests.begin();
498 : std::map<int, SocketInfo*>::iterator j;
499 : #ifdef DEBUG_MULTI_CLIENTS
500 : std::cout << SIMTIME << " Current socket ordering:\n";
501 : for (j = mySockets.begin(); j != mySockets.end(); ++j) {
502 : std::cout << " " << j->first << ": " << j->second->socket << "\n";
503 : }
504 : std::cout << "Reordering requests:\n";
505 : for (i = mySocketReorderRequests.begin(); i != mySocketReorderRequests.end(); ++i) {
506 : std::cout << " Socket " << i->second->socket << " -> " << i->first << "\n";
507 : }
508 : i = mySocketReorderRequests.begin();
509 : #endif
510 320 : while (i != mySocketReorderRequests.end()) {
511 : j = mySockets.begin();
512 573 : while (j != mySockets.end()) {
513 573 : if (j->second->socket == i->second->socket) {
514 : break;
515 : } else {
516 : j++;
517 : }
518 : }
519 : assert(j != mySockets.end());
520 : mySockets.erase(j);
521 241 : mySockets[i->first] = i->second;
522 : ++i;
523 : }
524 : mySocketReorderRequests.clear();
525 : #ifdef DEBUG_MULTI_CLIENTS
526 : std::cout << "New socket ordering:\n";
527 : for (j = mySockets.begin(); j != mySockets.end(); ++j) {
528 : std::cout << " " << j->first << ": " << j->second->socket << "\n";
529 : }
530 : std::cout << std::endl;
531 : #endif
532 : }
533 8402797 : }
534 :
535 :
536 : SUMOTime
537 16761791 : TraCIServer::nextTargetTime() const {
538 : #ifdef DEBUG_MULTI_CLIENTS
539 : std::cout << "\n Determining new target time..." << std::endl;
540 : if (mySockets.size() == 0) {
541 : std::cout << " All clients have disconnected." << std::endl;
542 : }
543 : #endif
544 : std::map<int, SocketInfo*>::const_iterator i;
545 : SUMOTime targetTime = std::numeric_limits<SUMOTime>::max();
546 33630220 : for (i = mySockets.begin(); i != mySockets.end(); ++i) {
547 : #ifdef DEBUG_MULTI_CLIENTS
548 : std::cout << " target time for client " << i->second->socket << ": " << i->second->targetTime << "\n";
549 : #endif
550 16868429 : targetTime = MIN2(targetTime, i->second->targetTime);
551 : }
552 : #ifdef DEBUG_MULTI_CLIENTS
553 : std::cout << std::endl;
554 : #endif
555 16761791 : return targetTime;
556 : }
557 :
558 :
559 : // send out subscription results to clients which will act in this step (i.e. with client target time <= myTargetTime)
560 : void
561 8400312 : TraCIServer::sendOutputToAll() const {
562 : #ifdef DEBUG_MULTI_CLIENTS
563 : std::cout << "\n Sending subscription results to clients:\n";
564 : #endif
565 : std::map<int, SocketInfo*>::const_iterator i = mySockets.begin();
566 16834764 : while (i != mySockets.end()) {
567 8434452 : if (i->second->targetTime <= MSNet::getInstance()->getCurrentTimeStep()) {
568 : // this client will become active before the next SUMO step. Provide subscription results.
569 8360481 : i->second->socket->sendExact(myOutputStorage);
570 : #ifdef DEBUG_MULTI_CLIENTS
571 : std::cout << i->second->socket << "\n";
572 : #endif
573 : }
574 : ++i;
575 : }
576 : #ifdef DEBUG_MULTI_CLIENTS
577 : std::cout << std::endl;
578 : #endif
579 8400312 : }
580 :
581 :
582 : int
583 8402797 : TraCIServer::processCommands(const SUMOTime step, const bool afterMove) {
584 : #ifdef DEBUG_MULTI_CLIENTS
585 : std::cout << SIMTIME << " processCommands(step = " << step << "):\n" << std::endl;
586 : #endif
587 : try {
588 : int finalCmd = 0;
589 : const bool firstStep = myCurrentSocket != mySockets.end();
590 : // update client order if requested
591 8402797 : processReorderingRequests();
592 8402797 : if (!firstStep && !afterMove) {
593 : // This is the entry point after performing a SUMO step (block is skipped before first SUMO step since then no simulation results have to be sent)
594 : // update subscription results
595 8400312 : postProcessSimulationStep();
596 : // Send out subscription results to clients which will act in this SUMO step (i.e. with client target time <= current sumo timestep end)
597 8400312 : sendOutputToAll();
598 8400312 : myOutputStorage.reset();
599 : }
600 :
601 : // determine minimal next target time among clients
602 8402797 : myTargetTime = nextTargetTime();
603 :
604 8402797 : if (step < myTargetTime) {
605 : #ifdef DEBUG_MULTI_CLIENTS
606 : std::cout << " next target time is larger than next SUMO simstep (" << step << "). Returning from processCommands()." << std::endl;
607 : #endif
608 : return finalCmd;
609 : }
610 :
611 : // Simulation should run until
612 : // 1. end time reached or
613 : // 2. got libsumo::CMD_CLOSE or
614 : // 3. got libsumo::CMD_LOAD or
615 : // 4. Client closes socket connection
616 16701412 : while (!myDoCloseConnection && myTargetTime <= (MSNet::getInstance()->getCurrentTimeStep()) && finalCmd != libsumo::CMD_EXECUTEMOVE) {
617 : #ifdef DEBUG_MULTI_CLIENTS
618 : std::cout << " Next target time: " << myTargetTime << std::endl;
619 : #endif
620 : // Iterate over clients and process communication for the ones with target time == myTargetTime
621 8361472 : myCurrentSocket = mySockets.begin();
622 16795417 : while (myCurrentSocket != mySockets.end()) {
623 : #ifdef DEBUG_MULTI_CLIENTS
624 : std::cout << " current socket: " << myCurrentSocket->second->socket
625 : << " with target time=" << myCurrentSocket->second->targetTime
626 : << std::endl;
627 : #endif
628 :
629 8433970 : if (myCurrentSocket->second->targetTime > myTargetTime || (afterMove && !myCurrentSocket->second->executeMove)) {
630 : // this client must wait
631 : #ifdef DEBUG_MULTI_CLIENTS
632 : std::cout << " skipping client " << myCurrentSocket->second->socket
633 : << " with target time=" << myCurrentSocket->second->targetTime << std::endl;
634 : #endif
635 : myCurrentSocket++;
636 51960 : continue;
637 : }
638 : finalCmd = 0;
639 26517926 : while (finalCmd == 0) {
640 18135941 : if (!myInputStorage.valid_pos()) {
641 : // have read request completely, send response if adequate
642 18135941 : if (myOutputStorage.size() > 0) {
643 : // send response to previous query
644 9753937 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
645 9753937 : myOutputStorage.reset();
646 : }
647 : #ifdef DEBUG_MULTI_CLIENTS
648 : std::cout << " resetting input storage and reading next command..." << std::endl;
649 : #endif
650 : // Read next request
651 18135941 : myInputStorage.reset();
652 18135941 : myCurrentSocket->second->socket->receiveExact(myInputStorage);
653 : }
654 :
655 36271832 : while (myInputStorage.valid_pos() && !myDoCloseConnection) {
656 18135916 : const int cmd = dispatchCommand();
657 18135916 : if (cmd == libsumo::CMD_SIMSTEP || cmd == libsumo::CMD_LOAD || cmd == libsumo::CMD_EXECUTEMOVE || cmd == libsumo::CMD_CLOSE) {
658 : finalCmd = cmd;
659 : }
660 : }
661 : }
662 : }
663 8361447 : if (!myLoadArgs.empty()) {
664 : #ifdef DEBUG_MULTI_CLIENTS
665 : std::cout << " Breaking loop to load new simulation." << std::endl;
666 : #endif
667 : break;
668 8361434 : } else if (myDoCloseConnection) {
669 : #ifdef DEBUG_MULTI_CLIENTS
670 : std::cout << " Breaking loop because last client closed connection." << std::endl;
671 : #endif
672 : break;
673 : }
674 8358994 : SUMOTime nextT = nextTargetTime();
675 : // minimal target time among clients should have been increased during the last loop through mySockets
676 : // XXX: The assert below is disabled since many tests do sth. like simulationStep(step). Such that for a first call step=0,
677 : // leading to targetTime==1000 (increased by DELTA_T in dispatchCommand()),
678 : // the next call is then usually simulationStep(step=1000) leading to no further increase
679 : // and thus a failing assertion here.
680 : //assert(myTargetTime < nextT || myDoCloseConnection);
681 8358994 : myTargetTime = nextT;
682 : }
683 : // All clients are done with the current time step
684 : // Reset myVehicleStateChanges and myTransportableStateChanges
685 116793502 : for (auto& item : myVehicleStateChanges) {
686 : item.second.clear();
687 : }
688 41711965 : for (auto& item : myTransportableStateChanges) {
689 : item.second.clear();
690 : }
691 : return finalCmd;
692 25 : } catch (std::invalid_argument& e) {
693 0 : throw ProcessError(e.what());
694 0 : } catch (libsumo::TraCIException& e) {
695 0 : throw ProcessError(e.what());
696 25 : } catch (tcpip::SocketException& e) {
697 50 : throw ProcessError(e.what());
698 25 : }
699 : }
700 :
701 :
702 : void
703 2480 : TraCIServer::cleanup() {
704 : mySubscriptions.clear();
705 4960 : myTargetTime = string2time(OptionsCont::getOptions().getString("begin"));
706 2520 : for (myCurrentSocket = mySockets.begin(); myCurrentSocket != mySockets.end(); ++myCurrentSocket) {
707 40 : myCurrentSocket->second->targetTime = myTargetTime;
708 40 : myCurrentSocket->second->executeMove = false;
709 : }
710 2480 : myOutputStorage.reset();
711 2480 : myInputStorage.reset();
712 2480 : mySubscriptionCache.reset();
713 34720 : for (auto& i : myVehicleStateChanges) {
714 : i.second.clear();
715 : }
716 12400 : for (auto& i : myTransportableStateChanges) {
717 : i.second.clear();
718 : }
719 2480 : myCurrentSocket = mySockets.begin();
720 2480 : }
721 :
722 :
723 : std::map<int, TraCIServer::SocketInfo*>::iterator
724 2602 : TraCIServer::removeCurrentSocket() {
725 : #ifdef DEBUG_MULTI_CLIENTS
726 : std::cout << " Removing socket " << myCurrentSocket->second->socket
727 : << " (order " << myCurrentSocket->first << ")" << std::endl;
728 : #endif
729 2602 : delete myCurrentSocket->second;
730 2602 : myCurrentSocket = mySockets.erase(myCurrentSocket);
731 2602 : return myCurrentSocket;
732 : }
733 :
734 :
735 : int
736 18136876 : TraCIServer::readCommandID(int& commandStart, int& commandLength) {
737 18136876 : commandStart = myInputStorage.position();
738 18136876 : commandLength = myInputStorage.readUnsignedByte();
739 18136876 : if (commandLength == 0) {
740 48661 : commandLength = myInputStorage.readInt();
741 : }
742 : #ifdef DEBUG_RAW_INPUT
743 : std::cout << " commandStart=" << commandStart << " commandLength=" << commandLength << " pos=" << myInputStorage.position() << " raw=";
744 : for (auto it = myInputStorage.begin(); it != myInputStorage.end(); ++it) {
745 : std::cout << (int)*it << " ";
746 : }
747 : std::cout << "\n";
748 : #endif
749 18136876 : return myInputStorage.readUnsignedByte();
750 : }
751 :
752 :
753 : int
754 18136394 : TraCIServer::dispatchCommand() {
755 : int commandStart, commandLength;
756 18136394 : int commandId = readCommandID(commandStart, commandLength);
757 : #ifdef DEBUG_MULTI_CLIENTS
758 : std::cout << " dispatchCommand() called for client " << myCurrentSocket->second->socket
759 : << ", commandId = " << commandId << std::endl;
760 : #endif
761 : bool success = false;
762 : // dispatch commands
763 18136394 : if (myExecutors.find(commandId) != myExecutors.end()) {
764 9744641 : success = myExecutors[commandId](*this, myInputStorage, myOutputStorage);
765 : } else {
766 8391753 : switch (commandId) {
767 2020 : case libsumo::CMD_GETVERSION:
768 2020 : success = commandGetVersion();
769 : break;
770 : case libsumo::CMD_LOAD: {
771 : std::vector<std::string> args;
772 13 : if (!readTypeCheckingStringList(myInputStorage, args)) {
773 0 : return writeErrorStatusCmd(libsumo::CMD_LOAD, "A load command needs a list of string arguments.", myOutputStorage);
774 : }
775 : #ifdef DEBUG_MULTI_CLIENTS
776 : std::cout << " commandId == libsumo::CMD_LOAD"
777 : << ", args = " << toString(args) << std::endl;
778 : #endif
779 : try {
780 13 : myLoadArgs = args;
781 : success = true;
782 26 : writeStatusCmd(libsumo::CMD_LOAD, libsumo::RTYPE_OK, "");
783 : // XXX: This only cares for the client that issued the load command.
784 : // Multiclient-load functionality is still to be implemented. Refs #3146.
785 13 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
786 13 : myCurrentSocket = mySockets.end();
787 13 : myOutputStorage.reset();
788 0 : } catch (libsumo::TraCIException& e) {
789 0 : return writeErrorStatusCmd(libsumo::CMD_LOAD, e.what(), myOutputStorage);
790 0 : }
791 : break;
792 13 : }
793 6 : case libsumo::CMD_EXECUTEMOVE:
794 6 : myCurrentSocket->second->executeMove = true;
795 : myCurrentSocket++;
796 : success = true;
797 12 : writeStatusCmd(libsumo::CMD_EXECUTEMOVE, libsumo::RTYPE_OK, "");
798 : break;
799 8379364 : case libsumo::CMD_SIMSTEP: {
800 8379364 : const double nextT = myInputStorage.readDouble();
801 8379364 : if (nextT == 0.) {
802 8333498 : myCurrentSocket->second->targetTime += DELTA_T;
803 : } else {
804 45866 : myCurrentSocket->second->targetTime = TIME2STEPS(nextT);
805 : }
806 8379364 : myCurrentSocket->second->executeMove = false;
807 : #ifdef DEBUG_MULTI_CLIENTS
808 : std::cout << " commandId == libsumo::CMD_SIMSTEP"
809 : << ", next target time for client is " << myCurrentSocket->second->targetTime << std::endl;
810 : #endif
811 8379364 : if (myCurrentSocket->second->targetTime <= MSNet::getInstance()->getCurrentTimeStep()) {
812 : // This is not the last TraCI simstep in the current SUMO simstep -> send single simstep response.
813 : // @note: In the other case the simstep results are sent to all after the SUMO step was performed, see entry point for processCommands()
814 18882 : sendSingleSimStepResponse();
815 : }
816 : // Clear vehicleStateChanges and transportableStateChanges for this client
817 : // -> For subsequent TraCI stepping
818 : // that is performed within this SUMO step, no updates on vehicle states
819 : // belonging to the last SUMO simulation step will be received by this client.
820 117311096 : for (auto& item : myCurrentSocket->second->vehicleStateChanges) {
821 : item.second.clear();
822 : }
823 41896820 : for (auto& item : myCurrentSocket->second->transportableStateChanges) {
824 : item.second.clear();
825 : }
826 : myCurrentSocket++;
827 8379364 : return commandId;
828 : }
829 : case libsumo::CMD_CLOSE:
830 5204 : writeStatusCmd(libsumo::CMD_CLOSE, libsumo::RTYPE_OK, "");
831 2602 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
832 2602 : myOutputStorage.reset();
833 2602 : if (mySockets.size() == 1) {
834 : // Last client has closed connection
835 2440 : myDoCloseConnection = true;
836 : }
837 : // remove current socket and increment to next socket in ordering
838 2602 : myCurrentSocket = removeCurrentSocket();
839 : success = true;
840 : break;
841 244 : case libsumo::CMD_SETORDER: {
842 244 : const int order = myInputStorage.readInt();
843 : #ifdef DEBUG_MULTI_CLIENTS
844 : std::cout << " commandId == libsumo::CMD_SETORDER"
845 : << ", order index is " << order << std::endl;
846 : #endif
847 244 : if (order > libsumo::MAX_ORDER) {
848 0 : return writeErrorStatusCmd(libsumo::CMD_SETORDER, "A set order command needs an int argument below " + toString(libsumo::MAX_ORDER) + ".", myOutputStorage);
849 : }
850 : if (mySockets.count(order) > 0 || mySocketReorderRequests.count(order) > 0) {
851 0 : return writeErrorStatusCmd(libsumo::CMD_SETORDER, "Order '" + toString(order) + "' is already taken.", myOutputStorage);
852 : }
853 : // memorize reorder request (will only take effect in the next step)
854 244 : mySocketReorderRequests[order] = myCurrentSocket->second;
855 : success = true;
856 244 : writeStatusCmd(libsumo::CMD_SETORDER, libsumo::RTYPE_OK, "");
857 244 : break;
858 : }
859 3652 : case libsumo::CMD_SUBSCRIBE_BUSSTOP_VARIABLE:
860 : case libsumo::CMD_SUBSCRIBE_CALIBRATOR_VARIABLE:
861 : case libsumo::CMD_SUBSCRIBE_CHARGINGSTATION_VARIABLE:
862 : case libsumo::CMD_SUBSCRIBE_EDGE_VARIABLE:
863 : case libsumo::CMD_SUBSCRIBE_GUI_VARIABLE:
864 : case libsumo::CMD_SUBSCRIBE_INDUCTIONLOOP_VARIABLE:
865 : case libsumo::CMD_SUBSCRIBE_JUNCTION_VARIABLE:
866 : case libsumo::CMD_SUBSCRIBE_LANE_VARIABLE:
867 : case libsumo::CMD_SUBSCRIBE_LANEAREA_VARIABLE:
868 : case libsumo::CMD_SUBSCRIBE_MEANDATA_VARIABLE:
869 : case libsumo::CMD_SUBSCRIBE_MULTIENTRYEXIT_VARIABLE:
870 : case libsumo::CMD_SUBSCRIBE_OVERHEADWIRE_VARIABLE:
871 : case libsumo::CMD_SUBSCRIBE_PARKINGAREA_VARIABLE:
872 : case libsumo::CMD_SUBSCRIBE_PERSON_VARIABLE:
873 : case libsumo::CMD_SUBSCRIBE_POI_VARIABLE:
874 : case libsumo::CMD_SUBSCRIBE_POLYGON_VARIABLE:
875 : case libsumo::CMD_SUBSCRIBE_REROUTER_VARIABLE:
876 : case libsumo::CMD_SUBSCRIBE_ROUTE_VARIABLE:
877 : case libsumo::CMD_SUBSCRIBE_ROUTEPROBE_VARIABLE:
878 : case libsumo::CMD_SUBSCRIBE_SIM_VARIABLE:
879 : case libsumo::CMD_SUBSCRIBE_TL_VARIABLE:
880 : case libsumo::CMD_SUBSCRIBE_VARIABLESPEEDSIGN_VARIABLE:
881 : case libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE:
882 : case libsumo::CMD_SUBSCRIBE_VEHICLETYPE_VARIABLE:
883 3652 : success = addObjectVariableSubscription(commandId, false);
884 : break;
885 3364 : case libsumo::CMD_SUBSCRIBE_BUSSTOP_CONTEXT:
886 : case libsumo::CMD_SUBSCRIBE_CALIBRATOR_CONTEXT:
887 : case libsumo::CMD_SUBSCRIBE_CHARGINGSTATION_CONTEXT:
888 : case libsumo::CMD_SUBSCRIBE_EDGE_CONTEXT:
889 : case libsumo::CMD_SUBSCRIBE_GUI_CONTEXT:
890 : case libsumo::CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT:
891 : case libsumo::CMD_SUBSCRIBE_JUNCTION_CONTEXT:
892 : case libsumo::CMD_SUBSCRIBE_LANE_CONTEXT:
893 : case libsumo::CMD_SUBSCRIBE_LANEAREA_CONTEXT:
894 : case libsumo::CMD_SUBSCRIBE_MEANDATA_CONTEXT:
895 : case libsumo::CMD_SUBSCRIBE_MULTIENTRYEXIT_CONTEXT:
896 : case libsumo::CMD_SUBSCRIBE_OVERHEADWIRE_CONTEXT:
897 : case libsumo::CMD_SUBSCRIBE_PARKINGAREA_CONTEXT:
898 : case libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT:
899 : case libsumo::CMD_SUBSCRIBE_POI_CONTEXT:
900 : case libsumo::CMD_SUBSCRIBE_POLYGON_CONTEXT:
901 : case libsumo::CMD_SUBSCRIBE_REROUTER_CONTEXT:
902 : case libsumo::CMD_SUBSCRIBE_ROUTE_CONTEXT:
903 : case libsumo::CMD_SUBSCRIBE_ROUTEPROBE_CONTEXT:
904 : case libsumo::CMD_SUBSCRIBE_SIM_CONTEXT:
905 : case libsumo::CMD_SUBSCRIBE_TL_CONTEXT:
906 : case libsumo::CMD_SUBSCRIBE_VARIABLESPEEDSIGN_CONTEXT:
907 : case libsumo::CMD_SUBSCRIBE_VEHICLE_CONTEXT:
908 : case libsumo::CMD_SUBSCRIBE_VEHICLETYPE_CONTEXT:
909 3364 : success = addObjectVariableSubscription(commandId, true);
910 : break;
911 387 : case libsumo::CMD_ADD_SUBSCRIPTION_FILTER:
912 387 : success = addSubscriptionFilter();
913 : break;
914 101 : default:
915 101 : if (commandId == libsumo::CMD_GET_GUI_VARIABLE || commandId == libsumo::CMD_SET_GUI_VARIABLE) {
916 200 : writeStatusCmd(commandId, libsumo::RTYPE_NOTIMPLEMENTED, "GUI is not running, command not implemented in command line sumo");
917 : } else {
918 2 : writeStatusCmd(commandId, libsumo::RTYPE_NOTIMPLEMENTED, "Command not implemented in sumo");
919 : }
920 : }
921 : }
922 9754165 : if (!success) {
923 2100 : while (myInputStorage.valid_pos() && (int)myInputStorage.position() < commandStart + commandLength) {
924 1148 : myInputStorage.readChar();
925 : }
926 : }
927 9757030 : if ((int)myInputStorage.position() != commandStart + commandLength) {
928 0 : std::ostringstream msg;
929 0 : msg << "Wrong position in requestMessage after dispatching command " << commandId << ".";
930 0 : msg << " Expected command length was " << commandLength;
931 0 : msg << " but " << myInputStorage.position() - commandStart << " Bytes were read.";
932 0 : writeStatusCmd(commandId, libsumo::RTYPE_ERR, msg.str());
933 0 : myDoCloseConnection = true;
934 0 : }
935 9757030 : return commandId;
936 : }
937 :
938 :
939 : // ---------- Server-internal command handling
940 : bool
941 2020 : TraCIServer::commandGetVersion() {
942 : // Prepare response
943 2020 : tcpip::Storage answerTmp;
944 2020 : answerTmp.writeInt(libsumo::TRACI_VERSION);
945 2020 : answerTmp.writeString("SUMO " VERSION_STRING);
946 : // When we get here, the response is stored in answerTmp -> put into myOutputStorage
947 2020 : writeStatusCmd(libsumo::CMD_GETVERSION, libsumo::RTYPE_OK, "");
948 : // command length
949 2020 : myOutputStorage.writeUnsignedByte(1 + 1 + static_cast<int>(answerTmp.size()));
950 : // command type
951 2020 : myOutputStorage.writeUnsignedByte(libsumo::CMD_GETVERSION);
952 : // and the parameter dependant part
953 2020 : myOutputStorage.writeStorage(answerTmp);
954 2020 : return true;
955 2020 : }
956 :
957 :
958 : void
959 8400312 : TraCIServer::postProcessSimulationStep() {
960 8400312 : SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
961 : #ifdef DEBUG_MULTI_CLIENTS
962 : std::cout << " postProcessSimulationStep() at time=" << t << std::endl;
963 : #endif
964 16800624 : writeStatusCmd(libsumo::CMD_SIMSTEP, libsumo::RTYPE_OK, "");
965 : int noActive = 0;
966 12589732 : for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
967 : const libsumo::Subscription& s = *i;
968 4189420 : bool isArrivedVehicle = (s.commandId == libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == libsumo::CMD_SUBSCRIBE_VEHICLE_CONTEXT)
969 4189420 : && (find(myVehicleStateChanges[MSNet::VehicleState::ARRIVED].begin(), myVehicleStateChanges[MSNet::VehicleState::ARRIVED].end(), s.id) != myVehicleStateChanges[MSNet::VehicleState::ARRIVED].end());
970 :
971 4189420 : bool isArrivedPerson = (s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_VARIABLE || s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT) && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
972 4189420 : if ((s.endTime < t) || isArrivedVehicle || isArrivedPerson) {
973 2042 : i = mySubscriptions.erase(i);
974 2042 : continue;
975 : }
976 : ++i;
977 4187378 : if (s.beginTime > t) {
978 4 : continue;
979 : }
980 4187374 : ++noActive;
981 : }
982 8400312 : mySubscriptionCache.reset();
983 : #ifdef DEBUG_SUBSCRIPTIONS
984 : std::cout << " Initial size of mySubscriptionCache is " << mySubscriptionCache.size()
985 : << "\n Nr. of active subscriptions = " << noActive << std::endl;
986 : #endif
987 8400312 : mySubscriptionCache.writeInt(noActive);
988 : #ifdef DEBUG_SUBSCRIPTIONS
989 : std::cout << " Size after writing an int is " << mySubscriptionCache.size() << std::endl;
990 : #endif
991 12587690 : for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
992 : const libsumo::Subscription& s = *i;
993 4187378 : if (s.beginTime > t) {
994 : ++i;
995 4 : continue;
996 : }
997 4187374 : tcpip::Storage into;
998 : std::string errors;
999 4187374 : bool ok = processSingleSubscription(s, into, errors);
1000 : #ifdef DEBUG_SUBSCRIPTIONS
1001 : std::cout << " Size of into-store for subscription " << s.id
1002 : << ": " << into.size() << std::endl;
1003 : #endif
1004 4187374 : mySubscriptionCache.writeStorage(into);
1005 4187374 : if (ok) {
1006 : ++i;
1007 : } else {
1008 0 : i = mySubscriptions.erase(i);
1009 : }
1010 4187374 : }
1011 8400312 : myOutputStorage.writeStorage(mySubscriptionCache);
1012 : #ifdef DEBUG_SUBSCRIPTIONS
1013 : std::cout << " Size after writing subscriptions is " << mySubscriptionCache.size() << std::endl;
1014 : #endif
1015 8400312 : }
1016 :
1017 :
1018 : void
1019 18882 : TraCIServer::sendSingleSimStepResponse() {
1020 : #ifdef DEBUG_MULTI_CLIENTS
1021 : std::cout << " Sending cached simstep response to current client " << myCurrentSocket->second->socket
1022 : << " (-> intermediate TraCI step)."
1023 : << "\n Size of mySubscriptionCache is " << mySubscriptionCache.size()
1024 : << std::endl;
1025 : #endif
1026 18882 : writeStatusCmd(libsumo::CMD_SIMSTEP, libsumo::RTYPE_OK, "");
1027 :
1028 : // NOTE: the commented code would send an empty response
1029 : // myOutputStorage.writeInt(0);
1030 : // myCurrentSocket->second->socket->sendExact(myOutputStorage);
1031 : // myOutputStorage.reset();
1032 18882 : myOutputStorage.writeStorage(mySubscriptionCache);
1033 : // send results to active client
1034 18882 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
1035 18882 : myOutputStorage.reset();
1036 18882 : }
1037 :
1038 :
1039 : void
1040 8431588 : TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description) {
1041 8431588 : writeStatusCmd(commandId, status, description, myOutputStorage);
1042 8431588 : }
1043 :
1044 :
1045 : void
1046 26624816 : TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description, tcpip::Storage& outputStorage) {
1047 26624816 : if (status == libsumo::RTYPE_ERR) {
1048 5277 : WRITE_ERROR("Answered with error to command " + toHex(commandId, 2) + ": " + description);
1049 26623057 : } else if (status == libsumo::RTYPE_NOTIMPLEMENTED) {
1050 303 : WRITE_ERROR("Requested command not implemented (" + toHex(commandId, 2) + "): " + description);
1051 : }
1052 26624816 : outputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(description.length())); // command length
1053 26624816 : outputStorage.writeUnsignedByte(commandId); // command type
1054 26624816 : outputStorage.writeUnsignedByte(status); // status
1055 26624816 : outputStorage.writeString(description); // description
1056 26624816 : }
1057 :
1058 :
1059 : bool
1060 847 : TraCIServer::writeErrorStatusCmd(int commandId, const std::string& description, tcpip::Storage& outputStorage) {
1061 847 : writeStatusCmd(commandId, libsumo::RTYPE_ERR, description, outputStorage);
1062 847 : return false;
1063 : }
1064 :
1065 :
1066 : void
1067 5821 : TraCIServer::initialiseSubscription(libsumo::Subscription& s) {
1068 5821 : tcpip::Storage writeInto;
1069 : std::string errors;
1070 5821 : libsumo::Subscription* modifiedSubscription = nullptr;
1071 : try {
1072 5821 : if (processSingleSubscription(s, writeInto, errors)) {
1073 4916 : if (s.endTime < MSNet::getInstance()->getCurrentTimeStep()) {
1074 0 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, "Subscription has ended.");
1075 : } else {
1076 4916 : if (libsumo::Helper::needNewSubscription(s, mySubscriptions, modifiedSubscription)) {
1077 : // Add new subscription to subscription cache (note: seems a bit inefficient)
1078 4145 : if (s.beginTime < MSNet::getInstance()->getCurrentTimeStep()) {
1079 : // copy new subscription into cache
1080 4123 : int noActive = 1 + (mySubscriptionCache.size() > 0 ? mySubscriptionCache.readInt() : 0);
1081 4123 : tcpip::Storage tmp;
1082 4123 : tmp.writeInt(noActive);
1083 28518371 : while (mySubscriptionCache.valid_pos()) {
1084 28514248 : tmp.writeByte(mySubscriptionCache.readByte());
1085 : }
1086 4123 : tmp.writeStorage(writeInto);
1087 4123 : mySubscriptionCache.reset();
1088 4123 : mySubscriptionCache.writeStorage(tmp);
1089 4123 : }
1090 : }
1091 9832 : writeStatusCmd(s.commandId, libsumo::RTYPE_OK, "");
1092 : }
1093 4916 : if (modifiedSubscription != nullptr && (
1094 : modifiedSubscription->isVehicleToVehicleContextSubscription()
1095 : || modifiedSubscription->isVehicleToPersonContextSubscription())) {
1096 : // Set last modified vehicle context subscription active for filter modifications
1097 148 : myLastContextSubscription = modifiedSubscription;
1098 : } else {
1099 : // adding other subscriptions deactivates the activation for filter addition
1100 4768 : myLastContextSubscription = nullptr;
1101 : }
1102 : } else {
1103 6 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, "Could not add subscription. " + errors);
1104 : }
1105 902 : } catch (libsumo::TraCIException& e) {
1106 902 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, e.what());
1107 902 : }
1108 5821 : myOutputStorage.writeStorage(writeInto);
1109 5821 : }
1110 :
1111 :
1112 : void
1113 1195 : TraCIServer::removeSubscription(int commandId, const std::string& id, int domain) {
1114 : bool found = false;
1115 : std::vector<libsumo::Subscription>::iterator j;
1116 2420 : for (j = mySubscriptions.begin(); j != mySubscriptions.end();) {
1117 1225 : if (j->id == id && j->commandId == commandId && j->contextDomain == domain) {
1118 1195 : j = mySubscriptions.erase(j);
1119 1195 : if (j != mySubscriptions.end() && myLastContextSubscription == &(*j)) {
1120 : // Remove also reference for filter additions
1121 0 : myLastContextSubscription = nullptr;
1122 : }
1123 : found = true;
1124 1195 : continue;
1125 : }
1126 : ++j;
1127 : }
1128 : // try unsubscribe
1129 1195 : if (found) {
1130 2390 : writeStatusCmd(commandId, libsumo::RTYPE_OK, "");
1131 : } else {
1132 0 : writeStatusCmd(commandId, libsumo::RTYPE_ERR, "The subscription to remove was not found.");
1133 : }
1134 1195 : }
1135 :
1136 :
1137 : bool
1138 4193195 : TraCIServer::processSingleSubscription(const libsumo::Subscription& s, tcpip::Storage& writeInto,
1139 : std::string& errors) {
1140 : bool ok = true;
1141 4193195 : tcpip::Storage outputStorage;
1142 4193195 : const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
1143 : std::set<std::string> objIDs;
1144 4193195 : if (s.contextDomain > 0) {
1145 61313 : if ((s.activeFilters & libsumo::SUBS_FILTER_NO_RTREE) == 0) {
1146 59607 : PositionVector shape;
1147 59607 : libsumo::Helper::findObjectShape(s.commandId, s.id, shape);
1148 59607 : libsumo::Helper::collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
1149 59607 : }
1150 60411 : libsumo::Helper::applySubscriptionFilters(s, objIDs);
1151 : } else {
1152 4131882 : objIDs.insert(s.id);
1153 : }
1154 4192293 : const int numVars = s.contextDomain > 0 && s.variables.size() == 1 && s.variables[0] == libsumo::TRACI_ID_LIST ? 0 : (int)s.variables.size();
1155 : int skipped = 0;
1156 8518206 : for (std::set<std::string>::iterator j = objIDs.begin(); j != objIDs.end(); ++j) {
1157 4325913 : if (s.contextDomain > 0) {
1158 : //if (centralObject(s, *j)) {
1159 : // skipped++;
1160 : // continue;
1161 : //}
1162 194031 : outputStorage.writeString(*j);
1163 : }
1164 4325913 : if (numVars > 0) {
1165 : std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
1166 12769244 : for (std::vector<int>::const_iterator i = s.variables.begin(); i != s.variables.end(); ++i, ++k) {
1167 8448592 : tcpip::Storage message;
1168 8448592 : message.writeUnsignedByte(*i);
1169 8448592 : message.writeString(*j);
1170 : // TODO check why writeStorage fails here (probably some kind of invalid iterator)
1171 8533399 : for (const auto& v :** k) {
1172 84807 : message.writeChar(v);
1173 : }
1174 8448592 : tcpip::Storage tmpOutput;
1175 8448592 : if (myExecutors.find(getCommandId) != myExecutors.end()) {
1176 8448592 : ok &= myExecutors[getCommandId](*this, message, tmpOutput);
1177 : } else {
1178 0 : writeStatusCmd(s.commandId, libsumo::RTYPE_NOTIMPLEMENTED, "Unsupported command specified", tmpOutput);
1179 : ok = false;
1180 : }
1181 : // copy response part
1182 8448592 : if (ok) {
1183 8448589 : int length = tmpOutput.readUnsignedByte();
1184 59140123 : while (--length > 0) {
1185 50691534 : tmpOutput.readUnsignedByte();
1186 : }
1187 : int lengthLength = 1;
1188 8448589 : length = tmpOutput.readUnsignedByte();
1189 8448589 : if (length == 0) {
1190 : lengthLength = 5;
1191 12 : length = tmpOutput.readInt();
1192 : }
1193 : //read responseType
1194 8448589 : tmpOutput.readUnsignedByte();
1195 8448589 : int variable = tmpOutput.readUnsignedByte();
1196 8448589 : std::string id = tmpOutput.readString();
1197 8448589 : outputStorage.writeUnsignedByte(variable);
1198 8448589 : outputStorage.writeUnsignedByte(libsumo::RTYPE_OK);
1199 8448589 : length -= (lengthLength + 1 + 4 + (int)id.length());
1200 106931849 : while (--length > 0) {
1201 98483260 : outputStorage.writeUnsignedByte(tmpOutput.readUnsignedByte());
1202 : }
1203 : } else {
1204 : //read length
1205 3 : tmpOutput.readUnsignedByte();
1206 : //read cmd
1207 3 : tmpOutput.readUnsignedByte();
1208 : //read status
1209 3 : tmpOutput.readUnsignedByte();
1210 3 : std::string msg = tmpOutput.readString();
1211 3 : outputStorage.writeUnsignedByte(*i);
1212 3 : outputStorage.writeUnsignedByte(libsumo::RTYPE_ERR);
1213 3 : outputStorage.writeUnsignedByte(libsumo::TYPE_STRING);
1214 3 : outputStorage.writeString(msg);
1215 6 : errors = errors + msg;
1216 : }
1217 8448592 : }
1218 : }
1219 : }
1220 4192293 : int length = (1 + 4) + 1 + (4 + (int)s.id.length()) + 1 + (int)outputStorage.size();
1221 4192293 : if (s.contextDomain > 0) {
1222 60411 : length += 1 + 4; // context domain and number of objects
1223 : }
1224 : // we always write extended command length here for backward compatibility
1225 4192293 : writeInto.writeUnsignedByte(0); // command length -> extended
1226 4192293 : writeInto.writeInt(length);
1227 4192293 : writeInto.writeUnsignedByte(s.commandId + 0x10);
1228 4192293 : writeInto.writeString(s.id);
1229 4192293 : if (s.contextDomain > 0) {
1230 60411 : writeInto.writeUnsignedByte(s.contextDomain);
1231 : }
1232 4192293 : writeInto.writeUnsignedByte(numVars);
1233 4192293 : if (s.contextDomain > 0) {
1234 60411 : writeInto.writeInt((int)objIDs.size() - skipped);
1235 : }
1236 4192293 : if (s.contextDomain == 0 || objIDs.size() != 0) {
1237 4170854 : writeInto.writeStorage(outputStorage);
1238 : }
1239 4192293 : return ok;
1240 4193195 : }
1241 :
1242 :
1243 : bool
1244 7016 : TraCIServer::addObjectVariableSubscription(const int commandId, const bool hasContext) {
1245 7016 : const double beginTime = myInputStorage.readDouble();
1246 7016 : const double endTime = myInputStorage.readDouble();
1247 7016 : const SUMOTime begin = beginTime == libsumo::INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
1248 7016 : const SUMOTime end = endTime == libsumo::INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
1249 7016 : const std::string id = myInputStorage.readString();
1250 7016 : const int domain = hasContext ? myInputStorage.readUnsignedByte() : 0;
1251 3364 : double range = hasContext ? myInputStorage.readDouble() : 0.;
1252 7016 : if (commandId == libsumo::CMD_SUBSCRIBE_SIM_CONTEXT) {
1253 : range = std::numeric_limits<double>::max();
1254 : }
1255 7016 : const int num = myInputStorage.readUnsignedByte();
1256 : std::vector<int> variables;
1257 : std::vector<std::shared_ptr<tcpip::Storage> > parameters;
1258 15689 : for (int i = 0; i < num; ++i) {
1259 8673 : const int varID = myInputStorage.readUnsignedByte();
1260 8673 : variables.push_back(varID);
1261 8673 : parameters.push_back(std::make_shared<tcpip::Storage>());
1262 8673 : if ((myParameterized.count(std::make_pair(0, varID)) > 0) || (myParameterized.count(std::make_pair(commandId, varID)) > 0)) {
1263 57 : const int parType = myInputStorage.readUnsignedByte();
1264 57 : parameters.back()->writeUnsignedByte(parType);
1265 57 : if (parType == libsumo::TYPE_DOUBLE) {
1266 36 : parameters.back()->writeDouble(myInputStorage.readDouble());
1267 21 : } else if (parType == libsumo::TYPE_STRING) {
1268 42 : parameters.back()->writeString(myInputStorage.readString());
1269 : } else {
1270 : // Error!
1271 : }
1272 : }
1273 : }
1274 : // check subscribe/unsubscribe
1275 7016 : if (variables.empty()) {
1276 1195 : removeSubscription(commandId, id, domain);
1277 : return true;
1278 : }
1279 : // process subscription
1280 5821 : libsumo::Subscription s(commandId, id, variables, parameters, begin, end, domain, range);
1281 5821 : initialiseSubscription(s);
1282 : return true;
1283 7016 : }
1284 :
1285 :
1286 :
1287 : bool
1288 387 : TraCIServer::addSubscriptionFilter() {
1289 : bool success = true;
1290 : // Read filter type
1291 387 : int filterType = myInputStorage.readUnsignedByte();
1292 :
1293 387 : if (myLastContextSubscription == nullptr) {
1294 2 : writeStatusCmd(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, libsumo::RTYPE_ERR,
1295 2 : "No previous vehicle context subscription exists to apply filter type " + toHex(filterType, 2));
1296 2 : return false;
1297 : }
1298 :
1299 : // dispatch according to filter type
1300 385 : switch (filterType) {
1301 0 : case libsumo::FILTER_TYPE_NONE:
1302 : // Remove all filters
1303 0 : removeFilters();
1304 : break;
1305 90 : case libsumo::FILTER_TYPE_LANES: {
1306 : // Read relative lanes to consider for context filter
1307 90 : int nrLanes = (int)myInputStorage.readByte();
1308 : std::vector<int> lanes;
1309 243 : for (int i = 0; i < nrLanes; ++i) {
1310 153 : lanes.push_back((int) myInputStorage.readByte());
1311 : }
1312 90 : addSubscriptionFilterLanes(lanes);
1313 90 : }
1314 : break;
1315 29 : case libsumo::FILTER_TYPE_NOOPPOSITE:
1316 : // Add no-opposite filter
1317 29 : addSubscriptionFilterNoOpposite();
1318 : break;
1319 103 : case libsumo::FILTER_TYPE_DOWNSTREAM_DIST: {
1320 103 : myInputStorage.readByte(); // read type double
1321 103 : double dist = myInputStorage.readDouble();
1322 103 : addSubscriptionFilterDownstreamDistance(dist);
1323 : }
1324 : break;
1325 87 : case libsumo::FILTER_TYPE_UPSTREAM_DIST: {
1326 87 : myInputStorage.readByte(); // read type double
1327 87 : double dist = myInputStorage.readDouble();
1328 87 : addSubscriptionFilterUpstreamDistance(dist);
1329 : }
1330 : break;
1331 13 : case libsumo::FILTER_TYPE_LEAD_FOLLOW: {
1332 : // Read relative lanes to consider for context filter
1333 13 : addSubscriptionFilterLeadFollow();
1334 : }
1335 : break;
1336 17 : case libsumo::FILTER_TYPE_TURN: {
1337 17 : myInputStorage.readByte(); // read type double
1338 17 : double dist = myInputStorage.readDouble();
1339 17 : addSubscriptionFilterTurn(dist);
1340 : }
1341 : break;
1342 7 : case libsumo::FILTER_TYPE_VCLASS: {
1343 7 : myInputStorage.readByte(); // read type stringlist
1344 7 : SVCPermissions vClasses = parseVehicleClasses(myInputStorage.readStringList());
1345 7 : addSubscriptionFilterVClass(vClasses);
1346 : }
1347 : break;
1348 7 : case libsumo::FILTER_TYPE_VTYPE: {
1349 7 : myInputStorage.readByte(); // read type stringlist
1350 7 : std::vector<std::string> vTypesVector = myInputStorage.readStringList();
1351 : std::set<std::string> vTypesSet;
1352 : vTypesSet.insert(vTypesVector.begin(), vTypesVector.end());
1353 14 : addSubscriptionFilterVType(vTypesSet);
1354 7 : }
1355 : break;
1356 17 : case libsumo::FILTER_TYPE_FIELD_OF_VISION: {
1357 17 : myInputStorage.readByte(); // read type double
1358 17 : double angle = myInputStorage.readDouble();
1359 17 : addSubscriptionFilterFieldOfVision(angle);
1360 : }
1361 : break;
1362 15 : case libsumo::FILTER_TYPE_LATERAL_DIST: {
1363 15 : myInputStorage.readByte(); // read type double
1364 15 : double dist = myInputStorage.readDouble();
1365 15 : addSubscriptionFilterLateralDistance(dist);
1366 : }
1367 : break;
1368 0 : default:
1369 0 : writeStatusCmd(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, libsumo::RTYPE_NOTIMPLEMENTED,
1370 0 : "'" + toString(filterType) + "' is no valid filter type code.");
1371 : success = false;
1372 : }
1373 :
1374 : if (success) {
1375 : // acknowledge filter addition
1376 770 : writeStatusCmd(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, libsumo::RTYPE_OK, "");
1377 : }
1378 :
1379 : return success;
1380 : }
1381 :
1382 :
1383 : void
1384 0 : TraCIServer::removeFilters() {
1385 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1386 : std::cout << "Removing filters" << std::endl;
1387 : #endif
1388 0 : myLastContextSubscription->activeFilters = libsumo::SUBS_FILTER_NONE;
1389 0 : }
1390 :
1391 : void
1392 90 : TraCIServer::addSubscriptionFilterLanes(std::vector<int> lanes) {
1393 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1394 : std::cout << "Adding lane filter (lanes=" << toString(lanes) << ")" << std::endl;
1395 : #endif
1396 90 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LANES;
1397 90 : myLastContextSubscription->filterLanes = lanes;
1398 90 : }
1399 :
1400 : void
1401 29 : TraCIServer::addSubscriptionFilterNoOpposite() {
1402 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1403 : std::cout << "Adding no opposite filter" << std::endl;
1404 : #endif
1405 29 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_NOOPPOSITE;
1406 29 : }
1407 :
1408 : void
1409 103 : TraCIServer::addSubscriptionFilterDownstreamDistance(double dist) {
1410 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1411 : std::cout << "Adding downstream dist filter (dist=" << toString(dist) << ")" << std::endl;
1412 : #endif
1413 103 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_DOWNSTREAM_DIST;
1414 103 : myLastContextSubscription->filterDownstreamDist = dist;
1415 103 : }
1416 :
1417 : void
1418 87 : TraCIServer::addSubscriptionFilterUpstreamDistance(double dist) {
1419 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1420 : std::cout << "Adding upstream dist filter (dist=" << toString(dist) << ")" << std::endl;
1421 : #endif
1422 87 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_UPSTREAM_DIST;
1423 87 : myLastContextSubscription->filterUpstreamDist = dist;
1424 87 : }
1425 :
1426 : void
1427 13 : TraCIServer::addSubscriptionFilterLeadFollow() {
1428 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1429 : std::cout << "Adding Lead/Follow-maneuver filter" << std::endl;
1430 : #endif
1431 13 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LEAD_FOLLOW;
1432 13 : }
1433 :
1434 : void
1435 17 : TraCIServer::addSubscriptionFilterTurn(double dist) {
1436 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1437 : std::cout << "Adding turn-maneuver filter" << std::endl;
1438 : #endif
1439 17 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_TURN;
1440 17 : myLastContextSubscription->filterFoeDistToJunction = dist;
1441 17 : }
1442 :
1443 : void
1444 7 : TraCIServer::addSubscriptionFilterVClass(SVCPermissions vClasses) {
1445 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1446 : std::cout << "Adding vClass filter (vClasses=" << toString(vClasses) << ")" << std::endl;
1447 : #endif
1448 7 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_VCLASS;
1449 7 : myLastContextSubscription->filterVClasses = vClasses;
1450 7 : }
1451 :
1452 : void
1453 7 : TraCIServer::addSubscriptionFilterVType(std::set<std::string> vTypes) {
1454 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1455 : std::cout << "Adding vType filter (vTypes=" << toString(vTypes) << ")" << std::endl;
1456 : #endif
1457 7 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_VTYPE;
1458 : myLastContextSubscription->filterVTypes = vTypes;
1459 7 : }
1460 :
1461 : void
1462 17 : TraCIServer::addSubscriptionFilterFieldOfVision(double openingAngle) {
1463 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1464 : std::cout << "Adding FieldOfVision filter (openingAngle=" << toString(openingAngle) << ")" << std::endl;
1465 : #endif
1466 17 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_FIELD_OF_VISION;
1467 17 : myLastContextSubscription->filterFieldOfVisionOpeningAngle = openingAngle;
1468 17 : }
1469 :
1470 : void
1471 15 : TraCIServer::addSubscriptionFilterLateralDistance(double dist) {
1472 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1473 : std::cout << "Adding lateral dist filter (dist=" << toString(dist) << ")" << std::endl;
1474 : #endif
1475 15 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LATERAL_DIST;
1476 15 : myLastContextSubscription->filterLateralDist = dist;
1477 15 : }
1478 :
1479 : void
1480 18083999 : TraCIServer::writeResponseWithLength(tcpip::Storage& outputStorage, tcpip::Storage& tempMsg) {
1481 18083999 : if (tempMsg.size() < 254) {
1482 18060236 : outputStorage.writeUnsignedByte(1 + (int)tempMsg.size()); // command length -> short
1483 : } else {
1484 23763 : outputStorage.writeUnsignedByte(0); // command length -> extended
1485 23763 : outputStorage.writeInt(1 + 4 + (int)tempMsg.size());
1486 : }
1487 18083999 : outputStorage.writeStorage(tempMsg);
1488 18083999 : }
1489 :
1490 :
1491 : void
1492 0 : TraCIServer::writePositionVector(tcpip::Storage& outputStorage, const libsumo::TraCIPositionVector& shape) {
1493 0 : outputStorage.writeUnsignedByte(libsumo::TYPE_POLYGON);
1494 0 : if (shape.value.size() < 256) {
1495 0 : outputStorage.writeUnsignedByte((int)shape.value.size());
1496 : } else {
1497 0 : outputStorage.writeUnsignedByte(0);
1498 0 : outputStorage.writeInt((int)shape.value.size());
1499 : }
1500 0 : for (const libsumo::TraCIPosition& pos : shape.value) {
1501 0 : outputStorage.writeDouble(pos.x);
1502 0 : outputStorage.writeDouble(pos.y);
1503 : }
1504 0 : }
1505 :
1506 :
1507 : bool
1508 80617 : TraCIServer::readTypeCheckingInt(tcpip::Storage& inputStorage, int& into) {
1509 80617 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_INTEGER) {
1510 : return false;
1511 : }
1512 80613 : into = inputStorage.readInt();
1513 80613 : return true;
1514 : }
1515 :
1516 :
1517 : bool
1518 225094 : TraCIServer::readTypeCheckingDouble(tcpip::Storage& inputStorage, double& into) {
1519 225094 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_DOUBLE) {
1520 : return false;
1521 : }
1522 225091 : into = inputStorage.readDouble();
1523 225091 : return true;
1524 : }
1525 :
1526 :
1527 : bool
1528 278399 : TraCIServer::readTypeCheckingString(tcpip::Storage& inputStorage, std::string& into) {
1529 278399 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_STRING) {
1530 : return false;
1531 : }
1532 278396 : into = inputStorage.readString();
1533 278396 : return true;
1534 : }
1535 :
1536 :
1537 : bool
1538 409 : TraCIServer::readTypeCheckingStringList(tcpip::Storage& inputStorage, std::vector<std::string>& into) {
1539 409 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_STRINGLIST) {
1540 : return false;
1541 : }
1542 409 : into = inputStorage.readStringList();
1543 409 : return true;
1544 : }
1545 :
1546 :
1547 : bool
1548 210 : TraCIServer::readTypeCheckingDoubleList(tcpip::Storage& inputStorage, std::vector<double>& into) {
1549 210 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_DOUBLELIST) {
1550 : return false;
1551 : }
1552 210 : into = inputStorage.readDoubleList();
1553 210 : return true;
1554 : }
1555 :
1556 :
1557 : bool
1558 324 : TraCIServer::readTypeCheckingColor(tcpip::Storage& inputStorage, libsumo::TraCIColor& into) {
1559 324 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_COLOR) {
1560 : return false;
1561 : }
1562 322 : into.r = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1563 322 : into.g = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1564 322 : into.b = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1565 322 : into.a = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1566 322 : return true;
1567 : }
1568 :
1569 :
1570 : bool
1571 125 : TraCIServer::readTypeCheckingPosition2D(tcpip::Storage& inputStorage, libsumo::TraCIPosition& into) {
1572 125 : if (inputStorage.readUnsignedByte() != libsumo::POSITION_2D) {
1573 : return false;
1574 : }
1575 124 : into.x = inputStorage.readDouble();
1576 124 : into.y = inputStorage.readDouble();
1577 124 : into.z = 0;
1578 124 : return true;
1579 : }
1580 :
1581 :
1582 : bool
1583 101613 : TraCIServer::readTypeCheckingByte(tcpip::Storage& inputStorage, int& into) {
1584 101613 : if (inputStorage.readByte() != libsumo::TYPE_BYTE) {
1585 : return false;
1586 : }
1587 101613 : into = inputStorage.readByte();
1588 101613 : return true;
1589 : }
1590 :
1591 :
1592 : bool
1593 3196 : TraCIServer::readTypeCheckingUnsignedByte(tcpip::Storage& inputStorage, int& into) {
1594 3196 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_UBYTE) {
1595 : return false;
1596 : }
1597 3196 : into = inputStorage.readUnsignedByte();
1598 3196 : return true;
1599 : }
1600 :
1601 :
1602 : bool
1603 105 : TraCIServer::readTypeCheckingPolygon(tcpip::Storage& inputStorage, PositionVector& into) {
1604 105 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_POLYGON) {
1605 : return false;
1606 : }
1607 : into.clear();
1608 104 : int size = inputStorage.readUnsignedByte();
1609 104 : if (size == 0) {
1610 3 : size = inputStorage.readInt();
1611 : }
1612 104 : PositionVector shape;
1613 1347 : for (int i = 0; i < size; ++i) {
1614 1246 : double x = inputStorage.readDouble();
1615 1246 : double y = inputStorage.readDouble();
1616 1246 : if (std::isnan(x) || std::isnan(y)) {
1617 6 : throw libsumo::TraCIException("NaN-Value in shape.");
1618 : }
1619 1243 : into.push_back(Position(x, y));
1620 : }
1621 : return true;
1622 104 : }
1623 :
1624 :
1625 : void
1626 135 : TraCIServer::stateLoaded(SUMOTime targetTime) {
1627 135 : myTargetTime = targetTime;
1628 270 : for (auto& s : mySockets) {
1629 135 : s.second->targetTime = targetTime;
1630 135 : s.second->executeMove = false;
1631 1890 : for (auto& stateChange : s.second->vehicleStateChanges) {
1632 : stateChange.second.clear();
1633 : }
1634 675 : for (auto& stateChange : s.second->transportableStateChanges) {
1635 : stateChange.second.clear();
1636 : }
1637 : }
1638 : mySubscriptions.clear();
1639 135 : mySubscriptionCache.reset();
1640 135 : }
1641 :
1642 :
1643 : bool
1644 0 : TraCIServer::centralObject(const libsumo::Subscription& s, const std::string& objID) {
1645 0 : return (s.id == objID && s.commandId + 32 == s.contextDomain);
1646 : }
1647 :
1648 :
1649 : /****************************************************************************/
|
