PositionVector NIXMLEdgesHandler::tryGetShape(const SUMOSAXAttributes& attrs) { if (!attrs.hasAttribute(SUMO_ATTR_SHAPE)) { return myShape; } // try to build shape bool ok = true; if (!attrs.hasAttribute(SUMO_ATTR_SHAPE)) { myReinitKeepEdgeShape = false; return PositionVector(); } PositionVector shape = attrs.getOpt<PositionVector>(SUMO_ATTR_SHAPE, 0, ok, PositionVector()); if (!NILoader::transformCoordinates(shape)) { WRITE_ERROR("Unable to project coordinates for edge '" + myCurrentID + "'."); } myReinitKeepEdgeShape = myKeepEdgeShape; return shape; }
// =========================================================================== // method definitions // =========================================================================== PositionVector GeomConvHelper::parseShapeReporting(const std::string& shpdef, const std::string& objecttype, const char* objectid, bool& ok, bool allowEmpty, bool report) { if (shpdef == "") { if (!allowEmpty) { emitError(report, "Shape", objecttype, objectid, "the shape is empty"); ok = false; } return PositionVector(); } StringTokenizer st(shpdef, " "); PositionVector shape; while (st.hasNext()) { StringTokenizer pos(st.next(), ","); if (pos.size() != 2 && pos.size() != 3) { emitError(report, "Shape", objecttype, objectid, "the position is neither x,y nor x,y,z"); ok = false; return PositionVector(); } try { SUMOReal x = TplConvert::_2SUMOReal(pos.next().c_str()); SUMOReal y = TplConvert::_2SUMOReal(pos.next().c_str()); if (pos.size() == 2) { shape.push_back(Position(x, y)); } else { SUMOReal z = TplConvert::_2SUMOReal(pos.next().c_str()); shape.push_back(Position(x, y, z)); } } catch (NumberFormatException&) { emitError(report, "Shape", objecttype, objectid, "not numeric position entry"); ok = false; return PositionVector(); } catch (EmptyData&) { emitError(report, "Shape", objecttype, objectid, "empty position entry"); ok = false; return PositionVector(); } } return shape; }
bool NGRandomNetBuilder::canConnect(NGNode* baseNode, NGNode* newNode) { bool connectable = true; const PositionVector n(baseNode->getPosition(), newNode->getPosition()); // check for range between Basenode and Newnode if (connectable) { SUMOReal dist = n.length(); if ((dist < myMinDistance) || (dist > myMaxDistance)) { connectable = false; } } // check for angle restrictions if (connectable) { connectable = checkAngles(baseNode); } if (connectable) { connectable = checkAngles(newNode); } // check for intersections and range restrictions with outer links if (connectable) { NGEdgeList::iterator li; li = myOuterLinks.begin(); while (connectable && (li != myOuterLinks.end())) { // check intersection only if links don't share a node const NGNode* const start = (*li)->getStartNode(); const NGNode* const end = (*li)->getEndNode(); const Position& p1 = start->getPosition(); const Position& p2 = end->getPosition(); if ((baseNode != start) && (baseNode != end) && (newNode != start) && (newNode != end)) { connectable = !n.intersects(p1, p2); } // check NewNode-To-Links distance only, if NewNode isn't part of link if (connectable && (newNode != start) && (newNode != end)) { const SUMOReal offset = GeomHelper::nearest_offset_on_line_to_point2D(p1, p2, n[1]); if (offset != GeomHelper::INVALID_OFFSET) { const Position p = PositionVector(p1, p2).positionAtOffset2D(offset); const SUMOReal dist = p.distanceTo2D(n[1]); if (dist < myMinDistance) { connectable = false; } } } ++li; } } return connectable; }
void NIImporter_VISUM::parse_AreaSubPartElement() { SUMOLong id = TplConvert::_2long(myLineParser.get("TFLAECHEID").c_str()); SUMOLong edgeid = TplConvert::_2long(myLineParser.get("KANTEID").c_str()); if (myEdges.find(edgeid) == myEdges.end()) { WRITE_ERROR("Unknown edge in TEILFLAECHENELEMENT"); return; } std::string dir = myLineParser.get("RICHTUNG"); // get index (unused) // std::string indexS = NBHelpers::normalIDRepresentation(myLineParser.get("INDEX")); // int index = -1; // try { // index = TplConvert::_2int(indexS.c_str()) - 1; // } catch (NumberFormatException&) { // WRITE_ERROR("An index for a TEILFLAECHENELEMENT is not numeric (id='" + toString(id) + "')."); // return; // } PositionVector shape; shape.push_back(myPoints[myEdges[edgeid].first]); shape.push_back(myPoints[myEdges[edgeid].second]); if (dir.length() > 0 && dir[0] == '1') { shape = shape.reverse(); } if (mySubPartsAreas.find(id) == mySubPartsAreas.end()) { WRITE_ERROR("Unkown are for area part '" + myCurrentID + "'."); return; } const std::vector<SUMOLong>& areas = mySubPartsAreas.find(id)->second; for (std::vector<SUMOLong>::const_iterator i = areas.begin(); i != areas.end(); ++i) { NBDistrict* d = myShapeDistrictMap[*i]; if (d == 0) { continue; } if (myDistrictShapes.find(d) == myDistrictShapes.end()) { myDistrictShapes[d] = PositionVector(); } if (dir.length() > 0 && dir[0] == '1') { myDistrictShapes[d].push_back(myPoints[myEdges[edgeid].second]); myDistrictShapes[d].push_back(myPoints[myEdges[edgeid].first]); } else { myDistrictShapes[d].push_back(myPoints[myEdges[edgeid].first]); myDistrictShapes[d].push_back(myPoints[myEdges[edgeid].second]); } } }
// ---- the root/junction - element void NLHandler::openJunction(const SUMOSAXAttributes& attrs) { myCurrentIsBroken = false; bool ok = true; // get the id, report an error if not given or empty... std::string id = attrs.get<std::string>(SUMO_ATTR_ID, 0, ok); if (!ok) { myCurrentIsBroken = true; return; } PositionVector shape; if (attrs.hasAttribute(SUMO_ATTR_SHAPE)) { // inner junctions have no shape shape = attrs.getOpt<PositionVector>(SUMO_ATTR_SHAPE, id.c_str(), ok, PositionVector()); } SUMOReal x = attrs.get<SUMOReal>(SUMO_ATTR_X, id.c_str(), ok); SUMOReal y = attrs.get<SUMOReal>(SUMO_ATTR_Y, id.c_str(), ok); bool typeOK = true; SumoXMLNodeType type = attrs.getNodeType(typeOK); if (!typeOK) { WRITE_ERROR("An unknown or invalid junction type occured in junction '" + id + "'."); ok = false; } std::string key = attrs.getOpt<std::string>(SUMO_ATTR_KEY, id.c_str(), ok, ""); // incoming lanes std::vector<MSLane*> incomingLanes; parseLanes(id, attrs.getStringSecure(SUMO_ATTR_INCLANES, ""), incomingLanes, ok); // internal lanes std::vector<MSLane*> internalLanes; #ifdef HAVE_INTERNAL_LANES if (MSGlobals::gUsingInternalLanes) { parseLanes(id, attrs.getStringSecure(SUMO_ATTR_INTLANES, ""), internalLanes, ok); } #endif if (!ok) { myCurrentIsBroken = true; } else { try { myJunctionControlBuilder.openJunction(id, key, type, x, y, shape, incomingLanes, internalLanes); } catch (InvalidArgument& e) { WRITE_ERROR(e.what() + std::string("\n Can not build according junction.")); myCurrentIsBroken = true; } } }
PositionVector NIVissimConnectionCluster::getOutgoingContinuationGeometry(NIVissimEdge* e) const { // collect connection where this edge is the outgoing one std::vector<NIVissimConnection*> edgeIsOutgoing; for (std::vector<int>::const_iterator i = myConnections.begin(); i != myConnections.end(); i++) { NIVissimConnection* c = NIVissimConnection::dictionary(*i); if (c->getToEdgeID() == e->getID()) { edgeIsOutgoing.push_back(c); } } // if (edgeIsOutgoing.size() == 0) { return PositionVector(); } // sort connected edges in same direction sort(edgeIsOutgoing.begin(), edgeIsOutgoing.end(), same_direction_sorter(e->getGeometry().beginEndAngle())); NIVissimConnection* c = *(edgeIsOutgoing.begin()); return c->getGeometry(); }
bool NIXMLEdgesHandler::setNodes(const SUMOSAXAttributes& attrs) { // the names and the coordinates of the beginning and the end node // may be found, try bool ok = true; std::string begNodeID = myIsUpdate ? myCurrentEdge->getFromNode()->getID() : ""; std::string endNodeID = myIsUpdate ? myCurrentEdge->getToNode()->getID() : ""; std::string oldBegID = begNodeID; std::string oldEndID = endNodeID; if (attrs.hasAttribute(SUMO_ATTR_FROM)) { begNodeID = attrs.get<std::string>(SUMO_ATTR_FROM, 0, ok); } else if (!myIsUpdate) { WRITE_ERROR("The from-node is not given for edge '" + myCurrentID + "'."); ok = false; } if (attrs.hasAttribute(SUMO_ATTR_TO)) { endNodeID = attrs.get<std::string>(SUMO_ATTR_TO, 0, ok); } else if (!myIsUpdate) { WRITE_ERROR("The to-node is not given for edge '" + myCurrentID + "'."); ok = false; } if (!ok) { return false; } myFromNode = myNodeCont.retrieve(begNodeID); myToNode = myNodeCont.retrieve(endNodeID); if (myFromNode == 0) { WRITE_ERROR("Edge's '" + myCurrentID + "' from-node '" + begNodeID + "' is not known."); } if (myToNode == 0) { WRITE_ERROR("Edge's '" + myCurrentID + "' to-node '" + endNodeID + "' is not known."); } if (myFromNode != 0 && myToNode != 0) { if (myIsUpdate && (myFromNode->getID() != oldBegID || myToNode->getID() != oldEndID)) { myShape = PositionVector(); } } return myFromNode != 0 && myToNode != 0; }
if (ret == "") { throw EmptyData(); } return ret; } template<> const RGBColor invalid_return<RGBColor>::value = RGBColor(); template<> const std::string invalid_return<RGBColor>::type = "color"; template<> RGBColor SUMOSAXAttributes::getInternal(const int /* attr */) const { return getColor(); } template<> const PositionVector invalid_return<PositionVector>::value = PositionVector(); template<> const std::string invalid_return<PositionVector>::type = "PositionVector"; template<> PositionVector SUMOSAXAttributes::getInternal(const int attr) const { return getShape(attr); } template<> const Boundary invalid_return<Boundary>::value = Boundary(); template<> const std::string invalid_return<Boundary>::type = "Boundary"; template<> Boundary SUMOSAXAttributes::getInternal(const int attr) const { return getBoundary(attr); }
void NIXMLEdgesHandler::addEdge(const SUMOSAXAttributes& attrs) { myIsUpdate = false; bool ok = true; // initialise the edge myCurrentEdge = 0; mySplits.clear(); // get the id, report an error if not given or empty... myCurrentID = attrs.get<std::string>(SUMO_ATTR_ID, 0, ok); if (!ok) { return; } myCurrentEdge = myEdgeCont.retrieve(myCurrentID); // check deprecated (unused) attributes // use default values, first myCurrentSpeed = myTypeCont.getSpeed(""); myCurrentPriority = myTypeCont.getPriority(""); myCurrentLaneNo = myTypeCont.getNumLanes(""); myPermissions = myTypeCont.getPermissions(""); myCurrentWidth = myTypeCont.getWidth(""); myCurrentOffset = NBEdge::UNSPECIFIED_OFFSET; myCurrentType = ""; myShape = PositionVector(); myLanesSpread = LANESPREAD_RIGHT; myLength = NBEdge::UNSPECIFIED_LOADED_LENGTH; myCurrentStreetName = ""; myReinitKeepEdgeShape = false; // check whether a type's values shall be used if (attrs.hasAttribute(SUMO_ATTR_TYPE)) { myCurrentType = attrs.get<std::string>(SUMO_ATTR_TYPE, myCurrentID.c_str(), ok); if (!ok) { return; } if (!myTypeCont.knows(myCurrentType)) { WRITE_ERROR("Type '" + myCurrentType + "' used by edge '" + myCurrentID + "' was not defined."); return; } myCurrentSpeed = myTypeCont.getSpeed(myCurrentType); myCurrentPriority = myTypeCont.getPriority(myCurrentType); myCurrentLaneNo = myTypeCont.getNumLanes(myCurrentType); myPermissions = myTypeCont.getPermissions(myCurrentType); myCurrentWidth = myTypeCont.getWidth(myCurrentType); } // use values from the edge to overwrite if existing, then if (myCurrentEdge != 0) { myIsUpdate = true; if (!myHaveReportedAboutOverwriting) { WRITE_MESSAGE("Duplicate edge id occured ('" + myCurrentID + "'); assuming overwriting is wished."); myHaveReportedAboutOverwriting = true; } if (attrs.getOpt<bool>(SUMO_ATTR_REMOVE, myCurrentID.c_str(), ok, false)) { myEdgeCont.erase(myDistrictCont, myCurrentEdge); myCurrentEdge = 0; return; } myCurrentSpeed = myCurrentEdge->getSpeed(); myCurrentPriority = myCurrentEdge->getPriority(); myCurrentLaneNo = myCurrentEdge->getNumLanes(); myCurrentType = myCurrentEdge->getTypeID(); myPermissions = myCurrentEdge->getPermissions(); if (!myCurrentEdge->hasDefaultGeometry()) { myShape = myCurrentEdge->getGeometry(); myReinitKeepEdgeShape = true; } myCurrentWidth = myCurrentEdge->getLaneWidth(); myCurrentOffset = myCurrentEdge->getOffset(); myLanesSpread = myCurrentEdge->getLaneSpreadFunction(); if (myCurrentEdge->hasLoadedLength()) { myLength = myCurrentEdge->getLoadedLength(); } myCurrentStreetName = myCurrentEdge->getStreetName(); } // speed, priority and the number of lanes have now default values; // try to read the real values from the file if (attrs.hasAttribute(SUMO_ATTR_SPEED)) { myCurrentSpeed = attrs.get<SUMOReal>(SUMO_ATTR_SPEED, myCurrentID.c_str(), ok); } if (myOptions.getBool("speed-in-kmh")) { myCurrentSpeed = myCurrentSpeed / (SUMOReal) 3.6; } // try to get the number of lanes if (attrs.hasAttribute(SUMO_ATTR_NUMLANES)) { myCurrentLaneNo = attrs.get<int>(SUMO_ATTR_NUMLANES, myCurrentID.c_str(), ok); } // try to get the priority if (attrs.hasAttribute(SUMO_ATTR_PRIORITY)) { myCurrentPriority = attrs.get<int>(SUMO_ATTR_PRIORITY, myCurrentID.c_str(), ok); } // try to get the width if (attrs.hasAttribute(SUMO_ATTR_WIDTH)) { myCurrentWidth = attrs.get<SUMOReal>(SUMO_ATTR_WIDTH, myCurrentID.c_str(), ok); } // try to get the width if (attrs.hasAttribute(SUMO_ATTR_ENDOFFSET)) { myCurrentOffset = attrs.get<SUMOReal>(SUMO_ATTR_ENDOFFSET, myCurrentID.c_str(), ok); } // try to get the street name myCurrentStreetName = attrs.getOpt<std::string>(SUMO_ATTR_NAME, myCurrentID.c_str(), ok, myCurrentStreetName); // try to get the allowed/disallowed classes if (attrs.hasAttribute(SUMO_ATTR_ALLOW) || attrs.hasAttribute(SUMO_ATTR_DISALLOW)) { std::string allowS = attrs.hasAttribute(SUMO_ATTR_ALLOW) ? attrs.getStringSecure(SUMO_ATTR_ALLOW, "") : ""; std::string disallowS = attrs.hasAttribute(SUMO_ATTR_DISALLOW) ? attrs.getStringSecure(SUMO_ATTR_DISALLOW, "") : ""; // XXX matter of interpretation: should updated permissions replace or extend previously set permissions? myPermissions = parseVehicleClasses(allowS, disallowS); } // try to set the nodes if (!setNodes(attrs)) { // return if this failed return; } // try to get the shape myShape = tryGetShape(attrs); // try to get the spread type myLanesSpread = tryGetLaneSpread(attrs); // try to get the length myLength = attrs.getOpt<SUMOReal>(SUMO_ATTR_LENGTH, myCurrentID.c_str(), ok, myLength); // insert the parsed edge into the edges map if (!ok) { return; } // check whether a previously defined edge shall be overwritten if (myCurrentEdge != 0) { myCurrentEdge->reinit(myFromNode, myToNode, myCurrentType, myCurrentSpeed, myCurrentLaneNo, myCurrentPriority, myShape, myCurrentWidth, myCurrentOffset, myCurrentStreetName, myLanesSpread, myReinitKeepEdgeShape); } else { // the edge must be allocated in dependence to whether a shape is given if (myShape.size() == 0) { myCurrentEdge = new NBEdge(myCurrentID, myFromNode, myToNode, myCurrentType, myCurrentSpeed, myCurrentLaneNo, myCurrentPriority, myCurrentWidth, myCurrentOffset, myCurrentStreetName, myLanesSpread); } else { myCurrentEdge = new NBEdge(myCurrentID, myFromNode, myToNode, myCurrentType, myCurrentSpeed, myCurrentLaneNo, myCurrentPriority, myCurrentWidth, myCurrentOffset, myShape, myCurrentStreetName, myLanesSpread, myKeepEdgeShape); } } myCurrentEdge->setLoadedLength(myLength); myCurrentEdge->setPermissions(myPermissions); }
void PCLoaderVisum::load(const std::string& file, OptionsCont& oc, PCPolyContainer& toFill, PCTypeMap& tm) { GeoConvHelper& geoConvHelper = GeoConvHelper::getProcessing(); std::string what; std::map<long, Position> punkte; std::map<long, PositionVector> kanten; std::map<long, PositionVector> teilflaechen; std::map<long, long> flaechenelemente; NamedColumnsParser lineParser; LineReader lr(file); while (lr.hasMore()) { std::string line = lr.readLine(); // reset if current is over if (line.length() == 0 || line[0] == '*' || line[0] == '$') { what = ""; } // read items if (what == "$PUNKT") { lineParser.parseLine(line); long id = TplConvert<char>::_2long(lineParser.get("ID").c_str()); SUMOReal x = TplConvert<char>::_2SUMOReal(lineParser.get("XKOORD").c_str()); SUMOReal y = TplConvert<char>::_2SUMOReal(lineParser.get("YKOORD").c_str()); Position pos(x, y); if (!geoConvHelper.x2cartesian(pos)) { WRITE_WARNING("Unable to project coordinates for point '" + toString(id) + "'."); } punkte[id] = pos; continue; } else if (what == "$KANTE") { lineParser.parseLine(line); long id = TplConvert<char>::_2long(lineParser.get("ID").c_str()); long fromID = TplConvert<char>::_2long(lineParser.get("VONPUNKTID").c_str()); long toID = TplConvert<char>::_2long(lineParser.get("NACHPUNKTID").c_str()); PositionVector vec; vec.push_back(punkte[fromID]); vec.push_back(punkte[toID]); kanten[id] = vec; continue; } else if (what == "$ZWISCHENPUNKT") { lineParser.parseLine(line); long id = TplConvert<char>::_2long(lineParser.get("KANTEID").c_str()); int index = TplConvert<char>::_2int(lineParser.get("INDEX").c_str()); SUMOReal x = TplConvert<char>::_2SUMOReal(lineParser.get("XKOORD").c_str()); SUMOReal y = TplConvert<char>::_2SUMOReal(lineParser.get("YKOORD").c_str()); Position pos(x, y); if (!geoConvHelper.x2cartesian(pos)) { WRITE_WARNING("Unable to project coordinates for edge '" + toString(id) + "'."); } kanten[id].insertAt(index, pos); continue; } else if (what == "$TEILFLAECHENELEMENT") { lineParser.parseLine(line); long id = TplConvert<char>::_2long(lineParser.get("TFLAECHEID").c_str()); int index = TplConvert<char>::_2int(lineParser.get("INDEX").c_str()); index = 0; /// hmmmm - assume it's sorted... long kid = TplConvert<char>::_2long(lineParser.get("KANTEID").c_str()); int dir = TplConvert<char>::_2int(lineParser.get("RICHTUNG").c_str()); if (teilflaechen.find(id) == teilflaechen.end()) { teilflaechen[id] = PositionVector(); } if (dir == 0) { for (int i = 0; i < (int) kanten[kid].size(); ++i) { teilflaechen[id].push_back_noDoublePos(kanten[kid][i]); } } else { for (int i = (int) kanten[kid].size() - 1; i >= 0; --i) { teilflaechen[id].push_back_noDoublePos(kanten[kid][i]); } } continue; } else if (what == "$FLAECHENELEMENT") { lineParser.parseLine(line); long id = TplConvert<char>::_2long(lineParser.get("FLAECHEID").c_str()); long tid = TplConvert<char>::_2long(lineParser.get("TFLAECHEID").c_str()); int enklave = TplConvert<char>::_2int(lineParser.get("ENKLAVE").c_str()); // !!! unused enklave = 0; flaechenelemente[id] = tid; continue; } // set if read if (line[0] == '$') { what = ""; if (line.find("$PUNKT") == 0) { what = "$PUNKT"; } else if (line.find("$KANTE") == 0) { what = "$KANTE"; } else if (line.find("$ZWISCHENPUNKT") == 0) { what = "$ZWISCHENPUNKT"; } else if (line.find("$TEILFLAECHENELEMENT") == 0) { what = "$TEILFLAECHENELEMENT"; } else if (line.find("$FLAECHENELEMENT") == 0) { what = "$FLAECHENELEMENT"; } if (what != "") { lineParser.reinit(line.substr(what.length() + 1)); } } } // do some more sane job... RGBColor c = RGBColor::parseColor(oc.getString("color")); std::map<std::string, std::string> typemap; // load the pois/polys lr.reinit(); bool parsingCategories = false; bool parsingPOIs = false; bool parsingDistrictsDirectly = false; PositionVector vec; std::string polyType, lastID; bool first = true; while (lr.hasMore()) { std::string line = lr.readLine(); // do not parse empty lines if (line.length() == 0) { continue; } // do not parse comment lines if (line[0] == '*') { continue; } if (line[0] == '$') { // reset parsing on new entry type parsingCategories = false; parsingPOIs = false; parsingDistrictsDirectly = false; polyType = ""; } if (parsingCategories) { // parse the category StringTokenizer st(line, ";"); std::string catid = st.next(); std::string catname = st.next(); typemap[catid] = catname; } if (parsingPOIs) { // parse the poi // $POI:Nr;CATID;CODE;NAME;Kommentar;XKoord;YKoord; StringTokenizer st(line, ";"); std::string num = st.next(); std::string catid = st.next(); std::string code = st.next(); std::string name = st.next(); std::string comment = st.next(); std::string xpos = st.next(); std::string ypos = st.next(); // process read values SUMOReal x = TplConvert<char>::_2SUMOReal(xpos.c_str()); SUMOReal y = TplConvert<char>::_2SUMOReal(ypos.c_str()); Position pos(x, y); if (!geoConvHelper.x2cartesian(pos)) { WRITE_WARNING("Unable to project coordinates for POI '" + num + "'."); } std::string type = typemap[catid]; // check the poi name = num; // patch the values bool discard = oc.getBool("discard"); int layer = oc.getInt("layer"); RGBColor color; if (tm.has(type)) { const PCTypeMap::TypeDef& def = tm.get(type); name = def.prefix + name; type = def.id; color = RGBColor::parseColor(def.color); discard = def.discard; layer = def.layer; } else { name = oc.getString("prefix") + name; type = oc.getString("type"); color = c; } if (!discard) { PointOfInterest* poi = new PointOfInterest(name, type, pos, color); if (!toFill.insert(name, poi, layer)) { WRITE_ERROR("POI '" + name + "' could not been added."); delete poi; } } } // poly if (polyType != "") { StringTokenizer st(line, ";"); std::string id = st.next(); std::string type; if (!first && lastID != id) { // we have parsed a polygon completely RGBColor color; int layer = oc.getInt("layer"); bool discard = oc.getBool("discard"); if (tm.has(polyType)) { const PCTypeMap::TypeDef& def = tm.get(polyType); id = def.prefix + id; type = def.id; color = RGBColor::parseColor(def.color); discard = def.discard; layer = def.layer; } else { id = oc.getString("prefix") + id; type = oc.getString("type"); color = c; } if (!discard) { Polygon* poly = new Polygon(id, type, color, vec, false); if (!toFill.insert(id, poly, 1)) { WRITE_ERROR("Polygon '" + id + "' could not been added."); delete poly; } } vec.clear(); } lastID = id; first = false; // parse current poly std::string index = st.next(); std::string xpos = st.next(); std::string ypos = st.next(); Position pos2D((SUMOReal) atof(xpos.c_str()), (SUMOReal) atof(ypos.c_str())); if (!geoConvHelper.x2cartesian(pos2D)) { WRITE_WARNING("Unable to project coordinates for polygon '" + id + "'."); } vec.push_back(pos2D); } // district refering a shape if (parsingDistrictsDirectly) { //$BEZIRK:NR CODE NAME TYPNR XKOORD YKOORD FLAECHEID BEZART IVANTEIL_Q IVANTEIL_Z OEVANTEIL METHODEANBANTEILE ZWERT1 ZWERT2 ZWERT3 ISTINAUSWAHL OBEZNR NOM_COM COD_COM StringTokenizer st(line, ";"); std::string num = st.next(); std::string code = st.next(); std::string name = st.next(); st.next(); // typntr std::string xpos = st.next(); std::string ypos = st.next(); long id = TplConvert<char>::_2long(st.next().c_str()); // patch the values std::string type = "district"; name = num; bool discard = oc.getBool("discard"); int layer = oc.getInt("layer"); RGBColor color; if (tm.has(type)) { const PCTypeMap::TypeDef& def = tm.get(type); name = def.prefix + name; type = def.id; color = RGBColor::parseColor(def.color); discard = def.discard; layer = def.layer; } else { name = oc.getString("prefix") + name; type = oc.getString("type"); color = c; } if (!discard) { if (teilflaechen[flaechenelemente[id]].size() > 0) { Polygon* poly = new Polygon(name, type, color, teilflaechen[flaechenelemente[id]], false); if (!toFill.insert(name, poly, layer)) { WRITE_ERROR("Polygon '" + name + "' could not been added."); delete poly; } } else { SUMOReal x = TplConvert<char>::_2SUMOReal(xpos.c_str()); SUMOReal y = TplConvert<char>::_2SUMOReal(ypos.c_str()); Position pos(x, y); if (!geoConvHelper.x2cartesian(pos)) { WRITE_WARNING("Unable to project coordinates for POI '" + name + "'."); } PointOfInterest* poi = new PointOfInterest(name, type, pos, color); if (!toFill.insert(name, poi, layer)) { WRITE_ERROR("POI '" + name + "' could not been added."); delete poi; } } } } if (line.find("$POIKATEGORIEDEF:") == 0 || line.find("$POIKATEGORIE:") == 0) { // ok, got categories, begin parsing from next line parsingCategories = true; } if (line.find("$POI:") == 0) { // ok, got pois, begin parsing from next line parsingPOIs = true; } if (line.find("$BEZIRK") == 0 && line.find("FLAECHEID") != std::string::npos) { // ok, have a district header, and it seems like districts would reference shapes... parsingDistrictsDirectly = true; } if (line.find("$BEZIRKPOLY") != std::string::npos) { polyType = "district"; } if (line.find("$GEBIETPOLY") != std::string::npos) { polyType = "area"; } } }
void NIImporter_SUMO::addEdge(const SUMOSAXAttributes& attrs) { // get the id, report an error if not given or empty... bool ok = true; std::string id = attrs.get<std::string>(SUMO_ATTR_ID, 0, ok); if (!ok) { return; } myCurrentEdge = new EdgeAttrs(); myCurrentEdge->builtEdge = 0; myCurrentEdge->id = id; // get the function myCurrentEdge->func = attrs.getEdgeFunc(ok); if (myCurrentEdge->func == EDGEFUNC_CROSSING) { // add the crossing but don't do anything else Crossing c; c.edgeID = id; SUMOSAXAttributes::parseStringVector(attrs.get<std::string>(SUMO_ATTR_CROSSING_EDGES, 0, ok), c.crossingEdges); myPedestrianCrossings[SUMOXMLDefinitions::getJunctionIDFromInternalEdge(id)].push_back(c); return; } else if (myCurrentEdge->func == EDGEFUNC_INTERNAL || myCurrentEdge->func == EDGEFUNC_WALKINGAREA) { return; // skip internal edges } // get the type myCurrentEdge->type = attrs.getOpt<std::string>(SUMO_ATTR_TYPE, id.c_str(), ok, ""); // get the origin and the destination node myCurrentEdge->fromNode = attrs.getOpt<std::string>(SUMO_ATTR_FROM, id.c_str(), ok, ""); myCurrentEdge->toNode = attrs.getOpt<std::string>(SUMO_ATTR_TO, id.c_str(), ok, ""); myCurrentEdge->priority = attrs.getOpt<int>(SUMO_ATTR_PRIORITY, id.c_str(), ok, -1); myCurrentEdge->type = attrs.getOpt<std::string>(SUMO_ATTR_TYPE, id.c_str(), ok, ""); myCurrentEdge->shape = attrs.getOpt<PositionVector>(SUMO_ATTR_SHAPE, id.c_str(), ok, PositionVector()); NBNetBuilder::transformCoordinates(myCurrentEdge->shape, true, myLocation); myCurrentEdge->length = attrs.getOpt<SUMOReal>(SUMO_ATTR_LENGTH, id.c_str(), ok, NBEdge::UNSPECIFIED_LOADED_LENGTH); myCurrentEdge->maxSpeed = 0; myCurrentEdge->streetName = attrs.getOpt<std::string>(SUMO_ATTR_NAME, id.c_str(), ok, ""); if (myCurrentEdge->streetName != "" && OptionsCont::getOptions().isDefault("output.street-names")) { OptionsCont::getOptions().set("output.street-names", "true"); } std::string lsfS = toString(LANESPREAD_RIGHT); lsfS = attrs.getOpt<std::string>(SUMO_ATTR_SPREADTYPE, id.c_str(), ok, lsfS); if (SUMOXMLDefinitions::LaneSpreadFunctions.hasString(lsfS)) { myCurrentEdge->lsf = SUMOXMLDefinitions::LaneSpreadFunctions.get(lsfS); } else { WRITE_ERROR("Unknown spreadType '" + lsfS + "' for edge '" + id + "'."); } }
//------------------------------------------------------------------------------ int main(void) { VICConfig(); TimingInit(); LEDInit(); UART1Init(); UART2Init(); I2CInit(); SPISlaveInit(); Wait(100); UART1Printf("University of Tokyo NaviCtrl firmware V2"); ReadEEPROM(); UBloxInit(); LSM303DLInit(); FlightCtrlCommsInit(); SDCardInit(); NavigationInit(); ExternalButtonInit(); // Enable the "new data" interrupt. VIC_Config(EXTIT0_ITLine, VIC_IRQ, IRQ_PRIORITY_NEW_DATA); VIC_ITCmd(EXTIT0_ITLine, ENABLE); // Enable the 50Hz Interrupt. VIC_Config(EXTIT3_ITLine, VIC_IRQ, IRQ_PRIORITY_50HZ); VIC_ITCmd(EXTIT3_ITLine, ENABLE); // Main loop. uint32_t led_timer = GetTimestamp(); for (;;) { #ifndef VISION // Check for new data from the magnetometer. ProcessIncomingLSM303DL(); // Skip the rest of the main loop if mag calibration is ongoing. if (MagCalibration(mag_calibration_)) continue; // Check for new data on the GPS UART port. ProcessIncomingUBlox(); #endif // Check for new data from the FlightCtrl. if (NewDataFromFlightCtrl()) { ClearNewDataFromFlightCtrlFlag(); #ifdef VISION KalmanAccelerometerUpdate(); #endif UpdateNavigation(); PrepareFlightCtrlDataExchange(); #ifndef VISION RequestLSM303DL(); #endif // Check if new data has come while processing the data. This indicates // that processing did not complete fast enough. if (NewDataFromFlightCtrl()) { overrun_counter_++; } } #ifndef VISION CheckUBXFreshness(); CheckLSM303DLFreshness(); // Normally the magnetometer is read every time new data comes from the // FlightCtrl. The following statement is a backup that ensures the // magnetometer is updated even if there is no connection to the FlightCtrl // and also deals with read errors. if (LSM303DLDataStale()) { if (MillisSinceTimestamp(LSM303DLLastRequestTimestamp()) > 20) RequestLSM303DL(); if (LSM303DLErrorBits() & LSM303DL_ERROR_BIT_I2C_BUSY) I2CReset(); } #else CheckVisionFreshness(); #endif // Check for incoming data on the "update & debug" UART port. ProcessIncomingUART1(); // Check for incoming data on the "FligthCtrl" UART port. ProcessIncomingUART2(); ProcessLogging(); if (TimestampInPast(led_timer)) { GreenLEDToggle(); while (TimestampInPast(led_timer)) led_timer += 100; // Debug output for GPS and magnetomter. Remove after testing is completed // UART1Printf("%+5.2f,%+5.2f,%+5.2f", // MagneticVector()[0], // MagneticVector()[1], // MagneticVector()[2]); // UART1Printf("%i,%i,%i", // MagnetometerVector()[0], // MagnetometerVector()[1], // MagnetometerVector()[2]); // UART1Printf("%i,%i,%i", // MagnetometerBiasVector()[0], // MagnetometerBiasVector()[1], // MagnetometerBiasVector()[2]); // UART1Printf("%f", CurrentHeading()); // UART1Printf("%f,%f,%f", // (float)(UBXPosLLH()->longitude * 1e-7), // (float)(UBXPosLLH()->latitude * 1e-7), // (float)(UBXPosLLH()->height_above_ellipsoid * 1e-3)); UART1PrintfSafe("%+5.2f,%+5.2f,%+5.2f,%+5.2f", PositionVector()[0], PositionVector()[1], PositionVector()[2], CurrentHeading()); } } }