1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2012-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 Helper.h
11 /// @author Robert Hilbrich
12 /// @author Leonhard Luecken
13 /// @date 15.09.2017
14 /// @version $Id$
15 ///
16 // C++ TraCI client API implementation
17 /****************************************************************************/
18 #ifndef Helper_h
19 #define Helper_h
20
21
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26
27 #include <vector>
28 #include <memory>
29 #include <libsumo/Subscription.h>
30 #include <microsim/MSNet.h>
31
32 // ===========================================================================
33 // class declarations
34 // ===========================================================================
35 class Position;
36 class PositionVector;
37 class RGBColor;
38 class MSEdge;
39 class MSPerson;
40
41 // ===========================================================================
42 // type definitions
43 // ===========================================================================
44 typedef std::map<const MSLane*, std::pair<double, double> > LaneCoverageInfo; // also declared in MSLane.h!
45
46 // ===========================================================================
47 // class definitions
48 // ===========================================================================
49
50 class LaneStoringVisitor {
51 public:
52 /// @brief Constructor
LaneStoringVisitor(std::set<std::string> & ids,const PositionVector & shape,const double range,const int domain)53 LaneStoringVisitor(std::set<std::string>& ids, const PositionVector& shape,
54 const double range, const int domain)
55 : myIDs(ids), myShape(shape), myRange(range), myDomain(domain) {}
56
57 /// @brief Destructor
~LaneStoringVisitor()58 ~LaneStoringVisitor() {}
59
60 /// @brief Adds the given object to the container
61 void add(const MSLane* const l) const;
62
63 /// @brief The container
64 std::set<std::string>& myIDs;
65 const PositionVector& myShape;
66 const double myRange;
67 const int myDomain;
68
69 private:
70 /// @brief invalidated copy constructor
71 LaneStoringVisitor(const LaneStoringVisitor& src);
72
73 /// @brief invalidated assignment operator
74 LaneStoringVisitor& operator=(const LaneStoringVisitor& src);
75 };
76
77 #define LANE_RTREE_QUAL RTree<MSLane*, MSLane, float, 2, LaneStoringVisitor>
78 template<>
RectSphericalVolume(Rect * a_rect)79 inline float LANE_RTREE_QUAL::RectSphericalVolume(Rect* a_rect) {
80 ASSERT(a_rect);
81 const float extent0 = a_rect->m_max[0] - a_rect->m_min[0];
82 const float extent1 = a_rect->m_max[1] - a_rect->m_min[1];
83 return .78539816f * (extent0 * extent0 + extent1 * extent1);
84 }
85
86 template<>
CombineRect(Rect * a_rectA,Rect * a_rectB)87 inline LANE_RTREE_QUAL::Rect LANE_RTREE_QUAL::CombineRect(Rect* a_rectA, Rect* a_rectB) {
88 ASSERT(a_rectA && a_rectB);
89 Rect newRect;
90 newRect.m_min[0] = rtree_min(a_rectA->m_min[0], a_rectB->m_min[0]);
91 newRect.m_max[0] = rtree_max(a_rectA->m_max[0], a_rectB->m_max[0]);
92 newRect.m_min[1] = rtree_min(a_rectA->m_min[1], a_rectB->m_min[1]);
93 newRect.m_max[1] = rtree_max(a_rectA->m_max[1], a_rectB->m_max[1]);
94 return newRect;
95 }
96
97 namespace libsumo {
98
99 /**
100 * @class Helper
101 * @brief C++ TraCI client API implementation
102 */
103 class Helper {
104 public:
105 static void subscribe(const int commandId, const std::string& id, const std::vector<int>& variables,
106 const double beginTime, const double endTime, const int contextDomain = 0, const double range = 0.);
107
108 static void handleSubscriptions(const SUMOTime t);
109
110 /// @brief helper functions
111 static TraCIPositionVector makeTraCIPositionVector(const PositionVector& positionVector);
112 static TraCIPosition makeTraCIPosition(const Position& position, const bool includeZ = false);
113 static Position makePosition(const TraCIPosition& position);
114
115 static PositionVector makePositionVector(const TraCIPositionVector& vector);
116 static TraCIColor makeTraCIColor(const RGBColor& color);
117 static RGBColor makeRGBColor(const TraCIColor& color);
118
119 static MSEdge* getEdge(const std::string& edgeID);
120 static const MSLane* getLaneChecking(const std::string& edgeID, int laneIndex, double pos);
121 static std::pair<MSLane*, double> convertCartesianToRoadMap(const Position& pos, const SUMOVehicleClass vClass);
122
123 static void findObjectShape(int domain, const std::string& id, PositionVector& shape);
124
125 static void collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into);
126
127 /// @brief Filter the given ID-Set (which was obtained from an R-Tree search)
128 /// according to the filters set by the subscription or firstly build the object ID list if
129 /// if the filters rather demand searching along the road network than considering a geometric range.
130 /// @param[in] s Subscription which holds the filter specification to be applied
131 /// @param[in/out] objIDs Set of object IDs that is to be filtered. Result is stored in place.
132 /// @note Currently this assumes that the objects are vehicles.
133 static void applySubscriptionFilters(const Subscription& s, std::set<std::string>& objIDs);
134
135 static void setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
136 int edgeOffset, ConstMSEdgeVector route, SUMOTime t);
137
138 static void setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
139 int edgeOffset, ConstMSEdgeVector route, SUMOTime t);
140
141 static void postProcessRemoteControl();
142
143 static void cleanup();
144
145 static void registerVehicleStateListener();
146
147 static const std::vector<std::string>& getVehicleStateChanges(const MSNet::VehicleState state);
148
149 static void clearVehicleStates();
150
151 /// @name functions for moveToXY
152 /// @{
153 static bool moveToXYMap(const Position& pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string& origID, const double angle,
154 double speed, const ConstMSEdgeVector& currentRoute, const int routePosition, MSLane* currentLane, double currentLanePos, bool onRoad,
155 double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset, ConstMSEdgeVector& edges);
156
157 static bool moveToXYMap_matchingRoutePosition(const Position& pos, const std::string& origID,
158 const ConstMSEdgeVector& currentRoute, int routeIndex,
159 double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset);
160
161 static bool findCloserLane(const MSEdge* edge, const Position& pos, double& bestDistance, MSLane** lane);
162
163 class LaneUtility {
164 public:
LaneUtility(double dist_,double perpendicularDist_,double lanePos_,double angleDiff_,bool ID_,bool onRoute_,bool sameEdge_,const MSEdge * prevEdge_,const MSEdge * nextEdge_)165 LaneUtility(double dist_, double perpendicularDist_, double lanePos_, double angleDiff_, bool ID_,
166 bool onRoute_, bool sameEdge_, const MSEdge* prevEdge_, const MSEdge* nextEdge_) :
167 dist(dist_), perpendicularDist(perpendicularDist_), lanePos(lanePos_), angleDiff(angleDiff_), ID(ID_),
168 onRoute(onRoute_), sameEdge(sameEdge_), prevEdge(prevEdge_), nextEdge(nextEdge_) {}
LaneUtility()169 LaneUtility() {}
~LaneUtility()170 ~LaneUtility() {}
171
172 double dist;
173 double perpendicularDist;
174 double lanePos;
175 double angleDiff;
176 bool ID;
177 bool onRoute;
178 bool sameEdge;
179 const MSEdge* prevEdge;
180 const MSEdge* nextEdge;
181 };
182 /// @}
183
184 class SubscriptionWrapper : public VariableWrapper {
185 public:
186 SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults& into, ContextSubscriptionResults& context);
187 void setContext(const std::string& refID);
188 bool wrapDouble(const std::string& objID, const int variable, const double value);
189 bool wrapInt(const std::string& objID, const int variable, const int value);
190 bool wrapString(const std::string& objID, const int variable, const std::string& value);
191 bool wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value);
192 bool wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value);
193 bool wrapColor(const std::string& objID, const int variable, const TraCIColor& value);
194 bool wrapRoadPosition(const std::string& objID, const int variable, const TraCIRoadPosition& value);
195 private:
196 SubscriptionResults myResults;
197 ContextSubscriptionResults myContextResults;
198 SubscriptionResults& myActiveResults;
199 private:
200 /// @brief Invalidated assignment operator
201 SubscriptionWrapper& operator=(const SubscriptionWrapper& s) = delete;
202 };
203
204 private:
205 static void handleSingleSubscription(const Subscription& s);
206
207 /// @brief Adds lane coverage information from newLaneCoverage into aggregatedLaneCoverage
208 /// @param[in/out] aggregatedLaneCoverage - aggregated lane coverage info, to which the new will be added
209 /// @param[in] newLaneCoverage - new lane coverage to be added
210 /// @todo Disjunct ranges are not handled (LaneCoverageInfo definition would need to allow several intervals per lane) but
211 /// the intermediate range is simply assimilated.
212 static void fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage);
213
214 private:
215 class VehicleStateListener : public MSNet::VehicleStateListener {
216 public:
217 void vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& info = "");
218 /// @brief Changes in the states of simulated vehicles
219 std::map<MSNet::VehicleState, std::vector<std::string> > myVehicleStateChanges;
220 };
221
222 /// @brief The list of known, still valid subscriptions
223 static std::vector<Subscription> mySubscriptions;
224
225 /// @brief Map of commandIds -> their executors; applicable if the executor applies to the method footprint
226 static std::map<int, std::shared_ptr<VariableWrapper> > myWrapper;
227
228 /// @brief Changes in the states of simulated vehicles
229 static VehicleStateListener myVehicleStateListener;
230
231 /// @brief A storage of objects
232 static std::map<int, NamedRTree*> myObjects;
233
234 /// @brief A storage of lanes
235 static LANE_RTREE_QUAL* myLaneTree;
236
237 static std::map<std::string, MSVehicle*> myRemoteControlledVehicles;
238 static std::map<std::string, MSPerson*> myRemoteControlledPersons;
239
240 /// @brief invalidated standard constructor
241 Helper() = delete;
242 };
243
244 }
245
246
247 #endif
248
249 /****************************************************************************/
250