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());
}
}
}