Commit b098b99a authored by Valentin Platzgummer's avatar Valentin Platzgummer

tile id int64_t > QString, for robustness

parent 9e620065
......@@ -517,8 +517,6 @@ HEADERS += \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
src/comm/ros_bridge/include/messages/std_msgs/header.h \
src/comm/ros_bridge/include/messages/std_msgs/time.h \
src/comm/utilities.h
contains (DEFINES, QGC_ENABLE_PAIRING) {
......@@ -560,8 +558,6 @@ SOURCES += \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
src/Settings/WimaSettings.cc \
contains (DEFINES, QGC_ENABLE_PAIRING) {
......
......@@ -3,6 +3,6 @@
#include <QVector>
typedef QVector<std::int64_t> IDArray;
typedef QVector<QString> IDArray;
#endif // IDARRAY_H
......@@ -10,7 +10,7 @@
#include "SettingsFact.h"
#include "AreaData.h"
#include "ProgressArray.h"
#include "geometry/ProgressArray.h"
class RoutingThread;
class RoutingResult;
......
......@@ -19,6 +19,8 @@
#define MAX_TILES 1000
#endif
QString randomId();
using namespace geometry;
namespace trans = bg::strategy::transform;
......@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
// find unique id
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
auto newId = MeasurementTile::randomId();
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
......@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
if (it == _indexMap.end()) {
break;
} else {
++newId;
newId = MeasurementTile::randomId();
}
}
......@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
// Convert to geo system.
for (const auto &t : tilesENU) {
auto geoTile = new MeasurementTile(pData.get());
std::size_t hashValue = 0;
std::hash<QGeoCoordinate> hashFun;
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
hashValue ^= hashFun(geoVertex);
}
geoTile->setId(std::int64_t(hashValue));
pData->append(geoTile);
}
}
......@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
// find unique id
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
auto newId = MeasurementTile::randomId();
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
......@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
if (it == _indexMap.end()) {
break;
} else {
++newId;
newId = MeasurementTile::randomId();
}
}
......@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
return true;
}
QString randomId() {
std::srand(std::time(nullptr));
std::int64_t r = 0;
for (int i = 0; i < 10; ++i) {
r ^= std::rand();
}
return QString::number(r);
}
......@@ -107,7 +107,7 @@ private:
// Tile stuff.
TilePtr _tiles;
std::map<std::int64_t /*id*/, int> _indexMap;
std::map<QString /*id*/, int> _indexMap;
QTimer _timer;
STATE _state;
QFutureWatcher<TilePtr> _watcher;
......
......@@ -6,7 +6,7 @@ const char *MeasurementTile::settingsGroup = "MeasurementTile";
const char *MeasurementTile::nameString = "MeasurementTile";
MeasurementTile::MeasurementTile(QObject *parent)
: GeoArea(parent), _progress(0), _id(0) {
: GeoArea(parent), _progress(0), _id(MeasurementTile::randomId()) {
init();
}
......@@ -38,9 +38,9 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
void MeasurementTile::init() { this->setObjectName(nameString); }
int64_t MeasurementTile::id() const { return _id; }
QString MeasurementTile::id() const { return _id; }
void MeasurementTile::setId(const int64_t &id) {
void MeasurementTile::setId(const QString &id) {
if (_id != id) {
_id = id;
emit idChanged();
......@@ -49,6 +49,19 @@ void MeasurementTile::setId(const int64_t &id) {
QList<QGeoCoordinate> MeasurementTile::tile() const { return coordinateList(); }
QString MeasurementTile::randomId(int length) {
static const QString values(
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789");
QString str;
std::srand(std::time(nullptr));
for (int i = 0; i < length; ++i) {
int index = std::rand() % values.length();
QChar c = values.at(index);
str.append(c);
}
return str;
}
double MeasurementTile::progress() const { return _progress; }
void MeasurementTile::setProgress(double progress) {
......
......@@ -24,11 +24,13 @@ public:
double progress() const;
void setProgress(double progress);
int64_t id() const;
void setId(const int64_t &id);
QString id() const;
void setId(const QString &id);
QList<QGeoCoordinate> tile() const;
static QString randomId(int length = 10);
// Static Variables
static const char *settingsGroup;
static const char *nameString;
......@@ -40,5 +42,5 @@ signals:
private:
void init();
double _progress;
int64_t _id;
QString _id;
};
......@@ -86,6 +86,8 @@ std::size_t TaskDispatcher::pendingTasks() {
return this->_taskQueue.size() + (_running ? 1 : 0);
}
bool TaskDispatcher::idle() { return this->pendingTasks() == 0; }
void TaskDispatcher::run() {
while (true) {
ULock lk1(this->_mutex);
......
......@@ -53,6 +53,7 @@ public:
bool isRunning();
std::size_t pendingTasks();
bool idle();
protected:
void run();
......
......@@ -16,6 +16,8 @@ static const char *rosTopics = "/rosapi/topics";
static constexpr auto pollIntervall = std::chrono::milliseconds(200);
Rosbridge::STATE translate(RosbridgeImpl::STATE s);
Rosbridge::Rosbridge(const QUrl url, QObject *parent)
: QObject(parent), _url(url), _running(false) {
static std::once_flag flag;
......@@ -34,6 +36,9 @@ void Rosbridge::start() {
_impl = new RosbridgeImpl(_url);
_impl->moveToThread(_t);
connect(_impl, &RosbridgeImpl::stateChanged, this,
&Rosbridge::_onImplStateChanged);
connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start);
connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop);
......@@ -77,20 +82,7 @@ void Rosbridge::stop() {
Rosbridge::STATE Rosbridge::state() {
if (_running) {
switch (_impl->state()) {
case RosbridgeImpl::STATE::STOPPED:
case RosbridgeImpl::STATE::STOPPING:
return STATE::STOPPED;
case RosbridgeImpl::STATE::STARTING:
return STATE::STARTED;
case RosbridgeImpl::STATE::CONNECTED:
return STATE::CONNECTED;
case RosbridgeImpl::STATE::TIMEOUT:
return STATE::TIMEOUT;
break;
}
qCritical() << "unreachable branch!";
return STATE::STOPPED;
return translate(_impl->state());
} else {
return STATE::STOPPED;
}
......@@ -251,3 +243,30 @@ void Rosbridge::waitForService(const QString &service) {
qDebug() << "waitForService: Rosbridge not connected.";
}
}
void Rosbridge::_onImplStateChanged() {
static STATE oldState = STATE::STOPPED;
auto newState = translate(_impl->state());
if (oldState != newState) {
emit stateChanged();
}
oldState = newState;
}
Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
switch (s) {
case RosbridgeImpl::STATE::STOPPED:
case RosbridgeImpl::STATE::STOPPING:
return Rosbridge::STATE::STOPPED;
case RosbridgeImpl::STATE::STARTING:
return Rosbridge::STATE::STARTED;
case RosbridgeImpl::STATE::CONNECTED:
return Rosbridge::STATE::CONNECTED;
case RosbridgeImpl::STATE::TIMEOUT:
return Rosbridge::STATE::TIMEOUT;
break;
}
qCritical() << "unreachable branch!";
return Rosbridge::STATE::STOPPED;
}
......@@ -101,6 +101,9 @@ signals:
void _unadvertiseAllServices();
private:
void _onImplStateChanged();
std::atomic<STATE> _state;
RosbridgeImpl *_impl;
QThread *_t;
QUrl _url;
......
......@@ -114,7 +114,6 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _advertisedTopics.find(topic);
if (Q_LIKELY(it != _advertisedTopics.end())) {
_advertisedTopics.erase(it);
QJsonObject o;
o[opKey] = unadvertiseOpKey;
......@@ -123,6 +122,8 @@ void RosbridgeImpl::unadvertiseTopic(const QString &topic) {
QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload);
_advertisedTopics.erase(it);
} else {
qDebug() << "Topic " << topic << " not advertised.";
}
......@@ -170,7 +171,6 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _subscribedTopics.find(topic);
if (Q_LIKELY(it != _subscribedTopics.end())) {
_subscribedTopics.erase(it);
QJsonObject o;
o[opKey] = unsubscribeOpKey;
......@@ -179,6 +179,8 @@ void RosbridgeImpl::unsubscribeTopic(const QString &topic) {
QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload);
_subscribedTopics.erase(it);
} else {
qDebug() << "unsubscribeTopic: topic " << topic << " already subscribed!";
}
......@@ -293,7 +295,6 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
if (_state == STATE::CONNECTED || _state == STATE::STOPPING) {
auto it = _advertisedServices.find(service);
if (it != _advertisedServices.end()) {
it = _advertisedServices.erase(it);
QJsonObject o;
o[opKey] = unadvertiseServiceOpKey;
......@@ -302,6 +303,8 @@ void RosbridgeImpl::unadvertiseService(const QString &service) {
QString payload =
QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
_webSocket.sendTextMessage(payload);
it = _advertisedServices.erase(it);
} else {
qDebug() << "unadvertiseService: Service " << service
<< " not advertised.";
......@@ -369,6 +372,7 @@ void RosbridgeImpl::_doAction() {
}
void RosbridgeImpl::_onTextMessageReceived(const QString &message) {
qDebug() << "_onTextMessageReceived: " << message;
QJsonParseError e;
auto d = QJsonDocument::fromJson(message.toUtf8(), &e);
if (!d.isNull()) {
......
......@@ -3,147 +3,140 @@
#include <string>
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QJsonObject>
#include <QJsonValue>
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
//! @brief Namespace containing classes and methodes for geometry_msgs
//! generation.
namespace geographic_msgs {
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message
//! generation.
namespace geo_point {
std::string messageType();
namespace {
const char *lonKey = "longitude";
const char *latKey = "latitude";
const char *altKey = "altitude";
} // namespace
//! @brief C++ representation of geographic_msgs/GeoPoint.
template<class FloatType = _Float64, class OStream = std::ostream>
class GenericGeoPoint{
template <class FloatType = _Float64, class OStream = std::ostream>
class GenericGeoPoint {
public:
GenericGeoPoint()
: _latitude(0)
, _longitude(0)
, _altitude(0)
{}
GenericGeoPoint(const GenericGeoPoint &other)
: _latitude(other.latitude())
, _longitude(other.longitude())
, _altitude(other.altitude())
{}
GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
: _latitude(latitude)
, _longitude(longitude)
, _altitude(altitude)
{}
FloatType latitude() const {return _latitude;}
FloatType longitude() const {return _longitude;}
FloatType altitude() const {return _altitude;}
void setLatitude (FloatType latitude) {_latitude = latitude;}
void setLongitude (FloatType longitude) {_longitude = longitude;}
void setAltitude (FloatType altitude) {_altitude = altitude;}
bool operator==(const GenericGeoPoint &p1) {
return ( p1._latitude == this->_latitude
&& p1._longitude == this->_longitude
&& p1._altitude == this->_altitude);
}
bool operator!=(const GenericGeoPoint &p1) {
return !this->operator==(p1);
}
friend OStream& operator<<(OStream& os, const GenericGeoPoint& p)
{
os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude;
return os;
}
GenericGeoPoint() : _latitude(0), _longitude(0), _altitude(0) {}
GenericGeoPoint(const GenericGeoPoint &other)
: _latitude(other.latitude()), _longitude(other.longitude()),
_altitude(other.altitude()) {}
GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
: _latitude(latitude), _longitude(longitude), _altitude(altitude) {}
FloatType latitude() const { return _latitude; }
FloatType longitude() const { return _longitude; }
FloatType altitude() const { return _altitude; }
void setLatitude(FloatType latitude) { _latitude = latitude; }
void setLongitude(FloatType longitude) { _longitude = longitude; }
void setAltitude(FloatType altitude) { _altitude = altitude; }
bool operator==(const GenericGeoPoint &p1) {
return (p1._latitude == this->_latitude &&
p1._longitude == this->_longitude &&
p1._altitude == this->_altitude);
}
bool operator!=(const GenericGeoPoint &p1) { return !this->operator==(p1); }
friend OStream &operator<<(OStream &os, const GenericGeoPoint &p) {
os << "lat: " << p._latitude << " lon: " << p._longitude
<< " alt: " << p._altitude;
return os;
}
private:
FloatType _latitude;
FloatType _longitude;
FloatType _altitude;
FloatType _latitude;
FloatType _longitude;
FloatType _altitude;
};
typedef GenericGeoPoint<> GeoPoint;
namespace detail {
template <class T>
auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>)
{
return p.altitude();
}
template <class T>
auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>)
{
(void)p;
return 0.0;
}
template <class T>
void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has3Components>)
{
p.setAltitude(doc.GetFloat());
}
template <class T>
void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has2Components>)
{
(void)doc;
(void)p;
}
} // namespace detail
template <class T>
auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>) {
return p.altitude();
}
template <class T>
auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>) {
(void)p;
return 0.0;
}
template <class T>
void setAltitude(const QJsonValue &o, T &p,
traits::Type2Type<traits::Has3Components>) {
p.setAltitude(o.toDouble());
}
template <class T>
bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
typedef typename traits::Select<
traits::HasMemberAltitude<T>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
return true;
void setAltitude(const QJsonValue &o, T &p,
traits::Type2Type<traits::Has2Components>) {
(void)o;
(void)p;
}
} // namespace detail
template <class T> bool toJson(const T &p, QJsonObject &value) {
value[latKey] = p.latitude();
value[lonKey] = p.longitude();
typedef typename traits::Select<traits::HasMemberAltitude<T>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
auto altitude = detail::getAltitude(
p, traits::Type2Type<Components>()); // If T has no member altitude()
// replace it by 0.0;
value[altKey] = altitude;
return true;
}
template <class PointType>
bool fromJson(const rapidjson::Value &value,
PointType &p) {
if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){
assert(false);
return false;
}
if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){
assert(false);
return false;
}
if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
assert(false);
return false;
}
p.setLatitude(value["latitude"].GetFloat());
p.setLongitude(value["longitude"].GetFloat());
typedef typename traits::Select<
traits::HasMemberSetAltitude<PointType>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
detail::setAltitude(value["altitude"], p, traits::Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
return true;
bool fromJson(const QJsonObject &value, PointType &p) {
if (!value.contains("latitude") || !value["latitude"].isDouble()) {
return false;
}
if (!value.contains("longitude") || !value["longitude"].isDouble()) {
return false;
}
if (!value.contains("altitude") || !value["altitude"].isDouble()) {
return false;
}
p.setLatitude(value["latitude"].toDouble());
p.setLongitude(value["longitude"].toDouble());
typedef
typename traits::Select<traits::HasMemberSetAltitude<PointType>::value,
traits::Has3Components,
traits::Has2Components>::Result
Components; // Check if PointType has 2 or 3 dimensions.
detail::setAltitude(
value["altitude"], p,
traits::Type2Type<Components>()); // If T has no member altitude() discard
// doc["altitude"];
return true;
} // namespace detail
} // namespace geopoint
} // namespace geometry_msgs
} // namespace geo_point
} // namespace geographic_msgs
} // namespace messages
} // namespace ros_bridge
#include "point32.h"
std::string ros_bridge::messages::geometry_msgs::point32::messageType(){
return "geometry_msgs/Point32";
}
#pragma once
#include "ros_bridge/include/message_traits.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs generation.
namespace geometry_msgs {
//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
namespace point32 {
std::string messageType();
namespace detail {
using namespace ros_bridge::traits;
template <class T>
auto getZ(const T &p, Type2Type<Has3Components>)
{
return p.z();
}
template <class T>
auto getZ(const T &p, Type2Type<Has2Components>)
{
(void)p;
return 0.0; // p has no member z() -> add 0.
}
template <class T, typename V>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<V>)
{
p.setZ(doc["z"].GetFloat());
return true;
}
template <class T>
bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
{
(void)doc;
(void)p;
return true;
}
} // namespace detail
//! @brief C++ representation of a geometry_msgs/Point/Point32
template<typename FloatType = _Float64, class OStream = std::ostream>
class GenericPoint {
public:
GenericPoint(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){}
FloatType x() const {return _x;}
FloatType y() const {return _y;}
FloatType z() const {return _z;}
void setX(FloatType x) {_x = x;}
void setY(FloatType y) {_y = y;}
void setZ(FloatType z) {_z = z;}
bool operator==(const GenericPoint &p1) {
return (p1._x == this->_x
&& p1._y == this->_y
&& p1._z == this->_z);
}
bool operator!=(const GenericPoint &p1) {
return !this->operator==(p1);
}
friend OStream& operator<<(OStream& os, const GenericPoint& p)
{