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 25831249 : TraCIServer::initWrapper(const int domainID, const int variable, const std::string& objID) {
113 25831249 : myWrapperStorage.reset();
114 25831249 : myWrapperStorage.writeUnsignedByte(domainID);
115 25831249 : myWrapperStorage.writeUnsignedByte(variable);
116 25831249 : myWrapperStorage.writeString(objID);
117 25831249 : }
118 :
119 :
120 : bool
121 8763096 : TraCIServer::wrapDouble(const std::string& /* objID */, const int /* variable */, const double value) {
122 8763096 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLE);
123 8763096 : myWrapperStorage.writeDouble(value);
124 8763096 : return true;
125 : }
126 :
127 :
128 : bool
129 6744607 : TraCIServer::wrapInt(const std::string& /* objID */, const int /* variable */, const int value) {
130 6744607 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_INTEGER);
131 6744607 : myWrapperStorage.writeInt(value);
132 6744607 : return true;
133 : }
134 :
135 :
136 : bool
137 8393199 : TraCIServer::wrapString(const std::string& /* objID */, const int /* variable */, const std::string& value) {
138 8393199 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
139 8393199 : myWrapperStorage.writeString(value);
140 8393199 : return true;
141 : }
142 :
143 :
144 : bool
145 286710 : TraCIServer::wrapStringList(const std::string& /* objID */, const int /* variable */, const std::vector<std::string>& value) {
146 286710 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRINGLIST);
147 286710 : myWrapperStorage.writeStringList(value);
148 286710 : return true;
149 : }
150 :
151 :
152 : bool
153 8 : TraCIServer::wrapDoubleList(const std::string& /* objID */, const int /* variable */, const std::vector<double>& value) {
154 8 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLELIST);
155 8 : myWrapperStorage.writeDoubleList(value);
156 8 : return true;
157 : }
158 :
159 :
160 : bool
161 1361475 : TraCIServer::wrapPosition(const std::string& /* objID */, const int variable, const libsumo::TraCIPosition& value) {
162 : const bool includeZ = variable == libsumo::VAR_POSITION3D;
163 2721351 : myWrapperStorage.writeUnsignedByte(includeZ ? libsumo::POSITION_3D : libsumo::POSITION_2D);
164 1361475 : myWrapperStorage.writeDouble(value.x);
165 1361475 : myWrapperStorage.writeDouble(value.y);
166 1361475 : if (includeZ) {
167 1599 : myWrapperStorage.writeDouble(value.z);
168 : }
169 1361475 : return true;
170 : }
171 :
172 :
173 : bool
174 18142 : TraCIServer::wrapPositionVector(const std::string& /* objID */, const int /* variable */, const libsumo::TraCIPositionVector& shape) {
175 18142 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_POLYGON);
176 18142 : if (shape.value.size() < 256) {
177 18138 : myWrapperStorage.writeUnsignedByte((int)shape.value.size());
178 : } else {
179 4 : myWrapperStorage.writeUnsignedByte(0);
180 4 : myWrapperStorage.writeInt((int)shape.value.size());
181 : }
182 64348 : for (const libsumo::TraCIPosition& pos : shape.value) {
183 46206 : myWrapperStorage.writeDouble(pos.x);
184 46206 : myWrapperStorage.writeDouble(pos.y);
185 : }
186 18142 : return true;
187 : }
188 :
189 :
190 : bool
191 1274 : TraCIServer::wrapColor(const std::string& /* objID */, const int /* variable */, const libsumo::TraCIColor& value) {
192 1274 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COLOR);
193 1274 : myWrapperStorage.writeUnsignedByte(value.r);
194 1274 : myWrapperStorage.writeUnsignedByte(value.g);
195 1274 : myWrapperStorage.writeUnsignedByte(value.b);
196 1274 : myWrapperStorage.writeUnsignedByte(value.a);
197 1274 : return true;
198 : }
199 :
200 :
201 : bool
202 33012 : TraCIServer::wrapStringDoublePair(const std::string& /* objID */, const int /* variable */, const std::pair<std::string, double>& value) {
203 33012 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COMPOUND);
204 33012 : myWrapperStorage.writeInt(2);
205 33012 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
206 33012 : myWrapperStorage.writeString(value.first);
207 33012 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_DOUBLE);
208 33012 : myWrapperStorage.writeDouble(value.second);
209 33012 : return true;
210 : }
211 :
212 :
213 : bool
214 127 : TraCIServer::wrapStringPair(const std::string& /* objID */, const int /* variable */, const std::pair<std::string, std::string>& value) {
215 127 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_COMPOUND);
216 127 : myWrapperStorage.writeInt(2);
217 127 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
218 127 : myWrapperStorage.writeString(value.first);
219 127 : myWrapperStorage.writeUnsignedByte(libsumo::TYPE_STRING);
220 127 : myWrapperStorage.writeString(value.second);
221 127 : return true;
222 : }
223 :
224 :
225 : tcpip::Storage&
226 26664243 : TraCIServer::getWrapperStorage() {
227 26664243 : return myWrapperStorage;
228 : }
229 :
230 :
231 :
232 3141 : TraCIServer::TraCIServer(const SUMOTime begin, const int port, const int numClients)
233 3141 : : 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 3141 : myVehicleStateChanges[MSNet::VehicleState::BUILT] = std::vector<std::string>();
238 3141 : myVehicleStateChanges[MSNet::VehicleState::DEPARTED] = std::vector<std::string>();
239 3141 : myVehicleStateChanges[MSNet::VehicleState::STARTING_TELEPORT] = std::vector<std::string>();
240 3141 : myVehicleStateChanges[MSNet::VehicleState::ENDING_TELEPORT] = std::vector<std::string>();
241 3141 : myVehicleStateChanges[MSNet::VehicleState::ARRIVED] = std::vector<std::string>();
242 3141 : myVehicleStateChanges[MSNet::VehicleState::NEWROUTE] = std::vector<std::string>();
243 3141 : myVehicleStateChanges[MSNet::VehicleState::STARTING_PARKING] = std::vector<std::string>();
244 3141 : myVehicleStateChanges[MSNet::VehicleState::MANEUVERING] = std::vector<std::string>();
245 3141 : myVehicleStateChanges[MSNet::VehicleState::ENDING_PARKING] = std::vector<std::string>();
246 3141 : myVehicleStateChanges[MSNet::VehicleState::STARTING_STOP] = std::vector<std::string>();
247 3141 : myVehicleStateChanges[MSNet::VehicleState::ENDING_STOP] = std::vector<std::string>();
248 3141 : myVehicleStateChanges[MSNet::VehicleState::COLLISION] = std::vector<std::string>();
249 3141 : myVehicleStateChanges[MSNet::VehicleState::EMERGENCYSTOP] = std::vector<std::string>();
250 :
251 3141 : myTransportableStateChanges[MSNet::TransportableState::PERSON_DEPARTED] = std::vector<std::string>();
252 3141 : myTransportableStateChanges[MSNet::TransportableState::PERSON_ARRIVED] = std::vector<std::string>();
253 3141 : myTransportableStateChanges[MSNet::TransportableState::CONTAINER_DEPARTED] = std::vector<std::string>();
254 3141 : myTransportableStateChanges[MSNet::TransportableState::CONTAINER_ARRIVED] = std::vector<std::string>();
255 :
256 3141 : myExecutors[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = &TraCIServerAPI_InductionLoop::processGet;
257 3141 : myExecutors[libsumo::CMD_SET_INDUCTIONLOOP_VARIABLE] = &TraCIServerAPI_InductionLoop::processSet;
258 3141 : myExecutors[libsumo::CMD_GET_LANEAREA_VARIABLE] = &TraCIServerAPI_LaneArea::processGet;
259 3141 : myExecutors[libsumo::CMD_SET_LANEAREA_VARIABLE] = &TraCIServerAPI_LaneArea::processSet;
260 3141 : myExecutors[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = &TraCIServerAPI_MultiEntryExit::processGet;
261 3141 : myExecutors[libsumo::CMD_SET_MULTIENTRYEXIT_VARIABLE] = &TraCIServerAPI_MultiEntryExit::processSet;
262 :
263 3141 : myExecutors[libsumo::CMD_GET_TL_VARIABLE] = &TraCIServerAPI_TrafficLight::processGet;
264 3141 : myExecutors[libsumo::CMD_SET_TL_VARIABLE] = &TraCIServerAPI_TrafficLight::processSet;
265 3141 : myExecutors[libsumo::CMD_GET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processGet;
266 3141 : myExecutors[libsumo::CMD_SET_LANE_VARIABLE] = &TraCIServerAPI_Lane::processSet;
267 3141 : myExecutors[libsumo::CMD_GET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processGet;
268 3141 : myExecutors[libsumo::CMD_SET_VEHICLE_VARIABLE] = &TraCIServerAPI_Vehicle::processSet;
269 3141 : myExecutors[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processGet;
270 3141 : myExecutors[libsumo::CMD_SET_VEHICLETYPE_VARIABLE] = &TraCIServerAPI_VehicleType::processSet;
271 3141 : myExecutors[libsumo::CMD_GET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processGet;
272 3141 : myExecutors[libsumo::CMD_SET_ROUTE_VARIABLE] = &TraCIServerAPI_Route::processSet;
273 3141 : myExecutors[libsumo::CMD_GET_POI_VARIABLE] = &TraCIServerAPI_POI::processGet;
274 3141 : myExecutors[libsumo::CMD_SET_POI_VARIABLE] = &TraCIServerAPI_POI::processSet;
275 3141 : myExecutors[libsumo::CMD_GET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processGet;
276 3141 : myExecutors[libsumo::CMD_SET_POLYGON_VARIABLE] = &TraCIServerAPI_Polygon::processSet;
277 3141 : myExecutors[libsumo::CMD_GET_JUNCTION_VARIABLE] = &TraCIServerAPI_Junction::processGet;
278 3141 : myExecutors[libsumo::CMD_SET_JUNCTION_VARIABLE] = &TraCIServerAPI_Junction::processSet;
279 3141 : myExecutors[libsumo::CMD_GET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processGet;
280 3141 : myExecutors[libsumo::CMD_SET_EDGE_VARIABLE] = &TraCIServerAPI_Edge::processSet;
281 3141 : myExecutors[libsumo::CMD_GET_SIM_VARIABLE] = &TraCIServerAPI_Simulation::processGet;
282 3141 : myExecutors[libsumo::CMD_SET_SIM_VARIABLE] = &TraCIServerAPI_Simulation::processSet;
283 3141 : myExecutors[libsumo::CMD_GET_PERSON_VARIABLE] = &TraCIServerAPI_Person::processGet;
284 3141 : myExecutors[libsumo::CMD_SET_PERSON_VARIABLE] = &TraCIServerAPI_Person::processSet;
285 3141 : myExecutors[libsumo::CMD_GET_CALIBRATOR_VARIABLE] = &TraCIServerAPI_Calibrator::processGet;
286 3141 : myExecutors[libsumo::CMD_SET_CALIBRATOR_VARIABLE] = &TraCIServerAPI_Calibrator::processSet;
287 3141 : myExecutors[libsumo::CMD_GET_BUSSTOP_VARIABLE] = &TraCIServerAPI_BusStop::processGet;
288 3141 : myExecutors[libsumo::CMD_SET_BUSSTOP_VARIABLE] = &TraCIServerAPI_BusStop::processSet;
289 3141 : myExecutors[libsumo::CMD_GET_PARKINGAREA_VARIABLE] = &TraCIServerAPI_ParkingArea::processGet;
290 3141 : myExecutors[libsumo::CMD_SET_PARKINGAREA_VARIABLE] = &TraCIServerAPI_ParkingArea::processSet;
291 3141 : myExecutors[libsumo::CMD_GET_CHARGINGSTATION_VARIABLE] = &TraCIServerAPI_ChargingStation::processGet;
292 3141 : myExecutors[libsumo::CMD_SET_CHARGINGSTATION_VARIABLE] = &TraCIServerAPI_ChargingStation::processSet;
293 3141 : myExecutors[libsumo::CMD_GET_ROUTEPROBE_VARIABLE] = &TraCIServerAPI_RouteProbe::processGet;
294 3141 : myExecutors[libsumo::CMD_SET_ROUTEPROBE_VARIABLE] = &TraCIServerAPI_RouteProbe::processSet;
295 3141 : myExecutors[libsumo::CMD_GET_REROUTER_VARIABLE] = &TraCIServerAPI_Rerouter::processGet;
296 3141 : myExecutors[libsumo::CMD_SET_REROUTER_VARIABLE] = &TraCIServerAPI_Rerouter::processSet;
297 3141 : myExecutors[libsumo::CMD_GET_VARIABLESPEEDSIGN_VARIABLE] = &TraCIServerAPI_VariableSpeedSign::processGet;
298 3141 : myExecutors[libsumo::CMD_SET_VARIABLESPEEDSIGN_VARIABLE] = &TraCIServerAPI_VariableSpeedSign::processSet;
299 3141 : myExecutors[libsumo::CMD_GET_MEANDATA_VARIABLE] = &TraCIServerAPI_MeanData::processGet;
300 : //myExecutors[libsumo::CMD_SET_MEANDATA_VARIABLE] = &TraCIServerAPI_MeanData::processSet;
301 3141 : myExecutors[libsumo::CMD_GET_OVERHEADWIRE_VARIABLE] = &TraCIServerAPI_OverheadWire::processGet;
302 3141 : myExecutors[libsumo::CMD_SET_OVERHEADWIRE_VARIABLE] = &TraCIServerAPI_OverheadWire::processSet;
303 :
304 3141 : myParameterized.insert(std::make_pair(libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE, libsumo::VAR_LEADER));
305 3141 : myParameterized.insert(std::make_pair(libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE, libsumo::VAR_FOLLOWER));
306 3141 : myParameterized.insert(std::make_pair(0, libsumo::VAR_PARAMETER));
307 3141 : myParameterized.insert(std::make_pair(0, libsumo::VAR_PARAMETER_WITH_KEY));
308 :
309 3141 : 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 3141 : if (!MSGlobals::gUsingInternalLanes && !MSGlobals::gUseMesoSim) {
314 4 : WRITE_WARNING(TL("Starting TraCI without using internal lanes!"));
315 4 : MsgHandler::getWarningInstance()->inform("Vehicles will jump over junctions.", false);
316 20 : MsgHandler::getWarningInstance()->inform("Use without option --no-internal-links to avoid unexpected behavior", false);
317 : }
318 :
319 : try {
320 6282 : WRITE_MESSAGEF(TL("***Starting server on port % ***"), toString(port));
321 3141 : tcpip::Socket serverSocket(port);
322 3141 : if (numClients > 1) {
323 236 : WRITE_MESSAGEF(TL(" waiting for % clients..."), toString(numClients));
324 : }
325 6526 : while ((int)mySockets.size() < numClients) {
326 3391 : int index = (int)mySockets.size() + libsumo::MAX_ORDER + 1;
327 3391 : mySockets[index] = new SocketInfo(serverSocket.accept(true), begin);
328 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::BUILT] = std::vector<std::string>();
329 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::DEPARTED] = std::vector<std::string>();
330 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_TELEPORT] = std::vector<std::string>();
331 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_TELEPORT] = std::vector<std::string>();
332 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ARRIVED] = std::vector<std::string>();
333 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::NEWROUTE] = std::vector<std::string>();
334 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_PARKING] = std::vector<std::string>();
335 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::MANEUVERING] = std::vector<std::string>();
336 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_PARKING] = std::vector<std::string>();
337 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::STARTING_STOP] = std::vector<std::string>();
338 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::ENDING_STOP] = std::vector<std::string>();
339 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::COLLISION] = std::vector<std::string>();
340 3385 : mySockets[index]->vehicleStateChanges[MSNet::VehicleState::EMERGENCYSTOP] = std::vector<std::string>();
341 :
342 3385 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::PERSON_DEPARTED] = std::vector<std::string>();
343 3385 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::PERSON_ARRIVED] = std::vector<std::string>();
344 3385 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::CONTAINER_DEPARTED] = std::vector<std::string>();
345 3385 : mySockets[index]->transportableStateChanges[MSNet::TransportableState::CONTAINER_ARRIVED] = std::vector<std::string>();
346 3385 : if (numClients > 1) {
347 736 : WRITE_MESSAGE(TL(" client connected"));
348 : }
349 : }
350 : // When got here, all clients have connected
351 3135 : if (numClients > 1) {
352 118 : checkClientOrdering();
353 : }
354 : // set myCurrentSocket != mySockets.end() to indicate that this is the first step in processCommands()
355 3129 : myCurrentSocket = mySockets.begin();
356 3153 : } catch (tcpip::SocketException& e) {
357 12 : throw ProcessError(e.what());
358 6 : }
359 3201 : }
360 :
361 :
362 6258 : TraCIServer::~TraCIServer() {
363 3165 : for (const auto& socket : mySockets) {
364 36 : 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 9387 : }
368 :
369 :
370 : // ---------- Initialisation and Shutdown
371 : void
372 35485 : TraCIServer::openSocket(const std::map<int, CmdExecutor>& execs) {
373 103280 : if (myInstance == nullptr && !myDoCloseConnection && (OptionsCont::getOptions().getInt("remote-port") != 0)) {
374 9411 : myInstance = new TraCIServer(string2time(OptionsCont::getOptions().getString("begin")),
375 6282 : OptionsCont::getOptions().getInt("remote-port"),
376 6282 : OptionsCont::getOptions().getInt("num-clients"));
377 3415 : for (std::map<int, CmdExecutor>::const_iterator i = execs.begin(); i != execs.end(); ++i) {
378 286 : myInstance->myExecutors[i->first] = i->second;
379 : }
380 : }
381 35473 : if (myInstance != nullptr) {
382 : // maybe net was deleted and built again
383 3146 : MSNet::getInstance()->addVehicleStateListener(myInstance);
384 3146 : MSNet::getInstance()->addTransportableStateListener(myInstance);
385 3146 : myInstance->mySubscriptionCache.writeInt(0);
386 : }
387 35473 : }
388 :
389 :
390 : void
391 35331 : TraCIServer::close() {
392 35331 : if (myInstance == nullptr) {
393 : return;
394 : }
395 3129 : delete myInstance;
396 3129 : myInstance = nullptr;
397 3129 : myDoCloseConnection = true;
398 : }
399 :
400 :
401 : bool
402 82572022 : TraCIServer::wasClosed() {
403 82572022 : return myDoCloseConnection;
404 : }
405 :
406 :
407 : // ---------- Initialisation and Shutdown
408 :
409 :
410 : void
411 148820 : TraCIServer::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
412 148820 : if (!myDoCloseConnection) {
413 148820 : myVehicleStateChanges[to].push_back(vehicle->getID());
414 315252 : for (std::map<int, SocketInfo*>::iterator i = mySockets.begin(); i != mySockets.end(); ++i) {
415 166432 : i->second->vehicleStateChanges[to].push_back(vehicle->getID());
416 : }
417 : }
418 148820 : }
419 :
420 :
421 : void
422 2517 : TraCIServer::transportableStateChanged(const MSTransportable* const transportable, MSNet::TransportableState to, const std::string& /*info*/) {
423 2517 : if (!myDoCloseConnection) {
424 2517 : myTransportableStateChanges[to].push_back(transportable->getID());
425 5034 : for (std::map<int, SocketInfo*>::iterator i = mySockets.begin(); i != mySockets.end(); ++i) {
426 2517 : i->second->transportableStateChanges[to].push_back(transportable->getID());
427 : }
428 : }
429 2517 : }
430 :
431 :
432 : void
433 118 : 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 118 : myCurrentSocket = mySockets.begin();
440 477 : 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 726 : myInputStorage.reset();
447 726 : myCurrentSocket->second->socket->receiveExact(myInputStorage);
448 : int commandStart, commandLength;
449 726 : 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 726 : bool initCommand = commandId == libsumo::CMD_SETORDER || commandId == libsumo::CMD_GETVERSION;
456 726 : 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 720 : tcpip::Storage tmp;
464 720 : tmp.writeStorage(myInputStorage);
465 720 : 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 1081 : myInputStorage.writeUnsignedByte(commandId == libsumo::CMD_SETORDER ? 6 : 2);
469 720 : myInputStorage.writeUnsignedByte(commandId);
470 720 : myInputStorage.writeStorage(tmp);
471 :
472 : // Handle initialization command completely
473 720 : dispatchCommand();
474 720 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
475 720 : myOutputStorage.reset();
476 720 : } else {
477 : #ifdef DEBUG_MULTI_CLIENTS
478 : std::cout << " Client " << myCurrentSocket->second->socket << " did not set order initially." << std::endl;
479 : #endif
480 12 : throw ProcessError(TL("Execution order (libsumo::CMD_SETORDER) was not set for all TraCI clients in pre-execution phase."));
481 : }
482 720 : if (commandId == libsumo::CMD_SETORDER) {
483 : // This is what we have waited for.
484 : break;
485 : }
486 361 : }
487 : ++myCurrentSocket;
488 : }
489 112 : }
490 :
491 :
492 : void
493 7071164 : TraCIServer::processReorderingRequests() {
494 : // Process reordering requests
495 7071164 : 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 484 : while (i != mySocketReorderRequests.end()) {
511 : j = mySockets.begin();
512 849 : while (j != mySockets.end()) {
513 849 : 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 364 : 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 7071164 : }
534 :
535 :
536 : SUMOTime
537 14079768 : 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 28326694 : 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 14246926 : targetTime = MIN2(targetTime, i->second->targetTime);
551 : }
552 : #ifdef DEBUG_MULTI_CLIENTS
553 : std::cout << std::endl;
554 : #endif
555 14079768 : 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 7068019 : 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 14190848 : while (i != mySockets.end()) {
567 7122829 : if (i->second->targetTime <= MSNet::getInstance()->getCurrentTimeStep()) {
568 : // this client will become active before the next SUMO step. Provide subscription results.
569 7014439 : 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 7068019 : }
580 :
581 :
582 : int
583 7071164 : 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 7071164 : processReorderingRequests();
592 7071164 : 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 7068019 : 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 7068019 : sendOutputToAll();
598 7068019 : myOutputStorage.reset();
599 : }
600 :
601 : // determine minimal next target time among clients
602 7071164 : myTargetTime = nextTargetTime();
603 :
604 7071164 : 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 13991766 : 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 7011740 : myCurrentSocket = mySockets.begin();
622 14135802 : 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 7124088 : 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 77940 : continue;
637 : }
638 : finalCmd = 0;
639 23307701 : while (finalCmd == 0) {
640 16261579 : if (!myInputStorage.valid_pos()) {
641 : // have read request completely, send response if adequate
642 16261579 : if (myOutputStorage.size() > 0) {
643 : // send response to previous query
644 9215439 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
645 9215439 : 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 16261579 : myInputStorage.reset();
652 16261579 : myCurrentSocket->second->socket->receiveExact(myInputStorage);
653 : }
654 :
655 32523106 : while (myInputStorage.valid_pos() && !myDoCloseConnection) {
656 16261553 : const int cmd = dispatchCommand();
657 16261553 : 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 7011714 : if (!myLoadArgs.empty()) {
664 : #ifdef DEBUG_MULTI_CLIENTS
665 : std::cout << " Breaking loop to load new simulation." << std::endl;
666 : #endif
667 : break;
668 7011697 : } 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 7008604 : 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 7008604 : myTargetTime = nextT;
682 : }
683 : // All clients are done with the current time step
684 : // Reset myVehicleStateChanges and myTransportableStateChanges
685 97763904 : for (auto& item : myVehicleStateChanges) {
686 : item.second.clear();
687 : }
688 34915680 : for (auto& item : myTransportableStateChanges) {
689 : item.second.clear();
690 : }
691 : return finalCmd;
692 26 : } catch (std::invalid_argument& e) {
693 0 : throw ProcessError(e.what());
694 0 : } catch (libsumo::TraCIException& e) {
695 0 : throw ProcessError(e.what());
696 26 : } catch (tcpip::SocketException& e) {
697 52 : throw ProcessError(e.what());
698 26 : }
699 : }
700 :
701 :
702 : void
703 3138 : TraCIServer::cleanup() {
704 : mySubscriptions.clear();
705 3138 : myTargetTime = string2time(OptionsCont::getOptions().getString("begin"));
706 3183 : for (myCurrentSocket = mySockets.begin(); myCurrentSocket != mySockets.end(); ++myCurrentSocket) {
707 45 : myCurrentSocket->second->targetTime = myTargetTime;
708 45 : myCurrentSocket->second->executeMove = false;
709 : }
710 3138 : myOutputStorage.reset();
711 3138 : myInputStorage.reset();
712 3138 : mySubscriptionCache.reset();
713 43932 : for (auto& i : myVehicleStateChanges) {
714 : i.second.clear();
715 : }
716 15690 : for (auto& i : myTransportableStateChanges) {
717 : i.second.clear();
718 : }
719 3138 : myCurrentSocket = mySockets.begin();
720 3138 : }
721 :
722 :
723 : std::map<int, TraCIServer::SocketInfo*>::iterator
724 3337 : TraCIServer::removeCurrentSocket() {
725 : #ifdef DEBUG_MULTI_CLIENTS
726 : std::cout << " Removing socket " << myCurrentSocket->second->socket
727 : << " (order " << myCurrentSocket->first << ")" << std::endl;
728 : #endif
729 3337 : delete myCurrentSocket->second;
730 3337 : myCurrentSocket = mySockets.erase(myCurrentSocket);
731 3337 : return myCurrentSocket;
732 : }
733 :
734 :
735 : int
736 16262999 : TraCIServer::readCommandID(int& commandStart, int& commandLength) {
737 16262999 : commandStart = myInputStorage.position();
738 16262999 : commandLength = myInputStorage.readUnsignedByte();
739 16262999 : if (commandLength == 0) {
740 95871 : 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 16262999 : return myInputStorage.readUnsignedByte();
750 : }
751 :
752 :
753 : int
754 16262273 : TraCIServer::dispatchCommand() {
755 : int commandStart, commandLength;
756 16262273 : 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 16262273 : if (myExecutors.find(commandId) != myExecutors.end()) {
764 9202823 : success = myExecutors[commandId](*this, myInputStorage, myOutputStorage);
765 : } else {
766 7059450 : switch (commandId) {
767 2769 : case libsumo::CMD_GETVERSION:
768 2769 : success = commandGetVersion();
769 : break;
770 : case libsumo::CMD_LOAD: {
771 : std::vector<std::string> args;
772 17 : 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 17 : myLoadArgs = args;
781 : success = true;
782 34 : 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 17 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
786 17 : myCurrentSocket = mySockets.end();
787 17 : myOutputStorage.reset();
788 0 : } catch (libsumo::TraCIException& e) {
789 0 : return writeErrorStatusCmd(libsumo::CMD_LOAD, e.what(), myOutputStorage);
790 0 : }
791 : break;
792 17 : }
793 8 : case libsumo::CMD_EXECUTEMOVE:
794 8 : myCurrentSocket->second->executeMove = true;
795 : myCurrentSocket++;
796 : success = true;
797 16 : writeStatusCmd(libsumo::CMD_EXECUTEMOVE, libsumo::RTYPE_OK, "");
798 : break;
799 7042760 : case libsumo::CMD_SIMSTEP: {
800 7042760 : const double nextT = myInputStorage.readDouble();
801 7042760 : if (nextT == 0.) {
802 6973941 : myCurrentSocket->second->targetTime += DELTA_T;
803 : } else {
804 68819 : myCurrentSocket->second->targetTime = TIME2STEPS(nextT);
805 : }
806 7042760 : 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 7042760 : 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 28320 : 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 98598640 : for (auto& item : myCurrentSocket->second->vehicleStateChanges) {
821 : item.second.clear();
822 : }
823 35213800 : for (auto& item : myCurrentSocket->second->transportableStateChanges) {
824 : item.second.clear();
825 : }
826 : myCurrentSocket++;
827 7042760 : return commandId;
828 : }
829 : case libsumo::CMD_CLOSE:
830 6674 : writeStatusCmd(libsumo::CMD_CLOSE, libsumo::RTYPE_OK, "");
831 3337 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
832 3337 : myOutputStorage.reset();
833 3337 : if (mySockets.size() == 1) {
834 : // Last client has closed connection
835 3093 : myDoCloseConnection = true;
836 : }
837 : // remove current socket and increment to next socket in ordering
838 3337 : myCurrentSocket = removeCurrentSocket();
839 : success = true;
840 : break;
841 369 : case libsumo::CMD_SETORDER: {
842 369 : 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 369 : 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 369 : mySocketReorderRequests[order] = myCurrentSocket->second;
855 : success = true;
856 369 : writeStatusCmd(libsumo::CMD_SETORDER, libsumo::RTYPE_OK, "");
857 369 : break;
858 : }
859 6721 : 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 6721 : success = addObjectVariableSubscription(commandId, false);
884 : break;
885 2818 : 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 2818 : success = addObjectVariableSubscription(commandId, true);
910 : break;
911 573 : case libsumo::CMD_ADD_SUBSCRIPTION_FILTER:
912 573 : success = addSubscriptionFilter();
913 : break;
914 78 : default:
915 78 : if (commandId == libsumo::CMD_GET_GUI_VARIABLE || commandId == libsumo::CMD_SET_GUI_VARIABLE) {
916 154 : 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 9215704 : if (!success) {
923 2185 : while (myInputStorage.valid_pos() && (int)myInputStorage.position() < commandStart + commandLength) {
924 1085 : myInputStorage.readChar();
925 : }
926 : }
927 9219513 : 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 9219513 : return commandId;
936 : }
937 :
938 :
939 : // ---------- Server-internal command handling
940 : bool
941 2769 : TraCIServer::commandGetVersion() {
942 : // Prepare response
943 2769 : tcpip::Storage answerTmp;
944 2769 : answerTmp.writeInt(libsumo::TRACI_VERSION);
945 2769 : answerTmp.writeString("SUMO " VERSION_STRING);
946 : // When we get here, the response is stored in answerTmp -> put into myOutputStorage
947 2769 : writeStatusCmd(libsumo::CMD_GETVERSION, libsumo::RTYPE_OK, "");
948 : // command length
949 2769 : myOutputStorage.writeUnsignedByte(1 + 1 + static_cast<int>(answerTmp.size()));
950 : // command type
951 2769 : myOutputStorage.writeUnsignedByte(libsumo::CMD_GETVERSION);
952 : // and the parameter dependant part
953 2769 : myOutputStorage.writeStorage(answerTmp);
954 2769 : return true;
955 2769 : }
956 :
957 :
958 : void
959 7068019 : TraCIServer::postProcessSimulationStep() {
960 7068019 : SUMOTime t = MSNet::getInstance()->getCurrentTimeStep();
961 : #ifdef DEBUG_MULTI_CLIENTS
962 : std::cout << " postProcessSimulationStep() at time=" << t << std::endl;
963 : #endif
964 14136038 : writeStatusCmd(libsumo::CMD_SIMSTEP, libsumo::RTYPE_OK, "");
965 : int noActive = 0;
966 15439882 : for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
967 : const libsumo::Subscription& s = *i;
968 8371863 : bool isArrivedVehicle = (s.commandId == libsumo::CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == libsumo::CMD_SUBSCRIBE_VEHICLE_CONTEXT)
969 8371863 : && (find(myVehicleStateChanges[MSNet::VehicleState::ARRIVED].begin(), myVehicleStateChanges[MSNet::VehicleState::ARRIVED].end(), s.id) != myVehicleStateChanges[MSNet::VehicleState::ARRIVED].end());
970 :
971 8371863 : bool isArrivedPerson = (s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_VARIABLE || s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT) && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
972 8371863 : if ((s.endTime < t) || isArrivedVehicle || isArrivedPerson) {
973 4020 : i = mySubscriptions.erase(i);
974 4020 : continue;
975 : }
976 : ++i;
977 8367843 : if (s.beginTime > t) {
978 4 : continue;
979 : }
980 8367839 : ++noActive;
981 : }
982 7068019 : 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 7068019 : mySubscriptionCache.writeInt(noActive);
988 : #ifdef DEBUG_SUBSCRIPTIONS
989 : std::cout << " Size after writing an int is " << mySubscriptionCache.size() << std::endl;
990 : #endif
991 15435862 : for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
992 : const libsumo::Subscription& s = *i;
993 8367843 : if (s.beginTime > t) {
994 : ++i;
995 4 : continue;
996 : }
997 8367839 : tcpip::Storage into;
998 : std::string errors;
999 8367839 : 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 8367839 : mySubscriptionCache.writeStorage(into);
1005 8367839 : if (ok) {
1006 : ++i;
1007 : } else {
1008 0 : i = mySubscriptions.erase(i);
1009 : }
1010 8367839 : }
1011 7068019 : myOutputStorage.writeStorage(mySubscriptionCache);
1012 : #ifdef DEBUG_SUBSCRIPTIONS
1013 : std::cout << " Size after writing subscriptions is " << mySubscriptionCache.size() << std::endl;
1014 : #endif
1015 7068019 : }
1016 :
1017 :
1018 : void
1019 28320 : 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 28320 : 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 28320 : myOutputStorage.writeStorage(mySubscriptionCache);
1033 : // send results to active client
1034 28320 : myCurrentSocket->second->socket->sendExact(myOutputStorage);
1035 28320 : myOutputStorage.reset();
1036 28320 : }
1037 :
1038 :
1039 : void
1040 7113034 : TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description) {
1041 7113034 : writeStatusCmd(commandId, status, description, myOutputStorage);
1042 7113034 : }
1043 :
1044 :
1045 : void
1046 33148940 : TraCIServer::writeStatusCmd(int commandId, int status, const std::string& description, tcpip::Storage& outputStorage) {
1047 33148940 : if (status == libsumo::RTYPE_ERR) {
1048 3442 : WRITE_ERROR("Answered with error to command " + toHex(commandId, 2) + ": " + description);
1049 33147219 : } else if (status == libsumo::RTYPE_NOTIMPLEMENTED) {
1050 156 : WRITE_ERROR("Requested command not implemented (" + toHex(commandId, 2) + "): " + description);
1051 : }
1052 33148940 : outputStorage.writeUnsignedByte(1 + 1 + 1 + 4 + static_cast<int>(description.length())); // command length
1053 33148940 : outputStorage.writeUnsignedByte(commandId); // command type
1054 33148940 : outputStorage.writeUnsignedByte(status); // status
1055 33148940 : outputStorage.writeString(description); // description
1056 33148940 : }
1057 :
1058 :
1059 : bool
1060 1017 : TraCIServer::writeErrorStatusCmd(int commandId, const std::string& description, tcpip::Storage& outputStorage) {
1061 1017 : writeStatusCmd(commandId, libsumo::RTYPE_ERR, description, outputStorage);
1062 1017 : return false;
1063 : }
1064 :
1065 :
1066 : void
1067 8527 : TraCIServer::initialiseSubscription(libsumo::Subscription& s) {
1068 8527 : tcpip::Storage writeInto;
1069 : std::string errors;
1070 8527 : libsumo::Subscription* modifiedSubscription = nullptr;
1071 : try {
1072 8527 : if (processSingleSubscription(s, writeInto, errors)) {
1073 7831 : if (s.endTime < MSNet::getInstance()->getCurrentTimeStep()) {
1074 0 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, "Subscription has ended.");
1075 : } else {
1076 7831 : if (libsumo::Helper::needNewSubscription(s, mySubscriptions, modifiedSubscription)) {
1077 : // Add new subscription to subscription cache (note: seems a bit inefficient)
1078 6711 : if (s.beginTime < MSNet::getInstance()->getCurrentTimeStep()) {
1079 : // copy new subscription into cache
1080 6678 : int noActive = 1 + (mySubscriptionCache.size() > 0 ? mySubscriptionCache.readInt() : 0);
1081 6678 : tcpip::Storage tmp;
1082 6678 : tmp.writeInt(noActive);
1083 56910608 : while (mySubscriptionCache.valid_pos()) {
1084 56903930 : tmp.writeByte(mySubscriptionCache.readByte());
1085 : }
1086 6678 : tmp.writeStorage(writeInto);
1087 6678 : mySubscriptionCache.reset();
1088 6678 : mySubscriptionCache.writeStorage(tmp);
1089 6678 : }
1090 : }
1091 15662 : writeStatusCmd(s.commandId, libsumo::RTYPE_OK, "");
1092 : }
1093 7831 : if (modifiedSubscription != nullptr && (
1094 : modifiedSubscription->isVehicleToVehicleContextSubscription()
1095 : || modifiedSubscription->isVehicleToPersonContextSubscription())) {
1096 : // Set last modified vehicle context subscription active for filter modifications
1097 213 : myLastContextSubscription = modifiedSubscription;
1098 : } else {
1099 : // adding other subscriptions deactivates the activation for filter addition
1100 7618 : myLastContextSubscription = nullptr;
1101 : }
1102 : } else {
1103 6 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, "Could not add subscription. " + errors);
1104 : }
1105 693 : } catch (libsumo::TraCIException& e) {
1106 693 : writeStatusCmd(s.commandId, libsumo::RTYPE_ERR, e.what());
1107 693 : }
1108 8527 : myOutputStorage.writeStorage(writeInto);
1109 8527 : }
1110 :
1111 :
1112 : void
1113 1012 : TraCIServer::removeSubscription(int commandId, const std::string& id, int domain) {
1114 : bool found = false;
1115 : std::vector<libsumo::Subscription>::iterator j;
1116 2069 : for (j = mySubscriptions.begin(); j != mySubscriptions.end();) {
1117 1057 : if (j->id == id && j->commandId == commandId && j->contextDomain == domain) {
1118 1012 : j = mySubscriptions.erase(j);
1119 1012 : if (j != mySubscriptions.end() && myLastContextSubscription == &(*j)) {
1120 : // Remove also reference for filter additions
1121 0 : myLastContextSubscription = nullptr;
1122 : }
1123 : found = true;
1124 1012 : continue;
1125 : }
1126 : ++j;
1127 : }
1128 : // try unsubscribe
1129 1012 : if (found) {
1130 2024 : writeStatusCmd(commandId, libsumo::RTYPE_OK, "");
1131 : } else {
1132 0 : writeStatusCmd(commandId, libsumo::RTYPE_ERR, "The subscription to remove was not found.");
1133 : }
1134 1012 : }
1135 :
1136 :
1137 : bool
1138 8376366 : TraCIServer::processSingleSubscription(const libsumo::Subscription& s, tcpip::Storage& writeInto,
1139 : std::string& errors) {
1140 : bool ok = true;
1141 8376366 : tcpip::Storage outputStorage;
1142 8376366 : const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
1143 : std::set<std::string> objIDs;
1144 8376366 : if (s.contextDomain > 0) {
1145 89641 : if ((s.activeFilters & libsumo::SUBS_FILTER_NO_RTREE) == 0) {
1146 87087 : PositionVector shape;
1147 87087 : libsumo::Helper::findObjectShape(s.commandId, s.id, shape);
1148 87087 : libsumo::Helper::collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
1149 87087 : }
1150 88948 : libsumo::Helper::applySubscriptionFilters(s, objIDs);
1151 : } else {
1152 8286725 : objIDs.insert(s.id);
1153 : }
1154 8375673 : 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 16947423 : for (std::set<std::string>::iterator j = objIDs.begin(); j != objIDs.end(); ++j) {
1157 8571750 : if (s.contextDomain > 0) {
1158 : //if (centralObject(s, *j)) {
1159 : // skipped++;
1160 : // continue;
1161 : //}
1162 285025 : outputStorage.writeString(*j);
1163 : }
1164 8571750 : if (numVars > 0) {
1165 : std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
1166 25398496 : for (std::vector<int>::const_iterator i = s.variables.begin(); i != s.variables.end(); ++i, ++k) {
1167 16833088 : tcpip::Storage message;
1168 16833088 : message.writeUnsignedByte(*i);
1169 16833088 : message.writeString(*j);
1170 : // TODO check why writeStorage fails here (probably some kind of invalid iterator)
1171 16957666 : for (const auto& v :** k) {
1172 124578 : message.writeChar(v);
1173 : }
1174 16833088 : tcpip::Storage tmpOutput;
1175 16833088 : if (myExecutors.find(getCommandId) != myExecutors.end()) {
1176 16833088 : 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 16833088 : if (ok) {
1183 16833085 : int length = tmpOutput.readUnsignedByte();
1184 117831595 : while (--length > 0) {
1185 100998510 : tmpOutput.readUnsignedByte();
1186 : }
1187 : int lengthLength = 1;
1188 16833085 : length = tmpOutput.readUnsignedByte();
1189 16833085 : if (length == 0) {
1190 : lengthLength = 5;
1191 24 : length = tmpOutput.readInt();
1192 : }
1193 : //read responseType
1194 16833085 : tmpOutput.readUnsignedByte();
1195 16833085 : int variable = tmpOutput.readUnsignedByte();
1196 16833085 : std::string id = tmpOutput.readString();
1197 16833085 : outputStorage.writeUnsignedByte(variable);
1198 16833085 : outputStorage.writeUnsignedByte(libsumo::RTYPE_OK);
1199 16833085 : length -= (lengthLength + 1 + 4 + (int)id.length());
1200 212664044 : while (--length > 0) {
1201 195830959 : 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 16833088 : }
1218 : }
1219 : }
1220 8375673 : int length = (1 + 4) + 1 + (4 + (int)s.id.length()) + 1 + (int)outputStorage.size();
1221 8375673 : if (s.contextDomain > 0) {
1222 88948 : length += 1 + 4; // context domain and number of objects
1223 : }
1224 : // we always write extended command length here for backward compatibility
1225 8375673 : writeInto.writeUnsignedByte(0); // command length -> extended
1226 8375673 : writeInto.writeInt(length);
1227 8375673 : writeInto.writeUnsignedByte(s.commandId + 0x10);
1228 8375673 : writeInto.writeString(s.id);
1229 8375673 : if (s.contextDomain > 0) {
1230 88948 : writeInto.writeUnsignedByte(s.contextDomain);
1231 : }
1232 8375673 : writeInto.writeUnsignedByte(numVars);
1233 8375673 : if (s.contextDomain > 0) {
1234 88948 : writeInto.writeInt((int)objIDs.size() - skipped);
1235 : }
1236 8375673 : if (s.contextDomain == 0 || objIDs.size() != 0) {
1237 8343828 : writeInto.writeStorage(outputStorage);
1238 : }
1239 8375673 : return ok;
1240 8376366 : }
1241 :
1242 :
1243 : bool
1244 9539 : TraCIServer::addObjectVariableSubscription(const int commandId, const bool hasContext) {
1245 9539 : const double beginTime = myInputStorage.readDouble();
1246 9539 : const double endTime = myInputStorage.readDouble();
1247 9539 : const SUMOTime begin = beginTime == libsumo::INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
1248 9539 : const SUMOTime end = endTime == libsumo::INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
1249 9539 : const std::string id = myInputStorage.readString();
1250 9539 : const int domain = hasContext ? myInputStorage.readUnsignedByte() : 0;
1251 9539 : double range = hasContext ? myInputStorage.readDouble() : 0.;
1252 9539 : if (commandId == libsumo::CMD_SUBSCRIBE_SIM_CONTEXT) {
1253 : range = std::numeric_limits<double>::max();
1254 : }
1255 9539 : const int num = myInputStorage.readUnsignedByte();
1256 : std::vector<int> variables;
1257 : std::vector<std::shared_ptr<tcpip::Storage> > parameters;
1258 23563 : for (int i = 0; i < num; ++i) {
1259 14024 : const int varID = myInputStorage.readUnsignedByte();
1260 14024 : variables.push_back(varID);
1261 14024 : parameters.push_back(std::make_shared<tcpip::Storage>());
1262 14024 : if ((myParameterized.count(std::make_pair(0, varID)) > 0) || (myParameterized.count(std::make_pair(commandId, varID)) > 0)) {
1263 80 : const int parType = myInputStorage.readUnsignedByte();
1264 80 : parameters.back()->writeUnsignedByte(parType);
1265 80 : if (parType == libsumo::TYPE_DOUBLE) {
1266 52 : parameters.back()->writeDouble(myInputStorage.readDouble());
1267 28 : } else if (parType == libsumo::TYPE_STRING) {
1268 56 : parameters.back()->writeString(myInputStorage.readString());
1269 : } else {
1270 : // Error!
1271 : }
1272 : }
1273 : }
1274 : // check subscribe/unsubscribe
1275 9539 : if (variables.empty()) {
1276 1012 : removeSubscription(commandId, id, domain);
1277 : return true;
1278 : }
1279 : // process subscription
1280 8527 : libsumo::Subscription s(commandId, id, variables, parameters, begin, end, domain, range);
1281 8527 : initialiseSubscription(s);
1282 : return true;
1283 9539 : }
1284 :
1285 :
1286 :
1287 : bool
1288 573 : TraCIServer::addSubscriptionFilter() {
1289 : bool success = true;
1290 : // Read filter type
1291 573 : int filterType = myInputStorage.readUnsignedByte();
1292 :
1293 573 : if (myLastContextSubscription == nullptr) {
1294 3 : writeStatusCmd(libsumo::CMD_ADD_SUBSCRIPTION_FILTER, libsumo::RTYPE_ERR,
1295 6 : "No previous vehicle context subscription exists to apply filter type " + toHex(filterType, 2));
1296 3 : return false;
1297 : }
1298 :
1299 : // dispatch according to filter type
1300 570 : switch (filterType) {
1301 0 : case libsumo::FILTER_TYPE_NONE:
1302 : // Remove all filters
1303 0 : removeFilters();
1304 : break;
1305 133 : case libsumo::FILTER_TYPE_LANES: {
1306 : // Read relative lanes to consider for context filter
1307 133 : int nrLanes = (int)myInputStorage.readByte();
1308 : std::vector<int> lanes;
1309 358 : for (int i = 0; i < nrLanes; ++i) {
1310 225 : lanes.push_back((int) myInputStorage.readByte());
1311 : }
1312 266 : addSubscriptionFilterLanes(lanes);
1313 : }
1314 : break;
1315 43 : case libsumo::FILTER_TYPE_NOOPPOSITE:
1316 : // Add no-opposite filter
1317 43 : addSubscriptionFilterNoOpposite();
1318 : break;
1319 154 : case libsumo::FILTER_TYPE_DOWNSTREAM_DIST: {
1320 154 : myInputStorage.readByte(); // read type double
1321 154 : double dist = myInputStorage.readDouble();
1322 154 : addSubscriptionFilterDownstreamDistance(dist);
1323 : }
1324 : break;
1325 130 : case libsumo::FILTER_TYPE_UPSTREAM_DIST: {
1326 130 : myInputStorage.readByte(); // read type double
1327 130 : double dist = myInputStorage.readDouble();
1328 130 : addSubscriptionFilterUpstreamDistance(dist);
1329 : }
1330 : break;
1331 18 : case libsumo::FILTER_TYPE_LEAD_FOLLOW: {
1332 : // Read relative lanes to consider for context filter
1333 18 : addSubscriptionFilterLeadFollow();
1334 : }
1335 : break;
1336 25 : case libsumo::FILTER_TYPE_TURN: {
1337 25 : myInputStorage.readByte(); // read type double
1338 25 : double dist = myInputStorage.readDouble();
1339 25 : addSubscriptionFilterTurn(dist);
1340 : }
1341 : break;
1342 10 : case libsumo::FILTER_TYPE_VCLASS: {
1343 10 : myInputStorage.readByte(); // read type stringlist
1344 10 : SVCPermissions vClasses = parseVehicleClasses(myInputStorage.readStringList());
1345 10 : addSubscriptionFilterVClass(vClasses);
1346 : }
1347 : break;
1348 10 : case libsumo::FILTER_TYPE_VTYPE: {
1349 10 : myInputStorage.readByte(); // read type stringlist
1350 10 : std::vector<std::string> vTypesVector = myInputStorage.readStringList();
1351 : std::set<std::string> vTypesSet;
1352 : vTypesSet.insert(vTypesVector.begin(), vTypesVector.end());
1353 20 : addSubscriptionFilterVType(vTypesSet);
1354 10 : }
1355 : break;
1356 25 : case libsumo::FILTER_TYPE_FIELD_OF_VISION: {
1357 25 : myInputStorage.readByte(); // read type double
1358 25 : double angle = myInputStorage.readDouble();
1359 25 : addSubscriptionFilterFieldOfVision(angle);
1360 : }
1361 : break;
1362 22 : case libsumo::FILTER_TYPE_LATERAL_DIST: {
1363 22 : myInputStorage.readByte(); // read type double
1364 22 : double dist = myInputStorage.readDouble();
1365 22 : 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 1140 : 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 133 : 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 133 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LANES;
1397 133 : myLastContextSubscription->filterLanes = lanes;
1398 133 : }
1399 :
1400 : void
1401 43 : TraCIServer::addSubscriptionFilterNoOpposite() {
1402 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1403 : std::cout << "Adding no opposite filter" << std::endl;
1404 : #endif
1405 43 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_NOOPPOSITE;
1406 43 : }
1407 :
1408 : void
1409 154 : TraCIServer::addSubscriptionFilterDownstreamDistance(double dist) {
1410 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1411 : std::cout << "Adding downstream dist filter (dist=" << toString(dist) << ")" << std::endl;
1412 : #endif
1413 154 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_DOWNSTREAM_DIST;
1414 154 : myLastContextSubscription->filterDownstreamDist = dist;
1415 154 : }
1416 :
1417 : void
1418 130 : TraCIServer::addSubscriptionFilterUpstreamDistance(double dist) {
1419 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1420 : std::cout << "Adding upstream dist filter (dist=" << toString(dist) << ")" << std::endl;
1421 : #endif
1422 130 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_UPSTREAM_DIST;
1423 130 : myLastContextSubscription->filterUpstreamDist = dist;
1424 130 : }
1425 :
1426 : void
1427 18 : TraCIServer::addSubscriptionFilterLeadFollow() {
1428 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1429 : std::cout << "Adding Lead/Follow-maneuver filter" << std::endl;
1430 : #endif
1431 18 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LEAD_FOLLOW;
1432 18 : }
1433 :
1434 : void
1435 25 : TraCIServer::addSubscriptionFilterTurn(double dist) {
1436 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1437 : std::cout << "Adding turn-maneuver filter" << std::endl;
1438 : #endif
1439 25 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_TURN;
1440 25 : myLastContextSubscription->filterFoeDistToJunction = dist;
1441 25 : }
1442 :
1443 : void
1444 10 : TraCIServer::addSubscriptionFilterVClass(SVCPermissions vClasses) {
1445 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1446 : std::cout << "Adding vClass filter (vClasses=" << toString(vClasses) << ")" << std::endl;
1447 : #endif
1448 10 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_VCLASS;
1449 10 : myLastContextSubscription->filterVClasses = vClasses;
1450 10 : }
1451 :
1452 : void
1453 10 : 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 10 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_VTYPE;
1458 : myLastContextSubscription->filterVTypes = vTypes;
1459 10 : }
1460 :
1461 : void
1462 25 : TraCIServer::addSubscriptionFilterFieldOfVision(double openingAngle) {
1463 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1464 : std::cout << "Adding FieldOfVision filter (openingAngle=" << toString(openingAngle) << ")" << std::endl;
1465 : #endif
1466 25 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_FIELD_OF_VISION;
1467 25 : myLastContextSubscription->filterFieldOfVisionOpeningAngle = openingAngle;
1468 25 : }
1469 :
1470 : void
1471 22 : TraCIServer::addSubscriptionFilterLateralDistance(double dist) {
1472 : #ifdef DEBUG_SUBSCRIPTION_FILTERS
1473 : std::cout << "Adding lateral dist filter (dist=" << toString(dist) << ")" << std::endl;
1474 : #endif
1475 22 : myLastContextSubscription->activeFilters = myLastContextSubscription->activeFilters | libsumo::SUBS_FILTER_LATERAL_DIST;
1476 22 : myLastContextSubscription->filterLateralDist = dist;
1477 22 : }
1478 :
1479 : void
1480 25830492 : TraCIServer::writeResponseWithLength(tcpip::Storage& outputStorage, tcpip::Storage& tempMsg) {
1481 25830492 : if (tempMsg.size() < 254) {
1482 25794812 : outputStorage.writeUnsignedByte(1 + (int)tempMsg.size()); // command length -> short
1483 : } else {
1484 35680 : outputStorage.writeUnsignedByte(0); // command length -> extended
1485 35680 : outputStorage.writeInt(1 + 4 + (int)tempMsg.size());
1486 : }
1487 25830492 : outputStorage.writeStorage(tempMsg);
1488 25830492 : }
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 133320 : TraCIServer::readTypeCheckingInt(tcpip::Storage& inputStorage, int& into) {
1509 133320 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_INTEGER) {
1510 : return false;
1511 : }
1512 133316 : into = inputStorage.readInt();
1513 133316 : return true;
1514 : }
1515 :
1516 :
1517 : bool
1518 433531 : TraCIServer::readTypeCheckingDouble(tcpip::Storage& inputStorage, double& into) {
1519 433531 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_DOUBLE) {
1520 : return false;
1521 : }
1522 433530 : into = inputStorage.readDouble();
1523 433530 : return true;
1524 : }
1525 :
1526 :
1527 : bool
1528 548892 : TraCIServer::readTypeCheckingString(tcpip::Storage& inputStorage, std::string& into) {
1529 548892 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_STRING) {
1530 : return false;
1531 : }
1532 548889 : into = inputStorage.readString();
1533 548889 : return true;
1534 : }
1535 :
1536 :
1537 : bool
1538 622 : TraCIServer::readTypeCheckingStringList(tcpip::Storage& inputStorage, std::vector<std::string>& into) {
1539 622 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_STRINGLIST) {
1540 : return false;
1541 : }
1542 622 : into = inputStorage.readStringList();
1543 622 : return true;
1544 : }
1545 :
1546 :
1547 : bool
1548 280 : TraCIServer::readTypeCheckingDoubleList(tcpip::Storage& inputStorage, std::vector<double>& into) {
1549 280 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_DOUBLELIST) {
1550 : return false;
1551 : }
1552 280 : into = inputStorage.readDoubleList();
1553 280 : return true;
1554 : }
1555 :
1556 :
1557 : bool
1558 459 : TraCIServer::readTypeCheckingColor(tcpip::Storage& inputStorage, libsumo::TraCIColor& into) {
1559 459 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_COLOR) {
1560 : return false;
1561 : }
1562 457 : into.r = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1563 457 : into.g = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1564 457 : into.b = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1565 457 : into.a = static_cast<unsigned char>(inputStorage.readUnsignedByte());
1566 457 : return true;
1567 : }
1568 :
1569 :
1570 : bool
1571 216 : TraCIServer::readTypeCheckingPosition2D(tcpip::Storage& inputStorage, libsumo::TraCIPosition& into) {
1572 216 : if (inputStorage.readUnsignedByte() != libsumo::POSITION_2D) {
1573 : return false;
1574 : }
1575 215 : into.x = inputStorage.readDouble();
1576 215 : into.y = inputStorage.readDouble();
1577 215 : into.z = 0;
1578 215 : return true;
1579 : }
1580 :
1581 :
1582 : bool
1583 200087 : TraCIServer::readTypeCheckingByte(tcpip::Storage& inputStorage, int& into) {
1584 200087 : if (inputStorage.readByte() != libsumo::TYPE_BYTE) {
1585 : return false;
1586 : }
1587 200087 : into = inputStorage.readByte();
1588 200087 : return true;
1589 : }
1590 :
1591 :
1592 : bool
1593 4263 : TraCIServer::readTypeCheckingUnsignedByte(tcpip::Storage& inputStorage, int& into) {
1594 4263 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_UBYTE) {
1595 : return false;
1596 : }
1597 4263 : into = inputStorage.readUnsignedByte();
1598 4263 : return true;
1599 : }
1600 :
1601 :
1602 : bool
1603 147 : TraCIServer::readTypeCheckingPolygon(tcpip::Storage& inputStorage, PositionVector& into) {
1604 147 : if (inputStorage.readUnsignedByte() != libsumo::TYPE_POLYGON) {
1605 : return false;
1606 : }
1607 146 : into.clear();
1608 146 : int size = inputStorage.readUnsignedByte();
1609 146 : if (size == 0) {
1610 4 : size = inputStorage.readInt();
1611 : }
1612 146 : PositionVector shape;
1613 1821 : for (int i = 0; i < size; ++i) {
1614 1679 : double x = inputStorage.readDouble();
1615 1679 : double y = inputStorage.readDouble();
1616 1679 : if (std::isnan(x) || std::isnan(y)) {
1617 8 : throw libsumo::TraCIException("NaN-Value in shape.");
1618 : }
1619 1675 : into.push_back(Position(x, y));
1620 : }
1621 : return true;
1622 146 : }
1623 :
1624 :
1625 : void
1626 179 : TraCIServer::stateLoaded(SUMOTime targetTime) {
1627 179 : myTargetTime = targetTime;
1628 358 : for (auto& s : mySockets) {
1629 179 : s.second->targetTime = targetTime;
1630 179 : s.second->executeMove = false;
1631 2506 : for (auto& stateChange : s.second->vehicleStateChanges) {
1632 : stateChange.second.clear();
1633 : }
1634 895 : for (auto& stateChange : s.second->transportableStateChanges) {
1635 : stateChange.second.clear();
1636 : }
1637 : }
1638 : mySubscriptions.clear();
1639 179 : mySubscriptionCache.reset();
1640 179 : }
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 : /****************************************************************************/
|
