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

temp

parent e107a34a
......@@ -453,6 +453,7 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.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/CommandDispatcher.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
......@@ -517,7 +518,7 @@ HEADERS += \
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/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/server.h \
src/comm/ros_bridge/include/messages/std_msgs/time.h \
......@@ -564,7 +565,8 @@ SOURCES += \
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/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/time.cpp \
src/comm/ros_bridge/include/server.cpp \
......
......@@ -79,15 +79,17 @@ public:
NemoInterface::STATUS status();
bool running();
private slots:
void _addTilesLocal(const TileArray &tileArray);
void _removeTilesLocal(const IDArray &idArray);
void _clearTilesLocal();
void _updateProgress(ProgressArray progressArray);
private:
typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint;
typedef std::map<long, MeasurementTile> TileMap;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
void _addTiles(const TileArray &tileArray);
void _removeTiles(const IDArray &idArray);
void _clearTiles();
void doTopicServiceSetup();
void loop();
......@@ -158,6 +160,8 @@ void NemoInterface::Impl::stop() {
emit this->_parent->runningChanged();
}
void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {}
TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
TileArray tileArray;
......@@ -292,23 +296,75 @@ QVector<int> NemoInterface::Impl::progress() {
bool NemoInterface::Impl::running() { return _running(this->_state); }
void NemoInterface::Impl::doTopicServiceSetup() {
using namespace ros_bridge::messages;
void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
bool anyChanges = false;
// snake tiles.
{
SharedLock lk(this->tilesENUMutex);
this->_pRosBridge->advertiseTopic(
"/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
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();
}
}
// snake origin.
{
SharedLock lk(this->ENUOriginMutex);
this->_pRosBridge->advertiseTopic(
"/snake/origin", geographic_msgs::geo_point::messageType().c_str());
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();
}
}
void NemoInterface::Impl::_clearTilesLocal() {
if (!_tileMap.empty()) {
_tileMap.clear();
emit _parent->tilesChanged();
}
}
void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
for (auto itPair = progressArray.begin(); itPair != progressArray.end();) {
const auto &id = itPair->first;
auto it = _tileMap.find(id);
if (Q_LIKELY(it != _tileMap.end())) {
const auto &progress = itPair->second;
it->second.setProgress(progress);
++itPair;
} 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.
this->_pRosBridge->subscribe(
......@@ -340,7 +396,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
this->_pRosBridge->subscribe(
"/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc) {
// auto start = std::chrono::high_resolution_clock::now();
nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
this->_setState(STATUS::INVALID_HEARTBEAT);
......@@ -355,61 +410,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
this->nextTimeout =
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 {
namespace nemo_msgs {
//! @brief Namespace containing methodes for geometry_msgs/Point32 message
//! generation.
namespace progress {
namespace labeled_progress {
std::string messageType();
//! @brief C++ representation of nemo_msgs/Progress message
template <class IntType = long,
template <class, class...> class ContainterType = std::vector>
class GenericProgress {
public:
GenericProgress() {}
GenericProgress(const ContainterType<IntType> &progress)
: _progress(progress) {}
GenericProgress(const GenericProgress &p) : _progress(p.progress()) {}
namespace {
const char *progressKey = "progress";
const char *idKey = "id";
} // namespace
virtual const ContainterType<IntType> &progress(void) const {
return _progress;
}
virtual ContainterType<IntType> &progress(void) { return _progress; }
//! @brief C++ representation of nemo_msgs/labeled_progress message
class LabeledProgress {
public:
LabeledProgress() {}
LabeledProgress(double progress, long id) : _id(id), _progress(progress) {}
bool operator==(const GenericProgress &other) const {
return this->_progress == other._progress;
}
long id() const { return _id; }
void setId(long id) { _id = id; }
bool operator!=(const GenericProgress &other) const {
return !this->operator==(other);
}
double progress() const { return _progress; }
void setProgress(double progress) { _progress = progress; }
protected:
ContainterType<IntType> _progress;
long _id;
double _progress;
};
typedef GenericProgress<> Progress;
template <class ProgressType>
bool toJson(const ProgressType &p, rapidjson::Value &value,
template <class LabeledProgressType>
bool toJson(const LabeledProgressType &lp, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
rapidjson::Value jProgress(rapidjson::kArrayType);
for (unsigned long i = 0; i < std::uint64_t(p.progress().size()); ++i) {
jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])),
allocator);
}
value.AddMember("progress", jProgress, allocator);
value.AddMember(idKey, lb.id(), allocator);
value.AddMember(progressKey, lb.progress(), allocator);
return true;
}
template <class ProgressType>
bool fromJson(const rapidjson::Value &value, ProgressType &p) {
if (!value.HasMember("progress") || !value["progress"].IsArray()) {
assert(false);
if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
return false;
}
const auto &jsonProgress = value["progress"];
unsigned long sz = jsonProgress.Size();
p.progress().clear();
p.progress().reserve(sz);
for (unsigned long i = 0; i < sz; ++i)
p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
return false;
}
p.setId(value[idKey].GetInt());
p.setProgress(value[progressKey].GetDouble());
return true;
}
} // namespace progress
} // namespace labeled_progress
} // namespace nemo_msgs
} // namespace messages
} // 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