49 #define FAR_AWAY 1000.0 60 for (MSLane::VehCont::const_iterator j = vehs.begin(); j != vehs.end(); ++j) {
62 myIDs.insert((*j)->getID());
71 for (
auto p : persons) {
73 myIDs.insert(p->getID());
101 std::map<int, NamedRTree*> Helper::myObjects;
103 std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
104 std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
112 for (
int i = 0; i < (int)positionVector.size(); ++i) {
113 tp.push_back(makeTraCIPosition(positionVector[i]));
122 for (
int i = 0; i < (int)vector.size(); i++) {
123 pv.push_back(
Position(vector[i].x, vector[i].y));
142 return RGBColor((
unsigned char)c.
r, (
unsigned char)c.
g, (
unsigned char)c.
b, (
unsigned char)c.
a);
147 Helper::makeTraCIPosition(
const Position& position) {
159 p.
set(tpos.
x, tpos.
y, tpos.
z);
165 Helper::getEdge(
const std::string& edgeID) {
168 throw TraCIException(
"Referenced edge '" + edgeID +
"' is not known.");
175 Helper::getLaneChecking(
const std::string& edgeID,
int laneIndex,
double pos) {
180 if (laneIndex < 0 || laneIndex >= (
int)edge->
getLanes().size()) {
184 if (pos < 0 || pos > lane->
getLength()) {
191 std::pair<MSLane*, double>
194 std::pair<MSLane*, double> result;
195 std::vector<std::string> allEdgeIds;
196 double minDistance = std::numeric_limits<double>::max();
199 for (std::vector<std::string>::iterator itId = allEdgeIds.begin(); itId != allEdgeIds.end(); itId++) {
201 for (std::vector<MSLane*>::const_iterator itLane = allLanes.begin(); itLane != allLanes.end(); itLane++) {
202 const double newDistance = (*itLane)->getShape().distance2D(pos);
203 if (newDistance < minDistance) {
204 minDistance = newDistance;
205 result.first = (*itLane);
210 result.second = result.first->getShape().nearest_offset_to_point2D(pos,
false);
216 for (std::map<int, NamedRTree*>::const_iterator i = myObjects.begin(); i != myObjects.end(); ++i) {
226 Helper::collectObjectsInRange(
int domain,
const PositionVector& shape,
double range, std::set<std::string>& into) {
228 if (myObjects.find(domain) == myObjects.end()) {
258 const float cmin[2] = {(float) b.
xmin(), (float) b.
ymin()};
259 const float cmax[2] = {(float) b.
xmax(), (float) b.
ymax()};
266 myObjects[domain]->Search(cmin, cmax, sv);
274 myLaneTree->Search(cmin, cmax, sv);
285 myRemoteControlledVehicles[v->
getID()] = v;
292 myRemoteControlledPersons[p->
getID()] = p;
298 Helper::postProcessRemoteControl() {
299 for (
auto& controlled : myRemoteControlledVehicles) {
301 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
303 WRITE_WARNING(
"Vehicle '" + controlled.first +
"' was removed though being controlled by TraCI");
306 myRemoteControlledVehicles.clear();
307 for (
auto& controlled : myRemoteControlledPersons) {
309 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
311 WRITE_WARNING(
"Person '" + controlled.first +
"' was removed though being controlled by TraCI");
314 myRemoteControlledPersons.clear();
319 Helper::moveToXYMap(
const Position& pos,
double maxRouteDistance,
bool mayLeaveNetwork,
const std::string& origID,
const double angle,
320 double speed,
const ConstMSEdgeVector& currentRoute,
const int routePosition,
MSLane* currentLane,
double currentLanePos,
bool onRoad,
323 const MSEdge*
const currentRouteEdge = currentRoute[routePosition];
324 std::set<std::string> into;
326 shape.push_back(pos);
329 std::map<MSLane*, LaneUtility> lane2utility;
331 for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
333 const MSEdge* prevEdge = 0;
334 const MSEdge* nextEdge = 0;
335 bool onRoute =
false;
339 #ifdef DEBUG_MOVEXY_ANGLE 340 std::cout <<
"Ego on normal" << std::endl;
346 ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
350 ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
351 onRoute = edgePos != currentRoute.end();
352 if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
358 nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? 0 : *(edgePos + 1);
359 #ifdef DEBUG_MOVEXY_ANGLE 360 std::cout <<
"normal:" << e->
getID() <<
" prev:" << prevEdge->
getID() <<
" next:";
362 std::cout << nextEdge->
getID();
364 std::cout << std::endl;
367 #ifdef DEBUG_MOVEXY_ANGLE 368 std::cout <<
"Ego on internal" << std::endl;
373 while (prevEdge != 0 && prevEdge->
isInternal()) {
376 prevEdge = l == 0 ? 0 : &l->
getEdge();
379 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
381 while (nextEdge != 0 && nextEdge->isInternal()) {
384 if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
385 onRoute = *(prevEdgePos + 1) == nextEdge;
387 #ifdef DEBUG_MOVEXY_ANGLE 388 std::cout <<
"internal:" << e->
getID() <<
" prev:" << prevEdge->
getID() <<
" next:" << nextEdge->getID() << std::endl;
394 const std::vector<MSLane*>& lanes = e->
getLanes();
395 const bool perpendicular =
false;
396 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
398 double langle = 180.;
400 double perpendicularDist =
FAR_AWAY;
419 if (mayLeaveNetwork && dist != perpendicularDist) {
424 #ifdef DEBUG_MOVEXY_ANGLE 425 std::cout << lane->
getID() <<
" lAngle:" << langle <<
" lLength=" << lane->
getLength()
426 <<
" angleDiff:" << angleDiff
428 <<
" pDist=" << perpendicularDist
430 <<
" dist2=" << dist2
435 dist2, perpendicularDist, off, angleDiff,
437 onRoute, sameEdge, prevEdge, nextEdge);
445 double bestValue = 0;
447 for (std::map<MSLane*, LaneUtility>::iterator i = lane2utility.begin(); i != lane2utility.end(); ++i) {
450 double distN = u.
dist > 999 ? -10 : 1. - (u.
dist / maxDist);
451 double angleDiffN = 1. - (u.
angleDiff / 180.);
452 double idN = u.
ID ? 1 : 0;
453 double onRouteN = u.
onRoute ? 1 : 0;
455 double value = (distN * .5
461 std::cout <<
" x; l:" << l->
getID() <<
" d:" << u.
dist <<
" dN:" << distN <<
" aD:" << angleDiffN <<
462 " ID:" << idN <<
" oRN:" << onRouteN <<
" sEN:" << sameEdgeN <<
" value:" << value << std::endl;
464 if (value > bestValue || bestLane == 0) {
477 const LaneUtility& u = lane2utility.find(bestLane)->second;
478 bestDistance = u.
dist;
483 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
484 routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
497 #ifdef DEBUG_MOVEXY_ANGLE 510 const std::vector<MSLane*>& lanes = edge->
getLanes();
511 bool newBest =
false;
512 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance >
POSITION_EPS; ++k) {
513 MSLane* candidateLane = *k;
516 std::cout <<
" b at lane " << candidateLane->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
518 if (dist < bestDistance) {
521 *lane = candidateLane;
529 Helper::moveToXYMap_matchingRoutePosition(
const Position& pos,
const std::string& origID,
531 double& bestDistance,
MSLane** lane,
double& lanePos,
int& routeOffset) {
538 for (
int i = routeIndex; i < (int)currentRoute.size(); ++i) {
539 const MSEdge* cand = currentRoute[i];
543 findCloserLane(internalCand, pos, bestDistance, lane);
546 if (findCloserLane(cand, pos, bestDistance, lane)) {
552 const MSEdge* next = currentRoute[routeIndex];
553 for (
int i = routeIndex; i > 0; --i) {
554 const MSEdge* cand = currentRoute[i];
559 findCloserLane(internalCand, pos, bestDistance, lane);
562 if (findCloserLane(cand, pos, bestDistance, lane)) {
572 std::cout <<
" b failed - no best route lane" << std::endl;
580 if (!(*lane)->getEdge().isInternal()) {
581 const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
582 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
591 (*lane)->interpolateGeometryPosToLanePos(
592 (*lane)->getShape().nearest_offset_to_point2D(pos,
false))));
595 std::cout <<
" b ok lane " << (*lane)->getID() <<
" lanePos:" << lanePos << std::endl;
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
double ymin() const
Returns minimum y-coordinate.
double xmax() const
Returns maximum x-coordinate.
MSEdge & getEdge() const
Returns the lane's edge.
Representation of a vehicle in the micro simulation.
double z() const
Returns the z-position.
#define CMD_GET_VEHICLE_VARIABLE
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
#define CMD_GET_INDUCTIONLOOP_VARIABLE
static void fill(RTREE &into)
Fills the given RTree with lane instances.
unsigned char alpha() const
Returns the alpha-amount of the color.
const double SUMO_const_laneWidth
double y() const
Returns the y-position.
#define CMD_GET_PERSON_VARIABLE
double x() const
Returns the x-position.
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
void visit(const LaneStoringVisitor &cont) const
Callback for visiting the lane when traversing an RTree.
double getLength() const
Returns the lane's length.
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
const PositionVector & getShape() const
Returns this lane's shape.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary...
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
unsigned char blue() const
Returns the blue-amount of the color.
#define CMD_GET_POLYGON_VARIABLE
std::vector< const MSEdge * > ConstMSEdgeVector
const std::string & getID() const
Returns the id.
void set(double x, double y)
set positions x and y
double getLength() const
return the length of the edge
std::vector< MSTransportable * > getSortedPersons(SUMOTime timestep, bool includeRiding=false) const
Returns this edge's persons sorted by pos.
A class that stores a 2D geometrical boundary.
#define WRITE_WARNING(msg)
std::vector< std::string > getEdgeNames() const
Returns the list of names of all known edges.
A road/street connecting two junctions.
static double naviDegree(const double angle)
Influencer & getInfluencer()
Returns the velocity/lane influencer.
A point in 2D or 3D with translation and scaling methods.
#define CMD_GET_POI_VARIABLE
#define CMD_GET_LANE_VARIABLE
void add(const MSLane *const l) const
Adds the given object to the container.
double xmin() const
Returns minimum x-coordinate.
const std::string & getID() const
returns the id of the transportable
#define CMD_GET_EDGE_VARIABLE
Boundary & grow(double by)
extends the boundary by the given amount
#define CMD_GET_JUNCTION_VARIABLE
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
bool isInternal() const
return whether this edge is an internal edge
std::vector< MSVehicle * > VehCont
Container for vehicles.
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal) const
#define INVALID_DOUBLE_VALUE
Allows to store the object; used as context while traveling the rtree in TraCI.
std::set< std::string > & myIDs
The container.
Influencer & getInfluencer()
Returns the velocity/lane influencer.
const PositionVector & myShape
unsigned char green() const
Returns the green-amount of the color.
const std::string getParameter(const std::string &key, const std::string &defaultValue="") const
Returns the value for a given key.
const std::string SUMO_PARAM_ORIGID
const MSEdgeVector & getSuccessors() const
Returns the following edges.
unsigned char red() const
Returns the red-amount of the color.
MSEdgeControl & getEdgeControl()
Returns the edge control.
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
double ymax() const
Returns maximum y-coordinate.
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
const std::string & getID() const
Returns the name of the vehicle.
Representation of a lane in the micro simulation.