Commit 67247b3b authored by Valentin Platzgummer's avatar Valentin Platzgummer

temp

parent e107a34a
...@@ -453,6 +453,7 @@ HEADERS += \ ...@@ -453,6 +453,7 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \ src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \ src/MeasurementComplexItem/HashFunctions.h \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
src/MeasurementComplexItem/nemo_interface/Command.h \ src/MeasurementComplexItem/nemo_interface/Command.h \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \ src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
...@@ -517,7 +518,7 @@ HEADERS += \ ...@@ -517,7 +518,7 @@ HEADERS += \
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \ src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \ src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
src/comm/ros_bridge/include/messages/nemo_msgs/progress.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/header.h \
src/comm/ros_bridge/include/server.h \ src/comm/ros_bridge/include/server.h \
src/comm/ros_bridge/include/messages/std_msgs/time.h \ src/comm/ros_bridge/include/messages/std_msgs/time.h \
...@@ -564,7 +565,8 @@ SOURCES += \ ...@@ -564,7 +565,8 @@ SOURCES += \
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \ src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \ src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.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/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \ src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
src/comm/ros_bridge/include/server.cpp \ src/comm/ros_bridge/include/server.cpp \
......
...@@ -79,15 +79,17 @@ public: ...@@ -79,15 +79,17 @@ public:
NemoInterface::STATUS status(); NemoInterface::STATUS status();
bool running(); bool running();
private slots:
void _addTilesLocal(const TileArray &tileArray);
void _removeTilesLocal(const IDArray &idArray);
void _clearTilesLocal();
void _updateProgress(ProgressArray progressArray);
private: private:
typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint; typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint;
typedef std::map<long, MeasurementTile> TileMap; typedef std::map<long, MeasurementTile> TileMap;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
void _addTiles(const TileArray &tileArray);
void _removeTiles(const IDArray &idArray);
void _clearTiles();
void doTopicServiceSetup(); void doTopicServiceSetup();
void loop(); void loop();
...@@ -158,6 +160,8 @@ void NemoInterface::Impl::stop() { ...@@ -158,6 +160,8 @@ void NemoInterface::Impl::stop() {
emit this->_parent->runningChanged(); emit this->_parent->runningChanged();
} }
void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {}
TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) { TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray tileArray; TileArray tileArray;
...@@ -292,24 +296,76 @@ QVector<int> NemoInterface::Impl::progress() { ...@@ -292,24 +296,76 @@ QVector<int> NemoInterface::Impl::progress() {
bool NemoInterface::Impl::running() { return _running(this->_state); } bool NemoInterface::Impl::running() { return _running(this->_state); }
void NemoInterface::Impl::doTopicServiceSetup() { void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
using namespace ros_bridge::messages; bool anyChanges = false;
for (const auto &tile : tileArray) {
const auto id = tile.id();
const auto it = _tileMap.find(id);
if (Q_LIKELY(it == _tileMap.end())) {
auto ret = _tileMap.insert(std::make_pair(id, tile));
anyChanges = true;
Q_ASSERT(ret.second == true);
Q_UNUSED(ret);
} else {
qCDebug(NemoInterfaceLog)
<< "_addTilesLocal(): tile with id " << id << " already inserted.";
}
}
if (anyChanges) {
emit _parent->tilesChanged();
}
}
void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) {
bool anyChanges = false;
for (const auto &id : idArray) {
const auto it = _tileMap.find(id);
if (Q_LIKELY(it != _tileMap.end())) {
_tileMap.erase(it);
anyChanges = true;
} else {
qCDebug(NemoInterfaceLog)
<< "_removeTilesLocal(): tile with id " << id << " not found.";
}
}
if (anyChanges) {
emit _parent->tilesChanged();
}
}
// snake tiles. void NemoInterface::Impl::_clearTilesLocal() {
{ if (!_tileMap.empty()) {
SharedLock lk(this->tilesENUMutex); _tileMap.clear();
this->_pRosBridge->advertiseTopic( emit _parent->tilesChanged();
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
} }
}
void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
for (auto itPair = progressArray.begin(); itPair != progressArray.end();) {
const auto &id = itPair->first;
auto it = _tileMap.find(id);
// snake origin. if (Q_LIKELY(it != _tileMap.end())) {
{ const auto &progress = itPair->second;
SharedLock lk(this->ENUOriginMutex); it->second.setProgress(progress);
this->_pRosBridge->advertiseTopic( ++itPair;
"/snake/origin", geographic_msgs::geo_point::messageType().c_str()); } else {
qCDebug(NemoInterfaceLog)
<< "_updateProgress(): tile with id " << id << " not found.";
itPair = progressArray.erase(itPair);
}
} }
emit _parent->progressChanged(progressArray);
}
void NemoInterface::Impl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
// Subscribe nemo progress. // Subscribe nemo progress.
this->_pRosBridge->subscribe( this->_pRosBridge->subscribe(
"/nemo/progress", "/nemo/progress",
...@@ -340,7 +396,6 @@ void NemoInterface::Impl::doTopicServiceSetup() { ...@@ -340,7 +396,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
this->_pRosBridge->subscribe( this->_pRosBridge->subscribe(
"/nemo/heartbeat", "/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) { /* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
nemo_msgs::heartbeat::Heartbeat heartbeatMsg; nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) { if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
this->_setState(STATUS::INVALID_HEARTBEAT); this->_setState(STATUS::INVALID_HEARTBEAT);
...@@ -355,61 +410,6 @@ void NemoInterface::Impl::doTopicServiceSetup() { ...@@ -355,61 +410,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
this->nextTimeout = this->nextTimeout =
std::chrono::high_resolution_clock::now() + timeoutInterval; std::chrono::high_resolution_clock::now() + timeoutInterval;
} }
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
// std::cout << "/nemo/heartbeat callback time: " <<
// delta.count() << " ms"
// << std::endl;
});
// Advertise /snake/get_origin.
this->_pRosBridge->advertiseService(
"/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr {
using namespace ros_bridge::messages;
SharedLock lk(this->ENUOriginMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
lk.unlock();
if (geographic_msgs::geo_point::toJson(origin, jOrigin,
pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_origin service: could not create json document.";
}
return pDoc;
});
// Advertise /snake/get_tiles.
this->_pRosBridge->advertiseService(
"/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr {
SharedLock lk(this->tilesENUMutex);
JsonDocUPtr pDoc(
std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
if (jsk_recognition_msgs::polygon_array::toJson(
this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
} else {
lk.unlock();
qCWarning(NemoInterfaceLog)
<< "/snake/get_tiles service: could not create json document.";
}
return pDoc;
}); });
} }
......
#include "labeled_progress.h"
std::string ros_bridge::messages::nemo_msgs::labeled_progress::messageType() {
return "nemo_msgs/LabeledProgress";
}
...@@ -12,67 +12,57 @@ namespace messages { ...@@ -12,67 +12,57 @@ namespace messages {
namespace nemo_msgs { namespace nemo_msgs {
//! @brief Namespace containing methodes for geometry_msgs/Point32 message //! @brief Namespace containing methodes for geometry_msgs/Point32 message
//! generation. //! generation.
namespace progress { namespace labeled_progress {
std::string messageType(); std::string messageType();
//! @brief C++ representation of nemo_msgs/Progress message namespace {
template <class IntType = long, const char *progressKey = "progress";
template <class, class...> class ContainterType = std::vector> const char *idKey = "id";
class GenericProgress { } // namespace
public:
GenericProgress() {}
GenericProgress(const ContainterType<IntType> &progress)
: _progress(progress) {}
GenericProgress(const GenericProgress &p) : _progress(p.progress()) {}
virtual const ContainterType<IntType> &progress(void) const { //! @brief C++ representation of nemo_msgs/labeled_progress message
return _progress; class LabeledProgress {
} public:
virtual ContainterType<IntType> &progress(void) { return _progress; } LabeledProgress() {}
LabeledProgress(double progress, long id) : _id(id), _progress(progress) {}
bool operator==(const GenericProgress &other) const { long id() const { return _id; }
return this->_progress == other._progress; void setId(long id) { _id = id; }
}
bool operator!=(const GenericProgress &other) const { double progress() const { return _progress; }
return !this->operator==(other); void setProgress(double progress) { _progress = progress; }
}
protected: protected:
ContainterType<IntType> _progress; long _id;
double _progress;
}; };
typedef GenericProgress<> Progress;
template <class ProgressType> template <class LabeledProgressType>
bool toJson(const ProgressType &p, rapidjson::Value &value, bool toJson(const LabeledProgressType &lp, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) { rapidjson::Document::AllocatorType &allocator) {
rapidjson::Value jProgress(rapidjson::kArrayType); value.AddMember(idKey, lb.id(), allocator);
for (unsigned long i = 0; i < std::uint64_t(p.progress().size()); ++i) { value.AddMember(progressKey, lb.progress(), allocator);
jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])),
allocator);
}
value.AddMember("progress", jProgress, allocator);
return true; return true;
} }
template <class ProgressType> template <class ProgressType>
bool fromJson(const rapidjson::Value &value, ProgressType &p) { bool fromJson(const rapidjson::Value &value, ProgressType &p) {
if (!value.HasMember("progress") || !value["progress"].IsArray()) { if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
assert(false);
return false; return false;
} }
const auto &jsonProgress = value["progress"]; if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
unsigned long sz = jsonProgress.Size(); return false;
p.progress().clear(); }
p.progress().reserve(sz);
for (unsigned long i = 0; i < sz; ++i) p.setId(value[idKey].GetInt());
p.progress().push_back(std::int32_t(jsonProgress[i].GetInt())); p.setProgress(value[progressKey].GetDouble());
return true; return true;
} }
} // namespace progress } // namespace labeled_progress
} // namespace nemo_msgs } // namespace nemo_msgs
} // namespace messages } // namespace messages
} // namespace ros_bridge } // namespace ros_bridge
#include "progress.h"
std::string ros_bridge::messages::nemo_msgs::progress::messageType(){
return "nemo_msgs/Progress";
}
#pragma once
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <vector>
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for nemo_msgs
//! generation.
namespace nemo_msgs {
//! @brief Namespace containing methodes for nemo_msgs/tile message
//! generation.
namespace labeled_progress {
std::string messageType();
namespace {
const char *progressKey = "progress";
const char *idKey = "id";
const char *tileKey = "tile";
} // namespace
//! @brief C++ representation of nemo_msgs/tile message
template <class GeoPoint, template <class, class> class Container = std::vector>
class GenericTile {
public:
typedef Container<GeoPoint> GeoContainer;
GenericTile() {}
GenericTile(const GeoContainer &tile, double progress, long id)
: _tile(tile), _id(id), _progress(progress) {}
long id() const { return _id; }
void setId(long id) { _id = id; }
double progress() const { return _progress; }
void setProgress(double progress) { _progress = progress; }
const GeoContainer &tile() const { return _tile; }
GeoContainer &tile() { return _tile; }
void setTile(const GeoContainer &tile) { _tile = tile; }
protected:
long _id;
double _progress;
GeoContainer _tile;
};
typedef GenericTile<> Tile;
template <class TileType>
bool toJson(const TileType &tile, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
value.AddMember(idKey, tile.id(), allocator);
value.AddMember(progressKey, tile.progress(), allocator);
using namespace messages::geographic_msgs;
rapidjson::Value geoPoints(rapidjson::kArrayType);
for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) {
rapidjson::Document geoPoint(rapidjson::kObjectType);
if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) {
return false;
}
geoPoints.PushBack(geoPoint, allocator);
}
value.AddMember(tileKey, geoPoints, allocator);
return true;
}
template <class ProgressType>
bool fromJson(const rapidjson::Value &value, ProgressType &p) {
if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
return false;
}
if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
return false;
}
p.setId(value[idKey].GetInt());
p.setProgress(value[progressKey].GetDouble());
return true;
}
} // namespace labeled_progress
} // namespace nemo_msgs
} // namespace messages
} // namespace ros_bridge
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment