1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2017-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file Person.cpp
11 /// @author Leonhard Luecken
12 /// @date 15.09.2017
13 /// @version $Id$
14 ///
15 // C++ TraCI client API implementation
16 /****************************************************************************/
17
18
19 // ===========================================================================
20 // included modules
21 // ===========================================================================
22 #include <config.h>
23
24 #include <microsim/MSTransportableControl.h>
25 #include <microsim/MSVehicleControl.h>
26 #include <microsim/MSEdge.h>
27 #include <microsim/MSNet.h>
28 #include <microsim/pedestrians/MSPerson.h>
29 #include <libsumo/TraCIConstants.h>
30 #include <utils/geom/GeomHelper.h>
31 #include <utils/common/StringTokenizer.h>
32 #include <utils/common/SUMOTime.h>
33 #include <utils/emissions/PollutantsInterface.h>
34 #include <utils/router/PedestrianRouter.h>
35 #include <utils/vehicle/SUMOVehicleParserHelper.h>
36 #include "VehicleType.h"
37 #include "Person.h"
38
39 #define FAR_AWAY 1000.0
40
41 //#define DEBUG_MOVEXY
42 //#define DEBUG_MOVEXY_ANGLE
43
44 namespace libsumo {
45 // ===========================================================================
46 // static member initializations
47 // ===========================================================================
48 SubscriptionResults Person::mySubscriptionResults;
49 ContextSubscriptionResults Person::myContextSubscriptionResults;
50
51
52 // ===========================================================================
53 // static member definitions
54 // ===========================================================================
55 std::vector<std::string>
getIDList()56 Person::getIDList() {
57 MSTransportableControl& c = MSNet::getInstance()->getPersonControl();
58 std::vector<std::string> ids;
59 for (MSTransportableControl::constVehIt i = c.loadedBegin(); i != c.loadedEnd(); ++i) {
60 if (i->second->getCurrentStageType() != MSTransportable::WAITING_FOR_DEPART) {
61 ids.push_back(i->first);
62 }
63 }
64 return std::move(ids);
65 }
66
67
68 int
getIDCount()69 Person::getIDCount() {
70 return MSNet::getInstance()->getPersonControl().size();
71 }
72
73
74 TraCIPosition
getPosition(const std::string & personID,const bool includeZ)75 Person::getPosition(const std::string& personID, const bool includeZ) {
76 return Helper::makeTraCIPosition(getPerson(personID)->getPosition(), includeZ);
77 }
78
79
80 double
getAngle(const std::string & personID)81 Person::getAngle(const std::string& personID) {
82 return GeomHelper::naviDegree(getPerson(personID)->getAngle());
83 }
84
85
86 double
getSlope(const std::string & personID)87 Person::getSlope(const std::string& personID) {
88 MSPerson* person = getPerson(personID);
89 const double ep = person->getEdgePos();
90 const MSLane* lane = getSidewalk<MSEdge, MSLane>(person->getEdge());
91 if (lane == nullptr) {
92 lane = person->getEdge()->getLanes()[0];
93 }
94 const double gp = lane->interpolateLanePosToGeometryPos(ep);
95 return lane->getShape().slopeDegreeAtOffset(gp);
96 }
97
98
99 double
getSpeed(const std::string & personID)100 Person::getSpeed(const std::string& personID) {
101 return getPerson(personID)->getSpeed();
102 }
103
104
105 std::string
getRoadID(const std::string & personID)106 Person::getRoadID(const std::string& personID) {
107 return getPerson(personID)->getEdge()->getID();
108 }
109
110
111 double
getLanePosition(const std::string & personID)112 Person::getLanePosition(const std::string& personID) {
113 return getPerson(personID)->getEdgePos();
114 }
115
116
117 TraCIColor
getColor(const std::string & personID)118 Person::getColor(const std::string& personID) {
119 const RGBColor& col = getPerson(personID)->getParameter().color;
120 TraCIColor tcol;
121 tcol.r = col.red();
122 tcol.g = col.green();
123 tcol.b = col.blue();
124 tcol.a = col.alpha();
125 return tcol;
126 }
127
128
129 std::string
getTypeID(const std::string & personID)130 Person::getTypeID(const std::string& personID) {
131 return getPerson(personID)->getVehicleType().getID();
132 }
133
134
135 double
getWaitingTime(const std::string & personID)136 Person::getWaitingTime(const std::string& personID) {
137 return getPerson(personID)->getWaitingSeconds();
138 }
139
140
141 std::string
getNextEdge(const std::string & personID)142 Person::getNextEdge(const std::string& personID) {
143 return getPerson(personID)->getNextEdge();
144 }
145
146
147 std::vector<std::string>
getEdges(const std::string & personID,int nextStageIndex)148 Person::getEdges(const std::string& personID, int nextStageIndex) {
149 MSTransportable* p = getPerson(personID);
150 if (nextStageIndex >= p->getNumRemainingStages()) {
151 throw TraCIException("The stage index must be lower than the number of remaining stages.");
152 }
153 if (nextStageIndex < (p->getNumRemainingStages() - p->getNumStages())) {
154 throw TraCIException("The negative stage index must refer to a valid previous stage.");
155 }
156 std::vector<std::string> edgeIDs;
157 for (auto& e : p->getEdges(nextStageIndex)) {
158 if (e != nullptr) {
159 edgeIDs.push_back(e->getID());
160 }
161 }
162 return edgeIDs;
163 }
164
165
166 int
getStage(const std::string & personID,int nextStageIndex)167 Person::getStage(const std::string& personID, int nextStageIndex) {
168 MSTransportable* p = getPerson(personID);
169 if (nextStageIndex >= p->getNumRemainingStages()) {
170 throw TraCIException("The stage index must be lower than the number of remaining stages.");
171 }
172 if (nextStageIndex < (p->getNumRemainingStages() - p->getNumStages())) {
173 throw TraCIException("The negative stage index must refer to a valid previous stage.");
174 }
175 return p->getStageType(nextStageIndex);
176 }
177
178
179 int
getRemainingStages(const std::string & personID)180 Person::getRemainingStages(const std::string& personID) {
181 return getPerson(personID)->getNumRemainingStages();
182 }
183
184
185 std::string
getVehicle(const std::string & personID)186 Person::getVehicle(const std::string& personID) {
187 const SUMOVehicle* veh = getPerson(personID)->getVehicle();
188 if (veh == nullptr) {
189 return "";
190 } else {
191 return veh->getID();
192 }
193 }
194
195
196 std::string
getParameter(const std::string & personID,const std::string & param)197 Person::getParameter(const std::string& personID, const std::string& param) {
198 return getPerson(personID)->getParameter().getParameter(param, "");
199 }
200
201
202 std::string
getEmissionClass(const std::string & personID)203 Person::getEmissionClass(const std::string& personID) {
204 return PollutantsInterface::getName(getPerson(personID)->getVehicleType().getEmissionClass());
205 }
206
207
208 std::string
getShapeClass(const std::string & personID)209 Person::getShapeClass(const std::string& personID) {
210 return getVehicleShapeName(getPerson(personID)->getVehicleType().getGuiShape());
211 }
212
213
214 double
getLength(const std::string & personID)215 Person::getLength(const std::string& personID) {
216 return getPerson(personID)->getVehicleType().getLength();
217 }
218
219
220 double
getSpeedFactor(const std::string & personID)221 Person::getSpeedFactor(const std::string& personID) {
222 return getPerson(personID)->getVehicleType().getSpeedFactor().getParameter()[0];
223 }
224
225
226 double
getAccel(const std::string & personID)227 Person::getAccel(const std::string& personID) {
228 return getPerson(personID)->getVehicleType().getCarFollowModel().getMaxAccel();
229 }
230
231
232 double
getDecel(const std::string & personID)233 Person::getDecel(const std::string& personID) {
234 return getPerson(personID)->getVehicleType().getCarFollowModel().getMaxDecel();
235 }
236
237
getEmergencyDecel(const std::string & personID)238 double Person::getEmergencyDecel(const std::string& personID) {
239 return getPerson(personID)->getVehicleType().getCarFollowModel().getEmergencyDecel();
240 }
241
242
getApparentDecel(const std::string & personID)243 double Person::getApparentDecel(const std::string& personID) {
244 return getPerson(personID)->getVehicleType().getCarFollowModel().getApparentDecel();
245 }
246
247
getActionStepLength(const std::string & personID)248 double Person::getActionStepLength(const std::string& personID) {
249 return getPerson(personID)->getVehicleType().getActionStepLengthSecs();
250 }
251
252
253 double
getTau(const std::string & personID)254 Person::getTau(const std::string& personID) {
255 return getPerson(personID)->getVehicleType().getCarFollowModel().getHeadwayTime();
256 }
257
258
259 double
getImperfection(const std::string & personID)260 Person::getImperfection(const std::string& personID) {
261 return getPerson(personID)->getVehicleType().getCarFollowModel().getImperfection();
262 }
263
264
265 double
getSpeedDeviation(const std::string & personID)266 Person::getSpeedDeviation(const std::string& personID) {
267 return getPerson(personID)->getVehicleType().getSpeedFactor().getParameter()[1];
268 }
269
270
271 std::string
getVehicleClass(const std::string & personID)272 Person::getVehicleClass(const std::string& personID) {
273 return toString(getPerson(personID)->getVehicleType().getVehicleClass());
274 }
275
276
277 double
getMinGap(const std::string & personID)278 Person::getMinGap(const std::string& personID) {
279 return getPerson(personID)->getVehicleType().getMinGap();
280 }
281
282
283 double
getMinGapLat(const std::string & personID)284 Person::getMinGapLat(const std::string& personID) {
285 return getPerson(personID)->getVehicleType().getMinGapLat();
286 }
287
288
289 double
getMaxSpeed(const std::string & personID)290 Person::getMaxSpeed(const std::string& personID) {
291 return getPerson(personID)->getVehicleType().getMaxSpeed();
292 }
293
294
295 double
getMaxSpeedLat(const std::string & personID)296 Person::getMaxSpeedLat(const std::string& personID) {
297 return getPerson(personID)->getVehicleType().getMaxSpeedLat();
298 }
299
300
301 std::string
getLateralAlignment(const std::string & personID)302 Person::getLateralAlignment(const std::string& personID) {
303 return toString(getPerson(personID)->getVehicleType().getPreferredLateralAlignment());
304 }
305
306
307 double
getWidth(const std::string & personID)308 Person::getWidth(const std::string& personID) {
309 return getPerson(personID)->getVehicleType().getWidth();
310 }
311
312
313 double
getHeight(const std::string & personID)314 Person::getHeight(const std::string& personID) {
315 return getPerson(personID)->getVehicleType().getHeight();
316 }
317
318
319
320
321 void
setSpeed(const std::string & personID,double speed)322 Person::setSpeed(const std::string& personID, double speed) {
323 getPerson(personID)->setSpeed(speed);
324 }
325
326
327 void
setType(const std::string & personID,const std::string & typeID)328 Person::setType(const std::string& personID, const std::string& typeID) {
329 MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(typeID);
330 if (vehicleType == nullptr) {
331 throw TraCIException("The vehicle type '" + typeID + "' is not known.");
332 }
333 getPerson(personID)->replaceVehicleType(vehicleType);
334 }
335
336
337 void
add(const std::string & personID,const std::string & edgeID,double pos,double departInSecs,const std::string typeID)338 Person::add(const std::string& personID, const std::string& edgeID, double pos, double departInSecs, const std::string typeID) {
339 MSTransportable* p;
340 try {
341 p = getPerson(personID);
342 } catch (TraCIException&) {
343 p = nullptr;
344 }
345
346 if (p != nullptr) {
347 throw TraCIException("The person " + personID + " to add already exists.");
348 }
349
350 SUMOTime depart = TIME2STEPS(departInSecs);
351 SUMOVehicleParameter vehicleParams;
352 vehicleParams.id = personID;
353
354 MSVehicleType* vehicleType = MSNet::getInstance()->getVehicleControl().getVType(typeID);
355 if (!vehicleType) {
356 throw TraCIException("Invalid type '" + typeID + "' for person '" + personID + "'");
357 }
358
359 const MSEdge* edge = MSEdge::dictionary(edgeID);
360 if (!edge) {
361 throw TraCIException("Invalid edge '" + edgeID + "' for person: '" + personID + "'");
362 }
363
364 if (departInSecs < 0.) {
365 const int proc = (int) - departInSecs;
366 if (proc >= static_cast<int>(DEPART_DEF_MAX)) {
367 throw TraCIException("Invalid departure time." + toString(depart) + " " + toString(proc));
368 }
369 vehicleParams.departProcedure = (DepartDefinition)proc;
370 vehicleParams.depart = MSNet::getInstance()->getCurrentTimeStep();
371 } else if (depart < MSNet::getInstance()->getCurrentTimeStep()) {
372 vehicleParams.depart = MSNet::getInstance()->getCurrentTimeStep();
373 WRITE_WARNING("Departure time " + toString(departInSecs) + " for person '" + personID
374 + "' is in the past; using current time " + time2string(vehicleParams.depart) + " instead.");
375 } else {
376 vehicleParams.depart = depart;
377 }
378
379 vehicleParams.departPosProcedure = DEPART_POS_GIVEN;
380 if (fabs(pos) > edge->getLength()) {
381 throw TraCIException("Invalid departure position.");
382 }
383 if (pos < 0) {
384 pos += edge->getLength();
385 }
386 vehicleParams.departPos = pos;
387
388 SUMOVehicleParameter* params = new SUMOVehicleParameter(vehicleParams);
389 MSTransportable::MSTransportablePlan* plan = new MSTransportable::MSTransportablePlan();
390 plan->push_back(new MSTransportable::Stage_Waiting(edge, nullptr, 0, depart, pos, "awaiting departure", true));
391
392 try {
393 MSTransportable* person = MSNet::getInstance()->getPersonControl().buildPerson(params, vehicleType, plan, nullptr);
394 MSNet::getInstance()->getPersonControl().add(person);
395 } catch (ProcessError& e) {
396 delete params;
397 delete plan;
398 throw TraCIException(e.what());
399 }
400 }
401
402
403 void
appendDrivingStage(const std::string & personID,const std::string & toEdge,const std::string & lines,const std::string & stopID)404 Person::appendDrivingStage(const std::string& personID, const std::string& toEdge, const std::string& lines, const std::string& stopID) {
405 MSTransportable* p = getPerson(personID);
406 const MSEdge* edge = MSEdge::dictionary(toEdge);
407 if (!edge) {
408 throw TraCIException("Invalid edge '" + toEdge + "' for person: '" + personID + "'");
409 }
410 if (lines.size() == 0) {
411 return throw TraCIException("Empty lines parameter for person: '" + personID + "'");
412 }
413 MSStoppingPlace* bs = nullptr;
414 if (stopID != "") {
415 bs = MSNet::getInstance()->getStoppingPlace(stopID, SUMO_TAG_BUS_STOP);
416 if (bs == nullptr) {
417 throw TraCIException("Invalid stopping place id '" + stopID + "' for person: '" + personID + "'");
418 }
419 }
420 p->appendStage(new MSPerson::MSPersonStage_Driving(edge, bs, -NUMERICAL_EPS, StringTokenizer(lines).getVector()));
421 }
422
423
424 void
appendWaitingStage(const std::string & personID,double duration,const std::string & description,const std::string & stopID)425 Person::appendWaitingStage(const std::string& personID, double duration, const std::string& description, const std::string& stopID) {
426 MSTransportable* p = getPerson(personID);
427 if (duration < 0) {
428 throw TraCIException("Duration for person: '" + personID + "' must not be negative");
429 }
430 MSStoppingPlace* bs = nullptr;
431 if (stopID != "") {
432 bs = MSNet::getInstance()->getStoppingPlace(stopID, SUMO_TAG_BUS_STOP);
433 if (bs == nullptr) {
434 throw TraCIException("Invalid stopping place id '" + stopID + "' for person: '" + personID + "'");
435 }
436 }
437 p->appendStage(new MSTransportable::Stage_Waiting(p->getArrivalEdge(), nullptr, TIME2STEPS(duration), 0, p->getArrivalPos(), description, false));
438 }
439
440
441 void
appendWalkingStage(const std::string & personID,const std::vector<std::string> & edgeIDs,double arrivalPos,double duration,double speed,const std::string & stopID)442 Person::appendWalkingStage(const std::string& personID, const std::vector<std::string>& edgeIDs, double arrivalPos, double duration, double speed, const std::string& stopID) {
443 MSTransportable* p = getPerson(personID);
444 ConstMSEdgeVector edges;
445 try {
446 MSEdge::parseEdgesList(edgeIDs, edges, "<unknown>");
447 } catch (ProcessError& e) {
448 throw TraCIException(e.what());
449 }
450 if (edges.empty()) {
451 throw TraCIException("Empty edge list for walking stage of person '" + personID + "'.");
452 }
453 if (fabs(arrivalPos) > edges.back()->getLength()) {
454 throw TraCIException("Invalid arrivalPos for walking stage of person '" + personID + "'.");
455 }
456 if (arrivalPos < 0) {
457 arrivalPos += edges.back()->getLength();
458 }
459 if (speed < 0) {
460 speed = p->getVehicleType().getMaxSpeed();
461 }
462 MSStoppingPlace* bs = nullptr;
463 if (stopID != "") {
464 bs = MSNet::getInstance()->getStoppingPlace(stopID, SUMO_TAG_BUS_STOP);
465 if (bs == nullptr) {
466 throw TraCIException("Invalid stopping place id '" + stopID + "' for person: '" + personID + "'");
467 }
468 }
469 p->appendStage(new MSPerson::MSPersonStage_Walking(p->getID(), edges, bs, TIME2STEPS(duration), speed, p->getArrivalPos(), arrivalPos, 0));
470 }
471
472
473 void
removeStage(const std::string & personID,int nextStageIndex)474 Person::removeStage(const std::string& personID, int nextStageIndex) {
475 MSTransportable* p = getPerson(personID);
476 if (nextStageIndex >= p->getNumRemainingStages()) {
477 throw TraCIException("The stage index must be lower than the number of remaining stages.");
478 }
479 if (nextStageIndex < 0) {
480 throw TraCIException("The stage index may not be negative.");
481 }
482 p->removeStage(nextStageIndex);
483 }
484
485
486 void
rerouteTraveltime(const std::string & personID)487 Person::rerouteTraveltime(const std::string& personID) {
488 MSPerson* p = getPerson(personID);
489 if (p->getNumRemainingStages() == 0) {
490 throw TraCIException("Person '" + personID + "' has no remaining stages.");
491 }
492 const MSEdge* from = p->getEdge();
493 double departPos = p->getEdgePos();
494 // reroute to the start of the next-non-walking stage
495 int firstIndex;
496 if (p->getCurrentStageType() == MSTransportable::MOVING_WITHOUT_VEHICLE) {
497 firstIndex = 0;
498 } else if (p->getCurrentStageType() == MSTransportable::WAITING) {
499 if (p->getNumRemainingStages() < 2 || p->getStageType(1) != MSTransportable::MOVING_WITHOUT_VEHICLE) {
500 throw TraCIException("Person '" + personID + "' cannot reroute after the current stop.");
501 }
502 firstIndex = 1;
503 } else {
504 throw TraCIException("Person '" + personID + "' cannot reroute in stage type '" + toString(p->getCurrentStageType()) + "'.");
505 }
506 int nextIndex = firstIndex + 1;
507 for (; nextIndex < p->getNumRemainingStages(); nextIndex++) {
508 if (p->getStageType(nextIndex) != MSTransportable::MOVING_WITHOUT_VEHICLE) {
509 break;
510 }
511 }
512 MSTransportable::Stage* destStage = p->getNextStage(nextIndex - 1);
513 const MSEdge* to = destStage->getEdges().back();
514 double arrivalPos = destStage->getArrivalPos();
515 double speed = p->getVehicleType().getMaxSpeed();
516 ConstMSEdgeVector newEdges;
517 MSNet::getInstance()->getPedestrianRouter().compute(from, to, departPos, arrivalPos, speed, 0, nullptr, newEdges);
518 if (newEdges.empty()) {
519 throw TraCIException("Could not find new route for person '" + personID + "'.");
520 }
521 ConstMSEdgeVector oldEdges = p->getEdges(firstIndex);
522 assert(!oldEdges.empty());
523 if (oldEdges.front()->getFunction() != EDGEFUNC_NORMAL) {
524 oldEdges.erase(oldEdges.begin());
525 }
526 //std::cout << " remainingStages=" << p->getNumRemainingStages() << " oldEdges=" << toString(oldEdges) << " newEdges=" << toString(newEdges) << " firstIndex=" << firstIndex << " nextIndex=" << nextIndex << "\n";
527 if (newEdges == oldEdges && (firstIndex + 1 == nextIndex)) {
528 return;
529 }
530 if (newEdges.front() != from) {
531 // @note: maybe this should be done automatically by the router
532 newEdges.insert(newEdges.begin(), from);
533 }
534 p->reroute(newEdges, departPos, firstIndex, nextIndex);
535 }
536
537
538 void
moveTo(const std::string & personID,const std::string & edgeID,double)539 Person::moveTo(const std::string& personID, const std::string& edgeID, double /* position */) {
540 MSPerson* p = getPerson(personID);
541 MSEdge* e = MSEdge::dictionary(edgeID);
542 if (e == nullptr) {
543 throw TraCIException("Unknown edge '" + edgeID + "'.");
544 }
545 /*
546 switch (p->getStageType(0)) {
547 case MSTransportable::MOVING_WITHOUT_VEHICLE: {
548 MSPerson::MSPersonStage_Walking* s = dynamic_cast<MSPerson::MSPersonStage_Walking*>(p->getCurrentStage());
549 assert(s != 0);
550 const std::string error = s->moveTo(p, Simulation::getCurrentTime());
551 if (error != "") {
552 throw TraCIException("Command moveTo failed for person '" + personID + "' (" + error + ").");
553 }
554 break;
555 }
556 default:
557 */
558 throw TraCIException("Command moveTo is not supported for person '" + personID + "' while " + p->getCurrentStageDescription() + ".");
559 //}
560 }
561
562
563 void
moveToXY(const std::string & personID,const std::string & edgeID,const double x,const double y,double angle,const int keepRouteFlag)564 Person::moveToXY(const std::string& personID, const std::string& edgeID, const double x, const double y, double angle, const int keepRouteFlag) {
565 MSPerson* p = getPerson(personID);
566 bool keepRoute = (keepRouteFlag == 1);
567 bool mayLeaveNetwork = (keepRouteFlag == 2);
568 Position pos(x, y);
569 #ifdef DEBUG_MOVEXY
570 const double origAngle = angle;
571 #endif
572 // angle must be in [0,360] because it will be compared against those returned by naviDegree()
573 // angle set to INVALID_DOUBLE_VALUE is ignored in the evaluated and later set to the angle of the matched lane
574 if (angle != INVALID_DOUBLE_VALUE) {
575 while (angle >= 360.) {
576 angle -= 360.;
577 }
578 while (angle < 0.) {
579 angle += 360.;
580 }
581 }
582 Position currentPos = p->getPosition();
583 #ifdef DEBUG_MOVEXY
584 std::cout << std::endl << "begin person " << p->getID() << " lanePos:" << p->getEdgePos() << " edge:" << Named::getIDSecure(p->getEdge()) << "\n";
585 std::cout << " want pos:" << pos << " edgeID:" << edgeID << " origAngle:" << origAngle << " angle:" << angle << " keepRoute:" << keepRoute << std::endl;
586 #endif
587
588 ConstMSEdgeVector edges;
589 MSLane* lane = nullptr;
590 double lanePos;
591 double lanePosLat = 0;
592 double bestDistance = std::numeric_limits<double>::max();
593 int routeOffset = 0;
594 bool found = false;
595 double maxRouteDistance = 100;
596
597 ConstMSEdgeVector ev;
598 ev.push_back(p->getEdge());
599 int routeIndex = 0;
600 MSLane* currentLane = const_cast<MSLane*>(getSidewalk<MSEdge, MSLane>(p->getEdge()));
601 switch (p->getStageType(0)) {
602 case MSTransportable::MOVING_WITHOUT_VEHICLE: {
603 MSPerson::MSPersonStage_Walking* s = dynamic_cast<MSPerson::MSPersonStage_Walking*>(p->getCurrentStage());
604 assert(s != 0);
605 ev = s->getEdges();
606 routeIndex = (int)(s->getRouteStep() - s->getRoute().begin());
607 }
608 break;
609 default:
610 break;
611 }
612 if (keepRoute) {
613 // case a): vehicle is on its earlier route
614 // we additionally assume it is moving forward (SUMO-limit);
615 // note that the route ("edges") is not changed in this case
616 found = Helper::moveToXYMap_matchingRoutePosition(pos, edgeID,
617 ev, routeIndex,
618 bestDistance, &lane, lanePos, routeOffset);
619 } else {
620 double speed = pos.distanceTo2D(p->getPosition()); // !!!veh->getSpeed();
621 found = Helper::moveToXYMap(pos, maxRouteDistance, mayLeaveNetwork, edgeID, angle,
622 speed, ev, routeIndex, currentLane, p->getEdgePos(), true,
623 bestDistance, &lane, lanePos, routeOffset, edges);
624 }
625 if ((found && bestDistance <= maxRouteDistance) || mayLeaveNetwork) {
626 // compute lateral offset
627 if (found) {
628 const double perpDist = lane->getShape().distance2D(pos, false);
629 if (perpDist != GeomHelper::INVALID_OFFSET) {
630 lanePosLat = perpDist;
631 if (!mayLeaveNetwork) {
632 lanePosLat = MIN2(lanePosLat, 0.5 * (lane->getWidth() + p->getVehicleType().getWidth()));
633 }
634 // figure out whether the offset is to the left or to the right
635 PositionVector tmp = lane->getShape();
636 try {
637 tmp.move2side(-lanePosLat); // moved to left
638 } catch (ProcessError&) {
639 WRITE_WARNING("Could not determine position on lane '" + lane->getID() + " at lateral position " + toString(-lanePosLat) + ".");
640 }
641 //std::cout << " lane=" << lane->getID() << " posLat=" << lanePosLat << " shape=" << lane->getShape() << " tmp=" << tmp << " tmpDist=" << tmp.distance2D(pos) << "\n";
642 if (tmp.distance2D(pos) > perpDist) {
643 lanePosLat = -lanePosLat;
644 }
645 }
646 }
647 if (found && !mayLeaveNetwork && MSGlobals::gLateralResolution < 0) {
648 // mapped position may differ from pos
649 pos = lane->geometryPositionAtOffset(lanePos, -lanePosLat);
650 }
651 assert((found && lane != 0) || (!found && lane == 0));
652 if (angle == INVALID_DOUBLE_VALUE) {
653 if (lane != nullptr) {
654 angle = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(lanePos));
655 } else {
656 // compute angle outside road network from old and new position
657 angle = GeomHelper::naviDegree(p->getPosition().angleTo2D(pos));
658 }
659 }
660 switch (p->getStageType(0)) {
661 case MSTransportable::MOVING_WITHOUT_VEHICLE: {
662 Helper::setRemoteControlled(p, pos, lane, lanePos, lanePosLat, angle, routeOffset, edges, MSNet::getInstance()->getCurrentTimeStep());
663 break;
664 }
665 default:
666 throw TraCIException("Command moveToXY is not supported for person '" + personID + "' while " + p->getCurrentStageDescription() + ".");
667 }
668 } else {
669 if (lane == nullptr) {
670 throw TraCIException("Could not map person '" + personID + "' no road found within " + toString(maxRouteDistance) + "m.");
671 } else {
672 throw TraCIException("Could not map person '" + personID + "' distance to road is " + toString(bestDistance) + ".");
673 }
674 }
675 }
676
677
678 /** untested setter functions which alter the person's vtype ***/
679
680 void
setParameter(const std::string & personID,const std::string & key,const std::string & value)681 Person::setParameter(const std::string& personID, const std::string& key, const std::string& value) {
682 MSTransportable* p = getPerson(personID);
683 ((SUMOVehicleParameter&)p->getParameter()).setParameter(key, value);
684 }
685
686 void
setLength(const std::string & personID,double length)687 Person::setLength(const std::string& personID, double length) {
688 getPerson(personID)->getSingularType().setLength(length);
689 }
690
691
692 void
setMaxSpeed(const std::string & personID,double speed)693 Person::setMaxSpeed(const std::string& personID, double speed) {
694 getPerson(personID)->getSingularType().setMaxSpeed(speed);
695 }
696
697
698 void
setVehicleClass(const std::string & personID,const std::string & clazz)699 Person::setVehicleClass(const std::string& personID, const std::string& clazz) {
700 getPerson(personID)->getSingularType().setVClass(getVehicleClassID(clazz));
701 }
702
703
704 void
setShapeClass(const std::string & personID,const std::string & clazz)705 Person::setShapeClass(const std::string& personID, const std::string& clazz) {
706 getPerson(personID)->getSingularType().setShape(getVehicleShapeID(clazz));
707 }
708
709
710 void
setEmissionClass(const std::string & personID,const std::string & clazz)711 Person::setEmissionClass(const std::string& personID, const std::string& clazz) {
712 getPerson(personID)->getSingularType().setEmissionClass(PollutantsInterface::getClassByName(clazz));
713 }
714
715
716 void
setWidth(const std::string & personID,double width)717 Person::setWidth(const std::string& personID, double width) {
718 getPerson(personID)->getSingularType().setWidth(width);
719 }
720
721
722 void
setHeight(const std::string & personID,double height)723 Person::setHeight(const std::string& personID, double height) {
724 getPerson(personID)->getSingularType().setHeight(height);
725 }
726
727
728 void
setMinGap(const std::string & personID,double minGap)729 Person::setMinGap(const std::string& personID, double minGap) {
730 getPerson(personID)->getSingularType().setMinGap(minGap);
731 }
732
733
734 void
setAccel(const std::string & personID,double accel)735 Person::setAccel(const std::string& personID, double accel) {
736 getPerson(personID)->getSingularType().setAccel(accel);
737 }
738
739
740 void
setDecel(const std::string & personID,double decel)741 Person::setDecel(const std::string& personID, double decel) {
742 getPerson(personID)->getSingularType().setDecel(decel);
743 }
744
745
746 void
setEmergencyDecel(const std::string & personID,double decel)747 Person::setEmergencyDecel(const std::string& personID, double decel) {
748 getPerson(personID)->getSingularType().setEmergencyDecel(decel);
749 }
750
751
752 void
setApparentDecel(const std::string & personID,double decel)753 Person::setApparentDecel(const std::string& personID, double decel) {
754 getPerson(personID)->getSingularType().setApparentDecel(decel);
755 }
756
757
758 void
setImperfection(const std::string & personID,double imperfection)759 Person::setImperfection(const std::string& personID, double imperfection) {
760 getPerson(personID)->getSingularType().setImperfection(imperfection);
761 }
762
763
764 void
setTau(const std::string & personID,double tau)765 Person::setTau(const std::string& personID, double tau) {
766 getPerson(personID)->getSingularType().setTau(tau);
767 }
768
769
770 void
setMinGapLat(const std::string & personID,double minGapLat)771 Person::setMinGapLat(const std::string& personID, double minGapLat) {
772 getPerson(personID)->getSingularType().setMinGapLat(minGapLat);
773 }
774
775
776 void
setMaxSpeedLat(const std::string & personID,double speed)777 Person::setMaxSpeedLat(const std::string& personID, double speed) {
778 getPerson(personID)->getSingularType().setMaxSpeedLat(speed);
779 }
780
781
782 void
setLateralAlignment(const std::string & personID,const std::string & latAlignment)783 Person::setLateralAlignment(const std::string& personID, const std::string& latAlignment) {
784 getPerson(personID)->getSingularType().setPreferredLateralAlignment(SUMOXMLDefinitions::LateralAlignments.get(latAlignment));
785 }
786
787
788 void
setSpeedFactor(const std::string & personID,double factor)789 Person::setSpeedFactor(const std::string& personID, double factor) {
790 getPerson(personID)->getSingularType().setSpeedFactor(factor);
791 }
792
793
794 void
setActionStepLength(const std::string & personID,double actionStepLength,bool resetActionOffset)795 Person::setActionStepLength(const std::string& personID, double actionStepLength, bool resetActionOffset) {
796 getPerson(personID)->getSingularType().setActionStepLength(SUMOVehicleParserHelper::processActionStepLength(actionStepLength), resetActionOffset);
797 }
798
799
800 void
setColor(const std::string & personID,const TraCIColor & c)801 Person::setColor(const std::string& personID, const TraCIColor& c) {
802 const SUMOVehicleParameter& p = getPerson(personID)->getParameter();
803 p.color.set((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
804 p.parametersSet |= VEHPARS_COLOR_SET;
805 }
806
807
LIBSUMO_SUBSCRIPTION_IMPLEMENTATION(Person,PERSON)808 LIBSUMO_SUBSCRIPTION_IMPLEMENTATION(Person, PERSON)
809
810
811 MSPerson*
812 Person::getPerson(const std::string& personID) {
813 MSTransportableControl& c = MSNet::getInstance()->getPersonControl();
814 MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
815 if (p == nullptr) {
816 throw TraCIException("Person '" + personID + "' is not known");
817 }
818 return p;
819 }
820
821
822 void
storeShape(const std::string & id,PositionVector & shape)823 Person::storeShape(const std::string& id, PositionVector& shape) {
824 shape.push_back(getPerson(id)->getPosition());
825 }
826
827
828 std::shared_ptr<VariableWrapper>
makeWrapper()829 Person::makeWrapper() {
830 return std::make_shared<Helper::SubscriptionWrapper>(handleVariable, mySubscriptionResults, myContextSubscriptionResults);
831 }
832
833
834 bool
handleVariable(const std::string & objID,const int variable,VariableWrapper * wrapper)835 Person::handleVariable(const std::string& objID, const int variable, VariableWrapper* wrapper) {
836 switch (variable) {
837 case TRACI_ID_LIST:
838 return wrapper->wrapStringList(objID, variable, getIDList());
839 case ID_COUNT:
840 return wrapper->wrapInt(objID, variable, getIDCount());
841 case VAR_POSITION:
842 return wrapper->wrapPosition(objID, variable, getPosition(objID));
843 case VAR_POSITION3D:
844 return wrapper->wrapPosition(objID, variable, getPosition(objID, true));
845 case VAR_ANGLE:
846 return wrapper->wrapDouble(objID, variable, getAngle(objID));
847 case VAR_SLOPE:
848 return wrapper->wrapDouble(objID, variable, getSlope(objID));
849 case VAR_SPEED:
850 return wrapper->wrapDouble(objID, variable, getSpeed(objID));
851 case VAR_ROAD_ID:
852 return wrapper->wrapString(objID, variable, getRoadID(objID));
853 case VAR_LANEPOSITION:
854 return wrapper->wrapDouble(objID, variable, getLanePosition(objID));
855 case VAR_COLOR:
856 return wrapper->wrapColor(objID, variable, getColor(objID));
857 case VAR_WAITING_TIME:
858 return wrapper->wrapDouble(objID, variable, getWaitingTime(objID));
859 case VAR_TYPE:
860 return wrapper->wrapString(objID, variable, getTypeID(objID));
861 case VAR_NEXT_EDGE:
862 return wrapper->wrapString(objID, variable, getNextEdge(objID));
863 case VAR_STAGES_REMAINING:
864 return wrapper->wrapInt(objID, variable, getRemainingStages(objID));
865 case VAR_VEHICLE:
866 return wrapper->wrapString(objID, variable, getVehicle(objID));
867 default:
868 return false;
869 }
870 }
871
872
873 }
874
875
876 /****************************************************************************/
877