Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2013-2026 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 MSDevice_FCDReplay.cpp
15 : /// @author Michael Behrisch
16 : /// @date 01.03.2024
17 : ///
18 : // A device which replays recorded floating car data
19 : /****************************************************************************/
20 : #include <config.h>
21 :
22 : #include <utils/common/StaticCommand.h>
23 : #include <utils/geom/Position.h>
24 : #include <utils/options/OptionsCont.h>
25 : #include <utils/xml/SUMOSAXReader.h>
26 : #include <utils/xml/XMLSubSys.h>
27 : #include <libsumo/Vehicle.h>
28 : #include <microsim/MSNet.h>
29 : #include <microsim/MSEdge.h>
30 : #include <microsim/MSLane.h>
31 : #include <microsim/MSRoute.h>
32 : #include <microsim/MSEventControl.h>
33 : #include <microsim/MSInsertionControl.h>
34 : #include <microsim/transportables/MSTransportableControl.h>
35 : #include <microsim/transportables/MSStageDriving.h>
36 : #include <microsim/transportables/MSStageWaiting.h>
37 : #include <microsim/transportables/MSStageWalking.h>
38 : #include "MSTransportableDevice_FCDReplay.h"
39 : #include "MSDevice_FCDReplay.h"
40 :
41 :
42 : // ===========================================================================
43 : // static member initializations
44 : // ===========================================================================
45 : std::vector<std::unique_ptr<MSDevice_FCDReplay::FCDHandler> > MSDevice_FCDReplay::myHandlers;
46 : std::vector<std::unique_ptr<SUMOSAXReader> > MSDevice_FCDReplay::myParsers;
47 : SumoXMLAttrMask MSDevice_FCDReplay::myUsedAttributes;
48 :
49 :
50 : // ===========================================================================
51 : // method definitions
52 : // ===========================================================================
53 : // ---------------------------------------------------------------------------
54 : // static initialisation methods
55 : // ---------------------------------------------------------------------------
56 : void
57 45763 : MSDevice_FCDReplay::insertOptions(OptionsCont& oc) {
58 45763 : oc.addOptionSubTopic("FCD Replay Device");
59 91526 : insertDefaultAssignmentOptions("fcd-replay", "FCD Replay Device", oc);
60 :
61 45763 : oc.doRegister("device.fcd-replay.files", new Option_FileName());
62 91526 : oc.addSynonyme("device.fcd-replay.files", "device.fcd-replay.file");
63 91526 : oc.addDescription("device.fcd-replay.files", "FCD Replay Device", TL("FCD files to read"));
64 183052 : oc.doRegister("device.fcd-replay.consistency", new Option_StringVector(StringVector({ "all" })));
65 91526 : oc.addDescription("device.fcd-replay.consistency", "FCD Replay Device", TL("Values to use when replaying ('all' or any combination of 'x', 'y', 'edge', 'lane', 'speed', 'position', 'angle', 'type')"));
66 137289 : }
67 :
68 :
69 : void
70 5440076 : MSDevice_FCDReplay::buildVehicleDevices(SUMOVehicle& v, std::vector<MSVehicleDevice*>& into) {
71 5440076 : OptionsCont& oc = OptionsCont::getOptions();
72 10880152 : if (equippedByDefaultAssignmentOptions(oc, "fcd-replay", v, oc.isSet("device.fcd-replay.files"))) {
73 44 : MSDevice_FCDReplay* device = new MSDevice_FCDReplay(v, "fcdReplay_" + v.getID());
74 44 : into.push_back(device);
75 : }
76 5440076 : }
77 :
78 :
79 : void
80 41631 : MSDevice_FCDReplay::init() {
81 41631 : const OptionsCont& oc = OptionsCont::getOptions();
82 : myUsedAttributes.reset();
83 124893 : for (const std::string& attr : oc.getStringVector("device.fcd-replay.consistency")) {
84 41631 : if (attr == "x" || attr == "all") {
85 : myUsedAttributes.set(SUMO_ATTR_X);
86 : }
87 41631 : if (attr == "y" || attr == "all") {
88 : myUsedAttributes.set(SUMO_ATTR_Y);
89 : }
90 41631 : if (attr == "edge" || attr == "all") {
91 : myUsedAttributes.set(SUMO_ATTR_EDGE);
92 : }
93 41631 : if (attr == "lane" || attr == "all") {
94 : myUsedAttributes.set(SUMO_ATTR_LANE);
95 : }
96 41631 : if (attr == "speed" || attr == "all") {
97 : myUsedAttributes.set(SUMO_ATTR_SPEED);
98 : }
99 41631 : if (attr == "position" || attr == "pos" || attr == "all") {
100 : myUsedAttributes.set(SUMO_ATTR_POSITION);
101 : }
102 41631 : if (attr == "angle" || attr == "all") {
103 : myUsedAttributes.set(SUMO_ATTR_ANGLE);
104 : }
105 41631 : if (attr == "type" || attr == "all") {
106 : myUsedAttributes.set(SUMO_ATTR_TYPE);
107 : }
108 : }
109 : myHandlers.clear();
110 83262 : if (oc.isSet("device.fcd-replay.files")) {
111 84 : for (const std::string& filename : oc.getStringVector("device.fcd-replay.files")) {
112 56 : myHandlers.push_back(std::unique_ptr<MSDevice_FCDReplay::FCDHandler>(new MSDevice_FCDReplay::FCDHandler(filename)));
113 56 : myParsers.push_back(std::unique_ptr<SUMOSAXReader>(XMLSubSys::getSAXReader(*myHandlers.back())));
114 56 : if (!myParsers.back()->parseFirst(filename)) {
115 0 : throw ProcessError(TLF("Can not read XML-file '%'.", filename));
116 : }
117 28 : const SUMOTime inc = parseNext(SIMSTEP);
118 28 : MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(new MoveVehicles(), SIMSTEP + DELTA_T);
119 28 : if (inc > 0) {
120 56 : MSNet::getInstance()->getBeginOfTimestepEvents()->addEvent(new StaticCommand<MSDevice_FCDReplay>(&MSDevice_FCDReplay::parseNext),
121 28 : SIMSTEP + inc);
122 : }
123 : }
124 : }
125 41631 : }
126 :
127 :
128 : SUMOTime
129 48 : MSDevice_FCDReplay::parseNext(SUMOTime t) {
130 48 : SUMOTime inc = string2time(OptionsCont::getOptions().getString("route-steps"));
131 : bool haveData = false;
132 96 : for (int i = 0; i < (int)myHandlers.size(); i++) {
133 48 : if (myParsers[i] != nullptr) {
134 : haveData = true;
135 : // make sure that we have always at least inc time steps buffered, so at time 200 we will parse 400 to 600
136 : const SUMOTime start = myHandlers[i]->getTime();
137 60388 : while (myHandlers[i]->getTime() < t + 2 * inc) {
138 60384 : if (!myParsers[i]->parseNext()) {
139 : myParsers[i] = nullptr;
140 : break;
141 : }
142 : }
143 32 : myHandlers[i]->updateTrafficObjects(start);
144 : }
145 : }
146 64 : return haveData ? inc : 0;
147 : }
148 :
149 :
150 : // ---------------------------------------------------------------------------
151 : // MSDevice_FCDReplay-methods
152 : // ---------------------------------------------------------------------------
153 44 : MSDevice_FCDReplay::MSDevice_FCDReplay(SUMOVehicle& holder, const std::string& id) :
154 44 : MSVehicleDevice(holder, id) {
155 44 : }
156 :
157 :
158 88 : MSDevice_FCDReplay::~MSDevice_FCDReplay() {
159 88 : }
160 :
161 :
162 : void
163 3872 : MSDevice_FCDReplay::move(SUMOTime currentTime) {
164 3872 : if (myTrajectory == nullptr) {
165 8 : for (auto& handler : myHandlers) {
166 8 : auto it = handler->getTrajectories().find(myHolder.getID());
167 8 : if (it != handler->getTrajectories().end()) {
168 8 : setTrajectory(&it->second);
169 : break;
170 : }
171 : }
172 8 : if (myTrajectory == nullptr) {
173 : return;
174 : }
175 3864 : } else if (myTrajectoryIndex == (int)myTrajectory->size()) {
176 : // removal happens via the usual MSVehicle::hasArrived mechanism
177 : // TODO we may need to set an arrivalPos
178 : return;
179 : }
180 3832 : MSVehicle* v = dynamic_cast<MSVehicle*>(&myHolder);
181 3832 : const TrajectoryEntry& te = myTrajectory->at(myTrajectoryIndex);
182 3832 : if (v == nullptr || te.time > currentTime) {
183 : return;
184 : }
185 3820 : if (te.edgeOrLane != "") {
186 5400 : const std::string& edgeID = SUMOXMLDefinitions::getEdgeIDFromLane(te.edgeOrLane);
187 2700 : const int laneIdx = SUMOXMLDefinitions::getIndexFromLane(te.edgeOrLane);
188 2700 : libsumo::Vehicle::moveToXY(myHolder.getID(), edgeID, laneIdx, te.pos.x(), te.pos.y(), te.angle, 7);
189 1120 : } else if (te.pos.x() != libsumo::INVALID_DOUBLE_VALUE) {
190 1120 : libsumo::Vehicle::moveToXY(myHolder.getID(), "", -1, te.pos.x(), te.pos.y(), te.angle, 0);
191 : }
192 3820 : v->getInfluencer().setSpeedMode(0);
193 3820 : libsumo::Vehicle::setSpeed(myHolder.getID(), te.speed);
194 : // libsumo::Vehicle::changeLane(myHolder.getID(), laneIdx, TS);
195 3820 : myTrajectoryIndex++;
196 : }
197 :
198 :
199 : SUMOTime
200 4856 : MSDevice_FCDReplay::MoveVehicles::execute(SUMOTime currentTime) {
201 4856 : MSVehicleControl& c = MSNet::getInstance()->getVehicleControl();
202 11728 : for (MSVehicleControl::constVehIt i = c.loadedVehBegin(); i != c.loadedVehEnd(); ++i) {
203 6872 : MSDevice_FCDReplay* device = static_cast<MSDevice_FCDReplay*>(i->second->getDevice(typeid(MSDevice_FCDReplay)));
204 6872 : if (device != nullptr && i->second->hasDeparted()) {
205 3872 : device->move(currentTime);
206 : }
207 : }
208 4856 : return DELTA_T;
209 : }
210 :
211 :
212 : // ---------------------------------------------------------------------------
213 : // MSDevice_FCDReplay::FCDHandler-methods
214 : // ---------------------------------------------------------------------------
215 28 : MSDevice_FCDReplay::FCDHandler::FCDHandler(const std::string& file) :
216 : SUMOSAXHandler(file),
217 : MapMatcher(false, false,
218 28 : OptionsCont::getOptions().getFloat("mapmatch.distance"),
219 112 : MsgHandler::getErrorInstance()) {}
220 :
221 :
222 : void
223 26164 : MSDevice_FCDReplay::FCDHandler::myStartElement(int element, const SUMOSAXAttributes& attrs) {
224 26164 : bool ok = true;
225 26164 : switch (element) {
226 4952 : case SUMO_TAG_TIMESTEP:
227 4952 : myTime = attrs.getSUMOTimeReporting(SUMO_ATTR_TIME, "", ok);
228 : myPositions.clear();
229 26136 : return;
230 21184 : case SUMO_TAG_VEHICLE:
231 : case SUMO_TAG_PERSON:
232 21184 : if (myTime >= SIMSTEP) {
233 21184 : const bool isPerson = element == SUMO_TAG_PERSON;
234 21184 : const std::string id = attrs.getString(SUMO_ATTR_ID);
235 105920 : auto getOptDouble = [&](const SumoXMLAttr attr) {
236 105920 : return myUsedAttributes.test(attr) ? attrs.getOpt<double>(attr, id.c_str(), ok, libsumo::INVALID_DOUBLE_VALUE) : libsumo::INVALID_DOUBLE_VALUE;
237 21184 : };
238 21184 : const Position xy = Position(getOptDouble(SUMO_ATTR_X), getOptDouble(SUMO_ATTR_Y));
239 42368 : const std::string type = myUsedAttributes.test(SUMO_ATTR_TYPE) ? attrs.getOpt<std::string>(SUMO_ATTR_TYPE, id.c_str(), ok, "") : "";
240 : std::string edgeOrLane;
241 21184 : if (myUsedAttributes.test(SUMO_ATTR_EDGE) || myUsedAttributes.test(SUMO_ATTR_LANE)) {
242 46308 : edgeOrLane = attrs.getOpt<std::string>(isPerson ? SUMO_ATTR_EDGE : SUMO_ATTR_LANE, id.c_str(), ok, "");
243 : }
244 21184 : const double speed = getOptDouble(SUMO_ATTR_SPEED);
245 21184 : const double pos = getOptDouble(SUMO_ATTR_POSITION);
246 21184 : const double angle = getOptDouble(SUMO_ATTR_ANGLE);
247 21184 : std::string vehicle = attrs.getOpt<std::string>(SUMO_ATTR_VEHICLE, id.c_str(), ok, "");
248 21184 : if (isPerson) {
249 17244 : if (vehicle == "") {
250 : const auto& veh = myPositions.find(xy);
251 17244 : if (veh != myPositions.end()) {
252 2788 : vehicle = veh->second;
253 : }
254 : }
255 : } else {
256 3940 : myPositions[xy] = id;
257 : }
258 21184 : if (!ok) {
259 0 : WRITE_WARNING("Invalid FCD data.");
260 : return;
261 : }
262 21184 : myTrajectories[id].push_back({myTime, xy, edgeOrLane, pos, speed, angle});
263 42368 : const MSEdge* edge = MSEdge::dictionary(isPerson ? edgeOrLane : SUMOXMLDefinitions::getEdgeIDFromLane(edgeOrLane));
264 21184 : if (edge == nullptr && edgeOrLane != "") {
265 0 : WRITE_WARNINGF(isPerson ? TL("Unknown edge '%' in fcd replay file for person '%'.") : TL("Unknown lane '%' in fcd replay file for vehicle '%'."), edgeOrLane, id);
266 : }
267 : if (myRoutes.count(id) == 0) {
268 208 : if (edgeOrLane == "") {
269 16 : const MSLane* const lane = getClosestLane(xy, SVC_PASSENGER);
270 16 : if (lane != nullptr) {
271 8 : myTrajectories[id].back().edgeOrLane = lane->getID();
272 8 : myTrajectories[id].back().lanePos = lane->interpolateGeometryPosToLanePos(
273 : lane->getShape().nearest_offset_to_point25D(xy, false));
274 8 : edge = &lane->getEdge();
275 8 : while (edge->isInternal()) { // we cannot use an internal edge for a route
276 0 : edge = edge->getSuccessors().front();
277 : }
278 : }
279 : }
280 624 : myRoutes[id] = std::make_tuple(myTime, type, isPerson, ConstMSEdgeVector{edge}, std::vector<StageStart>());
281 : } else {
282 20976 : ConstMSEdgeVector& route = std::get<3>(myRoutes[id]);
283 20976 : if (edge != nullptr && !edge->isInternal() && edge != route.back()) {
284 1288 : route.push_back(edge);
285 : }
286 : }
287 21184 : std::vector<StageStart>& vehicleUsage = std::get<4>(myRoutes[id]);
288 21184 : if ((vehicleUsage.empty() && vehicle != "") || (!vehicleUsage.empty() && vehicle != vehicleUsage.back().vehicle)) {
289 160 : vehicleUsage.push_back({vehicle, (int)myTrajectories[id].size() - 1, (int)std::get<3>(myRoutes[id]).size() - 1});
290 : }
291 : }
292 : return;
293 : default:
294 : break;
295 : }
296 42448 : }
297 :
298 :
299 : MSTransportable::MSTransportablePlan*
300 188 : MSDevice_FCDReplay::FCDHandler::makePlan(const SUMOVehicleParameter& params, const ConstMSEdgeVector& route,
301 : const std::vector<StageStart>& stages, const Trajectory& t) {
302 188 : MSTransportable::MSTransportablePlan* plan = new MSTransportable::MSTransportablePlan();
303 376 : plan->push_back(new MSStageWaiting(route.front(), nullptr, 0, params.depart, params.departPos, "awaiting departure", true));
304 : int prevRouteOffset = 0;
305 188 : const MSEdge* start = route.front();
306 : std::string prevVeh;
307 316 : for (const auto& stageStart : stages) {
308 128 : if (stageStart.vehicle != prevVeh) {
309 128 : if (stageStart.trajectoryOffset != 0) {
310 128 : const MSEdge* prevEdge = MSEdge::dictionary(t[stageStart.trajectoryOffset - 1].edgeOrLane);
311 128 : if (prevVeh == "") {
312 : MSStoppingPlace* finalStop = nullptr;
313 64 : if (prevRouteOffset < (int)route.size() - 1 && (route[prevRouteOffset]->getPermissions() & SVC_PEDESTRIAN) == 0) {
314 0 : prevRouteOffset++; // skip the access
315 : }
316 64 : int offset = stageStart.routeOffset;
317 64 : if (offset < (int)route.size() - 1 && (route[offset]->getPermissions() & SVC_PEDESTRIAN) == SVC_PEDESTRIAN) {
318 0 : offset++; // a bus stop or two consecutive walks so include the edge in both
319 : } else {
320 : // may have an access, let's find the stop
321 64 : const std::string& stop = MSNet::getInstance()->getStoppingPlaceID(route[offset]->getLanes()[0], t[stageStart.trajectoryOffset].lanePos, SUMO_TAG_BUS_STOP);
322 64 : if (stop != "") {
323 64 : finalStop = MSNet::getInstance()->getStoppingPlace(stop, SUMO_TAG_BUS_STOP);
324 : }
325 : }
326 64 : ConstMSEdgeVector subRoute = ConstMSEdgeVector(route.begin() + prevRouteOffset, route.begin() + offset);
327 64 : plan->push_back(new MSStageWalking(params.id, subRoute, finalStop, -1, params.departSpeed, params.departPos, 0, 0));
328 64 : } else {
329 192 : plan->push_back(new MSStageDriving(start, prevEdge, nullptr, -1, 0, {prevVeh}));
330 : }
331 128 : start = MSEdge::dictionary(t[stageStart.trajectoryOffset].edgeOrLane);
332 : prevVeh = stageStart.vehicle;
333 128 : prevRouteOffset = stageStart.routeOffset;
334 : }
335 : }
336 : }
337 : // final stage
338 188 : if (prevVeh == "") {
339 188 : if (prevRouteOffset < (int)route.size() - 1 && (route[prevRouteOffset]->getPermissions() & SVC_PEDESTRIAN) == 0) {
340 64 : prevRouteOffset++;
341 : }
342 : int offset = (int)route.size() - 1;
343 188 : if ((route[offset]->getPermissions() & SVC_PEDESTRIAN) == SVC_PEDESTRIAN) {
344 : offset++;
345 : }
346 188 : if (prevRouteOffset < offset) {
347 : // otherwise we may be still in a vehicle or in access, skip the stage for now
348 188 : ConstMSEdgeVector subRoute = ConstMSEdgeVector(route.begin() + prevRouteOffset, route.begin() + offset);
349 188 : plan->push_back(new MSStageWalking(params.id, subRoute, nullptr, -1, params.departSpeed, params.departPos, 0, 0));
350 188 : }
351 : } else {
352 0 : plan->push_back(new MSStageDriving(start, route.back(), nullptr, -1, 0, {prevVeh}));
353 : }
354 188 : return plan;
355 : }
356 :
357 :
358 : ConstMSEdgeVector
359 40 : MSDevice_FCDReplay::FCDHandler::checkRoute(const ConstMSEdgeVector& edges, const SUMOVehicle* const vehicle) {
360 : ConstMSEdgeVector checkedRoute;
361 480 : for (const MSEdge* const e : edges) {
362 440 : if (checkedRoute.empty() || checkedRoute.back()->isConnectedTo(*e, vehicle->getVehicleType().getVehicleClass())) {
363 432 : checkedRoute.push_back(e);
364 : } else {
365 8 : const MSEdge* fromEdge = checkedRoute.back();
366 : checkedRoute.pop_back();
367 16 : if (!MSNet::getInstance()->getRouterTT(0).compute(fromEdge, e, vehicle, myTime, checkedRoute)) {
368 : // TODO maybe warn about disconnected route
369 4 : checkedRoute.push_back(fromEdge);
370 4 : checkedRoute.push_back(e);
371 : }
372 : // TODO check whether we introduced a big detour
373 : }
374 : }
375 40 : return checkedRoute;
376 0 : }
377 :
378 :
379 : void
380 32 : MSDevice_FCDReplay::FCDHandler::updateTrafficObjects(const SUMOTime intervalStart) {
381 292 : for (const auto& desc : myRoutes) {
382 260 : const std::string& id = desc.first;
383 260 : const bool isPerson = std::get<2>(desc.second);
384 : const ConstMSEdgeVector& routeEdges = std::get<3>(desc.second);
385 260 : Trajectory& t = myTrajectories[id];
386 260 : if (t.front().time >= intervalStart) {
387 : // new vehicle or person
388 208 : SUMOVehicleParameter* params = new SUMOVehicleParameter();
389 208 : params->id = id;
390 208 : params->depart = std::get<0>(desc.second);
391 208 : params->departPos = t.front().lanePos;
392 208 : params->departSpeed = t.front().speed;
393 : // params->arrivalPos = t.back().lanePos;
394 : std::string vType = std::get<1>(desc.second);
395 208 : if (vType == "") {
396 56 : vType = isPerson ? DEFAULT_PEDTYPE_ID : DEFAULT_VTYPE_ID;
397 : }
398 208 : MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(vType);
399 208 : if (vehicleType == nullptr) {
400 0 : delete params;
401 0 : throw ProcessError(TLF("Unknown vType '%'.", vType));
402 : }
403 208 : if (routeEdges.front() == nullptr) {
404 8 : if (isPerson) {
405 0 : WRITE_WARNINGF(TL("No edge in fcd replay file for person '%' at time %."), id, time2string(t.front().time));
406 : } else {
407 24 : WRITE_WARNINGF(TL("No lane in fcd replay file for vehicle '%' at time %."), id, time2string(t.front().time));
408 : }
409 8 : delete params;
410 8 : continue;
411 : }
412 200 : if (isPerson) {
413 164 : MSTransportable::MSTransportablePlan* plan = makePlan(*params, routeEdges, std::get<4>(desc.second), t);
414 : // plan completed, now build the person
415 164 : MSTransportable* person = MSNet::getInstance()->getPersonControl().buildPerson(params, vehicleType, plan, nullptr);
416 164 : person->getSingularType().setVClass(SVC_IGNORING);
417 164 : if (!MSNet::getInstance()->getPersonControl().add(person)) {
418 0 : delete person;
419 0 : throw ProcessError(TLF("Duplicate person '%'.", id));
420 : }
421 164 : MSTransportableDevice_FCDReplay* device = static_cast<MSTransportableDevice_FCDReplay*>(person->getDevice(typeid(MSTransportableDevice_FCDReplay)));
422 164 : if (device == nullptr) { // Person did not get a replay device
423 0 : delete person;
424 0 : continue;
425 : }
426 : device->setTrajectory(&t);
427 : } else {
428 36 : const std::string dummyRouteID = "DUMMY_ROUTE_" + id;
429 : const StopParVector stops;
430 36 : ConstMSRoutePtr route = std::make_shared<MSRoute>(dummyRouteID, routeEdges, true, nullptr, stops);
431 72 : if (!MSRoute::dictionary(dummyRouteID, route)) {
432 0 : throw ProcessError(TLF("Could not add route '%'.", dummyRouteID));
433 : }
434 36 : if (t.front().edgeOrLane != "") {
435 36 : params->departPosProcedure = DepartPosDefinition::GIVEN;
436 36 : params->departLaneProcedure = DepartLaneDefinition::GIVEN;
437 36 : params->departLane = SUMOXMLDefinitions::getIndexFromLane(t.front().edgeOrLane);
438 : }
439 36 : SUMOVehicle* vehicle = MSNet::getInstance()->getVehicleControl().buildVehicle(params, route, vehicleType, false, MSVehicleControl::VehicleDefinitionSource::OTHER);
440 36 : if (!MSNet::getInstance()->getVehicleControl().addVehicle(id, vehicle)) {
441 0 : throw ProcessError(TLF("Duplicate vehicle '%'.", id));
442 : }
443 36 : MSNet::getInstance()->getInsertionControl().add(vehicle);
444 36 : MSDevice_FCDReplay* device = static_cast<MSDevice_FCDReplay*>(vehicle->getDevice(typeid(MSDevice_FCDReplay)));
445 0 : if (device == nullptr) { // Vehicle did not get a replay device
446 0 : MSNet::getInstance()->getVehicleControl().deleteVehicle(vehicle, true);
447 : continue;
448 : }
449 : device->setTrajectory(&t);
450 :
451 : // repair the route, cannot do this on parsing because a vehicle is needed
452 36 : ConstMSEdgeVector checkedRoute = checkRoute(routeEdges, vehicle);
453 36 : if (checkedRoute.size() != routeEdges.size()) {
454 8 : vehicle->replaceRouteEdges(checkedRoute, -1, 0, "FCDReplay", true);
455 : }
456 72 : }
457 52 : } else if (t.back().time >= intervalStart) {
458 : // new data for existing person / vehicle
459 28 : if (isPerson) {
460 24 : MSTransportable* person = MSNet::getInstance()->getPersonControl().get(id);
461 24 : if (person == nullptr) {
462 : // TODO this should not happen
463 0 : continue;
464 : }
465 : // TODO optimize: no need to regenerate the whole plan
466 24 : MSTransportable::MSTransportablePlan* plan = makePlan(person->getParameter(), routeEdges, std::get<4>(desc.second), t);
467 24 : const int stageIndex = person->getNumRemainingStages() - 1;
468 : const MSStage* const finalStage = person->getNextStage(stageIndex);
469 : bool append = false;
470 120 : for (MSStage* const stage : *plan) {
471 96 : if (stage->getStageType() == finalStage->getStageType() && stage->getFromEdge() == finalStage->getFromEdge()) {
472 : // TODO: circular plans?
473 : append = true;
474 : }
475 72 : if (append) {
476 24 : person->appendStage(stage);
477 : } else {
478 72 : delete stage;
479 : }
480 : }
481 24 : person->removeStage(stageIndex);
482 24 : delete plan;
483 : } else {
484 4 : SUMOVehicle* vehicle = MSNet::getInstance()->getVehicleControl().getVehicle(id);
485 4 : ConstMSEdgeVector checkedRoute = checkRoute(routeEdges, vehicle);
486 4 : if ((int)checkedRoute.size() != vehicle->getRoute().size()) {
487 8 : vehicle->replaceRouteEdges(checkedRoute, -1, 0, "FCDReplay", true);
488 : }
489 4 : }
490 : }
491 : }
492 32 : }
493 :
494 :
495 : void
496 8 : MSDevice_FCDReplay::FCDHandler::initLaneTree(NamedRTree* tree) {
497 128 : for (const auto& edge : MSEdge::getAllEdges()) {
498 312 : for (MSLane* lane : edge->getLanes()) {
499 192 : Boundary b = lane->getShape().getBoxBoundary();
500 192 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
501 192 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
502 192 : tree->Insert(cmin, cmax, lane);
503 : }
504 : }
505 8 : }
506 :
507 :
508 : MSEdge*
509 0 : MSDevice_FCDReplay::FCDHandler::retrieveEdge(const std::string& id) {
510 0 : return MSEdge::dictionary(id);
511 : }
512 :
513 :
514 : /****************************************************************************/
|
