SUMO - Simulation of Urban MObility
MSPModel_Remote.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2014-2018 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 /****************************************************************************/
15 // The pedestrian following model for remote controlled pedestrian movement
16 /****************************************************************************/
17 
18 #include "MSPModel_Remote.h"
19 #include <grpc++/grpc++.h>
20 #include "microsim/MSEdge.h"
21 #include "microsim/MSLane.h"
22 #include "microsim/MSEdgeControl.h"
24 #include "microsim/MSGlobals.h"
25 #include "hybridsim.grpc.pb.h"
26 #include "utils/geom/Position.h"
28 
29 using grpc::Channel;
30 using grpc::ClientContext;
31 using grpc::Status;
32 
34  : myNet(net) {
35  const std::string address = oc.getString("pedestrian.remote.address");
36  myHybridsimStub = hybridsim::HybridSimulation::NewStub(grpc::CreateChannel(
37  address, grpc::InsecureChannelCredentials()));
38 
39  initialize();
40 
41  Event* e = new Event(this);
43 }
45 
47 
48  PState* state = new PState(person, stage);
49 
50  hybridsim::Agent req;
51  int id = myLastId++;
52  remoteIdPStateMapping[id] = state;
53  req.set_id(id);
54 
55  MSLane* departureLane = getFirstPedestrianLane(*(stage->getRoute().begin()));
56  double departureOffsetAlongLane = stage->getDepartPos();
57 
58  //TODO fix this on casim side [GL]
59  double offset = departureOffsetAlongLane == 0 ? 0.4 : -0.4;
60  departureOffsetAlongLane += offset;
61 
62  Position departurePos = departureLane->getShape().positionAtOffset(departureOffsetAlongLane);
63  hybridsim::Coordinate* departureCoordinate = req.mutable_enterlocation();
64  departureCoordinate->set_x(departurePos.x());
65  departureCoordinate->set_y(departurePos.y());
66 
67  MSLane* arrivalLane = getFirstPedestrianLane(*(stage->getRoute().end() - 1));
68  double arrivalOffsetAlongLange = stage->getArrivalPos();
69  Position arrivalPos = arrivalLane->getShape().positionAtOffset(arrivalOffsetAlongLange);
70  hybridsim::Coordinate* arrivalCoordinate = req.mutable_leavelocation();
71  arrivalCoordinate->set_x(arrivalPos.x());
72  arrivalCoordinate->set_y(arrivalPos.y());
73 
74 
75  const MSEdge* prv = 0;
76  for (ConstMSEdgeVector::const_iterator it = stage->getRoute().begin(); it != stage->getRoute().end(); it++) {
77  const MSEdge* edge = *it;
78  int dir = FORWARD;//default
79  if (prv == 0) {
80  if (stage->getRoute().size() > 1) {
81  const MSEdge* nxt = *(it + 1);
82  dir = (edge->getFromJunction() == nxt->getFromJunction()
83  || edge->getFromJunction() == nxt->getToJunction()) ? BACKWARD : FORWARD;
84  } else {
85  dir = stage->getDepartPos() == 0 ? FORWARD : BACKWARD;
86  }
87  } else {
88  dir = (edge->getFromJunction() == prv->getToJunction()
89  || edge->getFromJunction() == prv->getFromJunction()) ? FORWARD : BACKWARD;
90  }
91  if (edgesTransitionsMapping.find(edge) == edgesTransitionsMapping.end()) {
92  throw ProcessError("Cannot map edge : " + edge->getID() + " to remote simulation");
93  };
94  std::tuple<int, int> transitions = edgesTransitionsMapping[edge];
95 
96  int frId = dir == FORWARD ? std::get<0>(transitions) : std::get<1>(transitions);
97  int toId = dir == FORWARD ? std::get<1>(transitions) : std::get<0>(transitions);
98  hybridsim::Destination* destFr = req.add_dests();
99  destFr->set_id(frId);
100  hybridsim::Destination* destTo = req.add_dests();
101  destTo->set_id(toId);
102  prv = edge;
103  }
104 
105  hybridsim::Boolean rpl;
106  ClientContext context;
107  Status st = myHybridsimStub->transferAgent(&context, req, &rpl);
108  if (!st.ok()) {
109  throw ProcessError("Person: " + person->getID() + " could not be transferred to remote simulation");
110  }
111  if (!rpl.val()) {
112  //TODO not yet implemented
113  throw ProcessError("Remote simulation declined to accept person: " + person->getID() + ".");
114  }
115 
116  return state;
117 }
118 
120 
121  hybridsim::Empty req;
122  hybridsim::Empty rpl;
123  ClientContext context1;
124  Status st = myHybridsimStub->shutdown(&context1, req, &rpl);
125  if (!st.ok()) {
126  throw ProcessError("Could not shutdown remote server");
127  }
128 
129 
130 }
131 
133 
134  hybridsim::LeftClosedRightOpenTimeInterval interval;
135  interval.set_fromtimeincluding(time / DELTA_T);
136  interval.set_totimeexcluding((time + DELTA_T) / DELTA_T);
137 
138 
139  //1. simulate time interval
140  hybridsim::Empty rpl;
141  ClientContext context1;
142  Status st = myHybridsimStub->simulatedTimeInerval(&context1, interval, &rpl);
143  if (!st.ok()) {
144  throw ProcessError("Could not simulated time interval from: " + toString(time) + " to: " + toString(time + DELTA_T));
145  }
146 
147  //2. receive trajectories
148  hybridsim::Empty req2;
149  hybridsim::Trajectories trajectories;
150  ClientContext context2;
151  Status st2 = myHybridsimStub->receiveTrajectories(&context2, req2, &trajectories);
152  if (!st2.ok()) {
153  throw ProcessError("Could not receive trajectories from remote simulation");
154  }
155  for (hybridsim::Trajectory trajectory : trajectories.trajectories()) {
156  if (remoteIdPStateMapping.find(trajectory.id()) != remoteIdPStateMapping.end()) {
157  PState* pState = remoteIdPStateMapping[trajectory.id()];
158  pState->setPosition(trajectory.x(), trajectory.y());
159  pState->setPhi(trajectory.phi());
160  if (transitionsEdgesMapping.find(trajectory.currentdest().id()) != transitionsEdgesMapping.end()) {
161  const MSEdge* nextTargetEdge = transitionsEdgesMapping[trajectory.currentdest().id()];
162  const MSEdge* nextStageEdge = pState->getStage()->getNextRouteEdge();
163 // const MSEdge* currentStageEdge = pState->getStage()->getEdge();
164  if (nextTargetEdge == nextStageEdge) {
165  const bool arrived = pState->getStage()->moveToNextEdge(pState->getPerson(), time);
166  std::cout << "next edge" << std::endl;
167  }
168  }
169 // pState.
170  } else {
171  throw ProcessError("Pedestrian with id: " + toString(trajectory.id()) + " is not known.");
172  }
173  }
174 
175  //3. retrieve agents that are ready to come back home to SUMO
176  hybridsim::Empty req3;
177  hybridsim::Agents agents;
178  ClientContext context3;
179  Status st3 = myHybridsimStub->queryRetrievableAgents(&context3, req3, &agents);
180  if (!st3.ok()) {
181  throw ProcessError("Could not query retrievable agents");
182  }
183  //TODO check whether agents can be retrieved
184  for (hybridsim::Agent agent : agents.agents()) {
185  if (remoteIdPStateMapping.find(agent.id()) != remoteIdPStateMapping.end()) {
186  PState* pState = remoteIdPStateMapping[agent.id()];
187  while (!pState->getStage()->moveToNextEdge(pState->getPerson(), time)) {
188  remoteIdPStateMapping.erase(agent.id());
189  }
190  }
191  }
192 
193  //4. confirm transferred agents
194  hybridsim::Empty rpl2;
195  ClientContext context4;
196  Status st4 = myHybridsimStub->confirmRetrievedAgents(&context4, agents, &rpl2);
197  if (!st4.ok()) {
198  throw ProcessError("Could not confirm retrieved agents");
199  }
200 
201  return DELTA_T;
202 }
204  for (MSLane* lane : edge->getLanes()) {
205  if (lane->getPermissions() == SVC_PEDESTRIAN) {
206  return lane;
207  }
208  }
209  throw ProcessError("Edge: " + edge->getID() + " does not allow pedestrians.");
210 }
211 
213 
214 }
215 
218 }
219 
221  hybridsim::Scenario req;
222 
223  //1. for all edges
224  for (MSEdge* e : myNet->getEdgeControl().getEdges()) {
225  if (e->isInternal()) {
226  continue;
227  }
228  if (e->isWalkingArea()) {
229  handleWalkingArea(e, req);
230  continue;
231  }
232  for (MSLane* l : e->getLanes()) {
233  if (l->getPermissions() == SVC_PEDESTRIAN) {
234  handlePedestrianLane(l, req);
235  }
236  }
237  }
238 
239  //add boundary
240  hybridsim::Edge* edge = req.add_edges();
241  edge->mutable_c0()->set_x(myBoundary.xmin());
242  edge->mutable_c0()->set_y(myBoundary.ymin());
243  edge->mutable_c1()->set_x(myBoundary.xmax());
244  edge->mutable_c1()->set_y(myBoundary.ymin());
245  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
246 
247  edge = req.add_edges();
248  edge->mutable_c0()->set_x(myBoundary.xmax());
249  edge->mutable_c0()->set_y(myBoundary.ymin());
250  edge->mutable_c1()->set_x(myBoundary.xmax());
251  edge->mutable_c1()->set_y(myBoundary.ymax());
252  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
253 
254  edge = req.add_edges();
255  edge->mutable_c0()->set_x(myBoundary.xmax());
256  edge->mutable_c0()->set_y(myBoundary.ymax());
257  edge->mutable_c1()->set_x(myBoundary.xmin());
258  edge->mutable_c1()->set_y(myBoundary.ymax());
259  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
260 
261  edge = req.add_edges();
262  edge->mutable_c0()->set_x(myBoundary.xmin());
263  edge->mutable_c0()->set_y(myBoundary.ymax());
264  edge->mutable_c1()->set_x(myBoundary.xmin());
265  edge->mutable_c1()->set_y(myBoundary.ymin());
266  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
267 
268 
269  hybridsim::Empty rpl;
270  ClientContext context;
271  Status st = myHybridsimStub->initScenario(&context, req, &rpl);
272  if (!st.ok()) {
273  throw ProcessError("Remote side could not initialize scenario!");
274  }
275 
276 }
277 void MSPModel_Remote::handleWalkingArea(MSEdge* msEdge, hybridsim::Scenario& scenario) {
278  MSLane* l = *(msEdge->getLanes().begin());
279 
280  const PositionVector shape = l->getShape();
281  if (shape.size() < 2) {//should never happen
282  return;
283  }
284 
285  handleShape(shape, scenario);
286 
287 
288  //close walking area
289  Position frst = *shape.begin();
290  Position scnd = *(shape.end() - 1);
291  hybridsim::Edge* edge = scenario.add_edges();
292  edge->mutable_c0()->set_x(frst.x());
293  edge->mutable_c0()->set_y(frst.y());
294  edge->mutable_c1()->set_x(scnd.x());
295  edge->mutable_c1()->set_y(scnd.y());
296  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
297 
298 
299 }
300 void MSPModel_Remote::handlePedestrianLane(MSLane* l, hybridsim::Scenario& scenario) {
301  double width = l->getWidth();
302  PositionVector centerLine = PositionVector(l->getShape());
303  if (centerLine.size() < 2) {//should never happen
304  return;
305  }
306 
307 
308  int fromId = myLastTransitionId++;
309  int toId = myLastTransitionId++;
310  edgesTransitionsMapping[&(l->getEdge())] = std::make_tuple(fromId, toId);
311  transitionsEdgesMapping[fromId] = &(l->getEdge());
312  transitionsEdgesMapping[toId] = &(l->getEdge());
313 
314  hybridsim::Edge_Type edgeType;
315  if (l->getEdge().isCrossing()) {
316  edgeType = hybridsim::Edge_Type_TRANSITION_HOLDOVER;
317  } else if (l->getEdge().isWalkingArea()) {
318  edgeType = hybridsim::Edge_Type_TRANSITION_INTERNAL;
319  } else {
320  edgeType = hybridsim::Edge_Type_TRANSITION;
321  }
322 
323  //start and end
324  Position frst = *centerLine.begin();
325  Position scnd = *(centerLine.begin() + 1);
326  makeStartOrEndTransition(frst, scnd, width, scenario, edgeType, fromId);
327  Position thrd = *(centerLine.end() - 1);
328  Position frth = *(centerLine.end() - 2);
329  makeStartOrEndTransition(thrd, frth, width, scenario, edgeType, toId);
330 
331  centerLine.move2side(-width / 2.);
332  handleShape(centerLine, scenario);
333  centerLine.move2side(width);
334  handleShape(centerLine, scenario);
335 
336 }
337 void
338 MSPModel_Remote::makeStartOrEndTransition(Position frst, Position scnd, double width, hybridsim::Scenario& scenario,
339  hybridsim::Edge_Type type, int id) {
340 
341  double dx = scnd.x() - frst.x();
342  double dy = scnd.y() - frst.y();
343  double length = sqrt(dx * dx + dy * dy);
344  dx /= length;
345  dy /= length;
346  double x0 = frst.x() - dy * width / 2.;
347  double y0 = frst.y() + dx * width / 2.;
348  double x1 = frst.x() + dy * width / 2.;
349  double y1 = frst.y() - dx * width / 2.;
350  hybridsim::Edge* edge = scenario.add_edges();
351  edge->mutable_c0()->set_x(x0);
352  edge->mutable_c0()->set_y(y0);
353  edge->mutable_c1()->set_x(x1);
354  edge->mutable_c1()->set_y(y1);
355  edge->set_type(type);
356  edge->set_id(id);
357 
358 }
359 void MSPModel_Remote::handleShape(const PositionVector& shape, hybridsim::Scenario& scenario) {
360  PositionVector::const_iterator it = shape.begin();
361  Position frst = *it;
362 
363  myBoundary.add(frst.x(), frst.y());
364  it++;
365  for (; it != shape.end(); it++) {
366  hybridsim::Edge* edge = scenario.add_edges();
367  edge->mutable_c0()->set_x(frst.x());
368  edge->mutable_c0()->set_y(frst.y());
369  edge->mutable_c1()->set_x((*it).x());
370  edge->mutable_c1()->set_y((*it).y());
371  edge->set_type(hybridsim::Edge_Type_OBSTACLE);
372  frst = *it;
373  myBoundary.add(frst.x(), frst.y());
374  }
375 }
376 
377 
378 
379 // ===========================================================================
380 // MSPModel_Remote::PState method definitions
381 // ===========================================================================
383  : myPerson(person), myPhi(0), myPosition(0, 0), myStage(stage) {
384 
385 
386 }
388 
389 }
391  return 0;
392 }
394  return myPosition;
395 }
397  return myPhi;
398 }
400  return 0;
401 }
403  return 0;
404 }
406  return nullptr;
407 }
408 void MSPModel_Remote::PState::setPosition(double x, double y) {
409  myPosition.set(x, y);
410 }
412  myPhi = phi;
413 }
415  return myStage;
416 }
418  return myPerson;
419 }
420 
421 bool
424 }
425 
PState(MSPerson *person, MSPerson::MSPersonStage_Walking *stage)
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:131
virtual void cleanupHelper()
Definition: MSPModel.h:100
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:125
MSEdge & getEdge() const
Returns the lane&#39;s edge.
Definition: MSLane.h:640
long long int SUMOTime
Definition: SUMOTime.h:36
is a pedestrian
MSLane * getFirstPedestrianLane(const MSEdge *const &edge)
double y() const
Returns the y-position.
Definition: Position.h:62
void makeStartOrEndTransition(Position position, Position scnd, double width, hybridsim::Scenario &scenario, hybridsim::Edge_Type type, int i)
static const int FORWARD
Definition: MSPModel.h:100
MSPModel_Remote(const OptionsCont &oc, MSNet *net)
double x() const
Returns the x-position.
Definition: Position.h:57
MSPerson::MSPersonStage_Walking * getStage()
const std::vector< MSLane * > & getLanes() const
Returns this edge&#39;s lanes.
Definition: MSEdge.h:162
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:165
SUMOTime DELTA_T
Definition: SUMOTime.cpp:35
const PositionVector & getShape() const
Returns this lane&#39;s shape.
Definition: MSLane.h:456
const std::string & getID() const
Returns the id.
Definition: Named.h:78
const MSEdge * getNextEdge(const MSPerson::MSPersonStage_Walking &stage) const override
return the list of internal edges if the pedestrian is on an intersection
std::map< int, const MSEdge * > transitionsEdgesMapping
void handlePedestrianLane(MSLane *pLane, hybridsim::Scenario &scenario)
void set(double x, double y)
set positions x and y
Definition: Position.h:87
const MSJunction * getToJunction() const
Definition: MSEdge.h:347
SUMOTime execute(SUMOTime time)
double getWidth() const
Returns the lane&#39;s width.
Definition: MSLane.h:530
The simulated network and simulation perfomer.
Definition: MSNet.h:84
A road/street connecting two junctions.
Definition: MSEdge.h:75
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
std::map< int, PState * > remoteIdPStateMapping
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
A list of positions.
MSEventControl * getBeginOfTimestepEvents()
Returns the event control for events executed at the begin of a time step.
Definition: MSNet.h:409
std::string getString(const std::string &name) const
Returns the string-value of the named option (only for Option_String)
SUMOTime getWaitingTime(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const override
return the time the person spent standing
virtual void addEvent(Command *operation, SUMOTime execTimeStep=-1)
Adds an Event.
SUMOTime getCurrentTimeStep() const
Returns the current simulation step.
Definition: MSNet.h:263
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:119
const std::string & getID() const
returns the id of the transportable
void remove(PedestrianState *state) override
remove the specified person from the pedestrian simulation
bool hasInternalLinks() const
return whether the network contains internal links
Definition: MSNet.h:605
static bool gUsingInternalLanes
Information whether the simulation regards internal lanes.
Definition: MSGlobals.h:69
void handleShape(const PositionVector &shape, hybridsim::Scenario &scenario)
double getArrivalPos() const
Definition: MSPerson.h:154
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:230
void setPosition(double x, double y)
const ConstMSEdgeVector & getRoute() const
Definition: MSPerson.h:168
void move2side(double amount)
move position vector to side using certain ammount
PedestrianState * add(MSPerson *person, MSPerson::MSPersonStage_Walking *stage, SUMOTime now) override
register the given person as a pedestrian
abstract base class for managing callbacks to retrieve various state information from the model ...
Definition: MSPModel.h:128
double getSpeed(const MSPerson::MSPersonStage_Walking &stage) const override
return the current speed of the person
Container for pedestrian state and individual position update function.
bool usingInternalLanes()
whether movements on intersections are modelled
const MSJunction * getFromJunction() const
Definition: MSEdge.h:343
Position getPosition(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const override
return the network coordinate of the person
bool moveToNextEdge(MSPerson *person, SUMOTime currentTime, MSEdge *nextInternal=nullptr)
move forward and return whether the person arrived
Definition: MSPerson.cpp:315
A storage for options typed value containers)
Definition: OptionsCont.h:92
std::unique_ptr< hybridsim::HybridSimulation::Stub > myHybridsimStub
const MSEdgeVector & getEdges() const
Returns loaded edges.
std::map< const MSEdge *, std::tuple< int, int > > edgesTransitionsMapping
static const int BACKWARD
Definition: MSPModel.h:104
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:244
MSPerson::MSPersonStage_Walking * myStage
MSEdgeControl & getEdgeControl()
Returns the edge control.
Definition: MSNet.h:359
const MSEdge * getNextRouteEdge() const
Definition: MSPerson.h:165
double getAngle(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const override
return the direction in which the person faces in degrees
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:79
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:137
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
Representation of a lane in the micro simulation.
Definition: MSLane.h:78
void cleanupHelper() override
double getEdgePos(const MSPerson::MSPersonStage_Walking &stage, SUMOTime now) const override
return the offset from the start of the current edge measured in its natural direction ...
void handleWalkingArea(MSEdge *msEdge, hybridsim::Scenario &scenario)
StageType getCurrentStageType() const
the current stage type of the transportable