void loadWaypointsFile(std::string path, std::vector<geometry_msgs::PointStamped>& waypoints)
{
if (path.empty())
{
ROS_ERROR_STREAM("Could not load empty file path.");
return;
}
std::ifstream file;
file.open(path.c_str());
if (!file.is_open())
{
ROS_INFO_STREAM("Could not open file: " << path);
return;
}
std::string line = "";
auto lineIndex = 1;
while (!file.eof())
{
getline(file, line);
if (!line.empty() && line[0] != '#')
{
std::vector<std::string> tokens = split(line, ',');
if (tokens.size() != 2)
{
ROS_ERROR_STREAM(path << ":" << lineIndex << " - " << tokens.size() << " tokens instead of 2.");
return;
}
double lat, lon;
if (tokens[0].find('?') != std::string::npos)
lat = dmsToDec(tokens[0]);
else
lat = stod(tokens[0]);
if (tokens[1].find('?') != std::string::npos)
lon = dmsToDec(tokens[1]);
else
lon = stod(tokens[1]);
geometry_msgs::PointStamped p;
UTM(lat, lon, &(p.point.x), &(p.point.y));
waypoints.push_back(p);
}
lineIndex++;
}
}
void originCallback(const sensor_msgs::NavSatFixConstPtr& msg)
{
std::lock_guard<std::mutex> lock(current_mutex);
tf::TransformListener tf_listener;
tf::StampedTransform transform;
geometry_msgs::Point position;
UTM(msg->latitude, msg->longitude, &(position.x), &(position.y));
if (tf_listener.waitForTransform("/odom", "/base_footprint", ros::Time(0), ros::Duration(3.0)))
{
tf_listener.lookupTransform("/odom", "/base_footprint", ros::Time(0), transform);
geometry_msgs::TransformStamped result;
tf::transformStampedTFToMsg(transform, result);
position.x -= result.transform.translation.x;
position.y -= result.transform.translation.y;
map_origin = position;
}
}
UTM LLAtoUTM(LLA const &lla, ReferenceEllipsoid const &_ref, char _zone)
{
// central-meridian scale factor
static double k0 = 0.9996;
double lon = lla.longitude();
double lat = lla.latitude();
double lon0 = 0.0;
double nu, T, T2, C, CP, A, A2, A4, M, S;
//Make sure the longitude is between -180.00 .. 179.9
if (lon >= PI) {
int n = (int) (0.5 * lon / PI + 0.5);
lon -= n * 2.0 * PI;
} else
if (lon < -PI) {
int n = (int) (0.5 * lon / PI - 0.5);
lon -= n * 2.0 * PI;
}
if (_zone >= 0) {
switch (_zone) {
case 32:
lon0 = 0.1308996938996;
break;
case 31:
lon0 = 0.078539816339744;
break;
case 33:
lon0 = 0.261799387799149;
break;
case 35:
lon0 = 0.471238898038469;
break;
case 37:
lon0 = 0.654498469497874;
break;
default:
lon0 = toRadians((int(_zone) - 1)*6.0 - 180.0 + 3.0);
}
}
// special zone for Norway, lat 56 - 64 deg, lon 3 - 12 deg
if (lat >= 0.9773843811168 && lat < 1.1170107212764 &&
lon >= 0.0523598775598 && lon < 0.2094395102393) {
_zone = 32;
lon0 = 0.1308996938996;
} else
// special zones for Svalbard, lat 72 - 84
if( lat >= 1.2566370614359 && lat < 1.4660765716752) {
if (lon < 0.0) {
} else
if (lon < 0.157079632679490) { // 0 - 9 deg
_zone = 31;
lon0 = 0.078539816339744;
} else
if (lon < 0.366519142918809) { // 9 - 21 deg
_zone = 33;
lon0 = 0.261799387799149;
} else
if (lon < 0.575958653158129) { // 21 - 33 deg
_zone = 35;
lon0 = 0.471238898038469;
} else
if (lon < 0.733038285837618) { // 33 - 42 deg
_zone = 37;
lon0 = 0.654498469497874;
}
}
if (_zone == -1) {
_zone = char((lon / PI + 1.0) * 30.0) + 1;
lon0 = toRadians((int(_zone) - 1)*6.0 - 180.0 + 3.0);
}
S = sin(lat);
C = cos(lat);
T = S/C;
T2 = T * T;
nu = _ref.A/sqrt(1.0-_ref.e2*S*S);
CP = _ref.ep2*C*C;
A = C*(lon - lon0);
A2 = A * A;
A4 = A2 * A2;
// approximation for length of arc of a meridian from equator to lat
M = _ref.A*(_ref.m_0*lat + _ref.m_1*sin(2.0*lat) + _ref.m_2*sin(4.0*lat) - _ref.m_3*sin(6.0*lat));
double _easting, _northing;
_easting = k0 * nu * ( A +
(1.0 - T2 + CP) * A2 * A / 6 +
(5.0 + T2*(T2-18.0) + 72*CP - 58.0*_ref.ep2) * A4 *A / 120.0
) + 500000.0;
_northing = k0 * ( M +
nu * T * ( 0.5*A2 +
(5.0 - T2 + CP*(9.0+4.0*CP))*A4/24.0 +
(61.0 + T2*(T2-58.0) + 600.0*CP - 330.0*_ref.ep2)*A4*A2/720.0
)
);
if (lat < 0.0) {
_northing += 10000000.0; //10000000 meter offset for southern hemisphere
}
char _designator = UTM::getDesignator(lat);
return UTM(_easting, _northing, _zone, _designator, lla.altitude());
}
StreckeScene::StreckeScene(const std::vector<std::unique_ptr<Strecke>>& strecken, Visualisierung& visualisierung, QObject *parent) :
QGraphicsScene(parent)
{
this->setItemIndexMethod(QGraphicsScene::NoIndex);
size_t anzahlSegmente = 0, anzahlStreckenelemente = 0;
// Berechne UTM-Referenzpunkt als Mittelwert der Strecken-Referenzpunkte
double utmRefWe = 0.0;
double utmRefNs = 0.0;
for (const auto& strecke : strecken)
{
if (strecke->UTM) {
utmRefWe += strecke->UTM->UTM_WE / static_cast<double>(strecken.size());
utmRefNs += strecke->UTM->UTM_NS / static_cast<double>(strecken.size());
}
}
this->m_utmRefPunkt.UTM_WE = static_cast<int>(utmRefWe);
this->m_utmRefPunkt.UTM_NS = static_cast<int>(utmRefNs);
std::unique_ptr<Segmentierer> segmentierer = visualisierung.segmentierer();
const auto richtungen_zusi2 = { StreckenelementRichtung::Norm };
const auto richtungen_zusi3 = { StreckenelementRichtung::Norm, StreckenelementRichtung::Gegen };
for (const std::unique_ptr<Strecke>& strecke : strecken)
{
const UTM strecke_utm = (strecke->UTM ? *strecke->UTM : UTM());
const auto utm_dx = 1000 * (strecke_utm.UTM_WE - this->m_utmRefPunkt.UTM_WE);
const auto utm_dy = 1000 * (strecke_utm.UTM_NS - this->m_utmRefPunkt.UTM_NS);
// Die Betriebsstellen werden pro Streckenmodul beschriftet, da manche Betriebsstellennamen
// (z.B. Sbk-Bezeichnungen) in mehreren Modulen vorkommen und dann falsch platziert wuerden.
std::unordered_map<std::string, QRectF> betriebsstellenKoordinaten;
bool istZusi2 = false;
auto richtungen = istZusi2 ? richtungen_zusi2 : richtungen_zusi3;
float offset = (segmentierer->beideRichtungen() || istZusi2) ? 0.49f : 0.0f;
for (const auto& streckenelement : strecke->children_StrElement)
{
if (streckenelement)
{
anzahlStreckenelemente++;
for (StreckenelementRichtung richtung : richtungen)
{
StreckenelementUndRichtung elementRichtung { streckenelement.get(), richtung }; // streckenelement->richtung(richtung);
// Streckenelement-Segmente
if (segmentierer->istSegmentStart(elementRichtung))
{
auto item = std::make_unique<StreckensegmentItem>(elementRichtung, *segmentierer, offset, nullptr);
auto startNr = streckenelement->Nr;
auto endeNr = item->ende()->Nr;
// Fuer Zusi-3-Strecken wird jedes Segment doppelt gefunden (einmal von jedem Ende).
// Manche Visualisierungen sind nicht richtungsspezifisch und brauchen daher nur eines davon.
// Behalte nur die Segmente, deren Endelement eine groessere Nummer hat als das Startelement.
// (Fuer 1-Element-Segmente behalte dasjenige, das in Normrichtung beginnt).
if (istZusi2 || segmentierer->beideRichtungen() || endeNr > startNr ||
(endeNr == startNr && elementRichtung.getRichtung() == StreckenelementRichtung::Norm))
{
// Zusi 3: x = Ost, y = Nord
visualisierung.setzeDarstellung(*item);
item->moveBy(utm_dx, utm_dy);
this->addItem(item.release());
anzahlSegmente++;
}
}
// Signale
const auto& richtungsInfo = elementRichtung.richtungsInfo();
if (richtungsInfo.has_value()) {
const auto& signal = richtungsInfo->Signal;
if (signal && !signal->Signalname.empty() &&
(istZusi2 || (static_cast<SignalTyp>(signal->SignalTyp) != SignalTyp::Weiche
&& static_cast<SignalTyp>(signal->SignalTyp) != SignalTyp::Unbestimmt
&& static_cast<SignalTyp>(signal->SignalTyp) != SignalTyp::Sonstiges
&& static_cast<SignalTyp>(signal->SignalTyp) != SignalTyp::Bahnuebergang))) {
Vec3 vec = elementRichtung.endpunkt() - elementRichtung.gegenrichtung().endpunkt();
float phi = atan2(-vec.Y, vec.X);
QColor farbe = Qt::red;
switch (static_cast<SignalTyp>(signal->SignalTyp)) {
case SignalTyp::Vorsignal:
farbe = Qt::darkGreen;
break;
case SignalTyp::Gleissperre:
case SignalTyp::Rangiersignal:
farbe = Qt::blue;
break;
default:
break;
}
auto si = std::make_unique<DreieckItem>(phi, QString::fromUtf8(signal->Signalname.data(), signal->Signalname.size()), farbe);
string tooltip = signal->NameBetriebsstelle + " " + signal->Signalname;
if (!signal->Stellwerk.empty()) {
tooltip += "\n[" + signal->Stellwerk + "]";
}
si->setToolTip(QString::fromUtf8(tooltip.data(), tooltip.size()));
QPointF pos(elementRichtung.endpunkt().X, elementRichtung.endpunkt().Y);
si->setPos(pos);
si->moveBy(utm_dx, utm_dy);
this->addItem(si.release());
if (static_cast<SignalTyp>(signal->SignalTyp) != SignalTyp::Vorsignal && !signal->NameBetriebsstelle.empty())
{
if (betriebsstellenKoordinaten.find(signal->NameBetriebsstelle) == betriebsstellenKoordinaten.end())
{
betriebsstellenKoordinaten[signal->NameBetriebsstelle] = QRectF(pos, pos);
}
else
{
QRectF& r = betriebsstellenKoordinaten[signal->NameBetriebsstelle];
// TODO somehow QRect::unite does not work
if (pos.x() < r.left()) { r.setLeft(pos.x()); }
if (pos.x() > r.right()) { r.setRight(pos.x()); }
if (pos.y() > r.bottom()) { r.setBottom(pos.y()); }
if (pos.y() < r.top()) { r.setTop(pos.y()); }
}
}
}
}
}
}
}
const auto elementRichtung = [&strecke](const ReferenzElement& refpunkt) -> StreckenelementUndRichtung {
if ((refpunkt.StrElement < 0) || (static_cast<size_t>(refpunkt.StrElement) >= strecke->children_StrElement.size())) {
return { nullptr, static_cast<StreckenelementRichtung>(refpunkt.StrNorm) };
} else {
return { strecke->children_StrElement[refpunkt.StrElement].get(), static_cast<StreckenelementRichtung>(refpunkt.StrNorm) };
}
}; // TODO refpunkt->elementRichtung()
for (const auto& refpunkt : strecke->children_ReferenzElemente)
{
if (!refpunkt || !elementRichtung(*refpunkt).getStreckenelement()) continue;
if (static_cast<ReferenzpunktTyp>(refpunkt->RefTyp) == ReferenzpunktTyp::Aufgleispunkt) {
Vec3 vec = elementRichtung(*refpunkt).endpunkt() - elementRichtung(*refpunkt).gegenrichtung().endpunkt();
qreal phi = atan2(-vec.Y, vec.X);
assert(refpunkt->Info.data() != nullptr);
auto si = std::make_unique<DreieckItem>(phi,
QString::fromUtf8(refpunkt->Info.data(), refpunkt->Info.size()), Qt::magenta);
si->setPos(elementRichtung(*refpunkt).endpunkt().X, elementRichtung(*refpunkt).endpunkt().Y);
si->moveBy(utm_dx, utm_dy);
this->addItem(si.release());
}
}
for (const auto& p : betriebsstellenKoordinaten) {
std::string betriebsstelle = p.first;
#if 0
auto ri = this->addRect(p.second);
ri->moveBy(1000 * (strecke->utmPunkt.UTM_WE - this->m_utmRefPunkt.UTM_WE), 1000 * (strecke->utmPunkt.UTM_NS - this->m_utmRefPunkt.UTM_NS));
#endif
auto ti = std::unique_ptr<Label>(new Label(QString::fromUtf8(betriebsstelle.data(), betriebsstelle.size())));
ti->setAlignment(Qt::AlignCenter | Qt::AlignHCenter);
ti->setPos((p.second.left() + p.second.right()) / 2.0, (p.second.top() + p.second.bottom()) / 2.0);
ti->moveBy(utm_dx, utm_dy);
ti->setFlag(QGraphicsItem::ItemIgnoresTransformations);
ti->setPen(QPen(Qt::black));
ti->setBrush(QBrush(Qt::black));
this->addItem(ti.release());
}
}
// TODO: Kreise ohne jegliche Weichen werden nicht als Segmente erkannt.
qDebug() << anzahlSegmente << "Segmente für" << anzahlStreckenelemente << "Streckenelemente";
}
