Skip to content

Q
qgroundcontrol
Commit 4f76dc12 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

cannot infere topic of type... issue solved

parent be175073
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 281 additions and 142 deletions
qgroundcontrol.pro
View file @ 4f76dc12
......@@ -27,7 +27,7 @@ QGCROOT = $$PWD
DebugBuild {
DESTDIR = $${OUT_PWD}/debug
#DEFINES += DEBUG
DEFINES += DEBUG
#DEFINES += ROS_BRIDGE_CLIENT_DEBUG
}
else {
......@@ -50,6 +50,7 @@ MacBuild {
LinuxBuild {
CONFIG += qesp_linux_udev
CONFIG += ccache # improves build time
}
WindowsBuild {
......
src/Wima/WaypointManager/Slicer.cpp
View file @ 4f76dc12
......@@ -43,16 +43,15 @@ void Slicer::_updateIdx(long size)
{
_overlap = _overlap < _N ? _overlap : _N-1;
long maxStart = size-_N;
_idxStart = _idxStart <= maxStart ? _idxStart : maxStart;
_idxStart = _idxStart < 0 ? 0 : _idxStart;
_idxStart = _idxStart < size ? _idxStart : size-1;
_idxStart = _idxStart < 0 ? 0 : _idxStart;
_idxEnd = _idxStart + _N - 1;
_idxEnd = _idxEnd < size ? _idxEnd : size-1;
_idxNext = _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1;
_idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1;
_idxPrevious = _idxStart + _overlap - _N;
_idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious;
......
src/Wima/WimaController.cc
View file @ 4f76dc12
......@@ -80,9 +80,11 @@ WimaController::WimaController(QObject *parent)
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _nemoHeartbeat (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _bridgeSetupDone (false)
, _pRosBridge (new ROSBridge::ROSBridge())
, _pCasePacker (_pRosBridge->casePacker())
, _pCasePacker (_pRosBridge->casePacker())
, _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
, _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/)
, _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
{
// Set up facts.
_showAllMissionItems.setRawValue(true);
......@@ -120,8 +122,6 @@ WimaController::WimaController(QObject *parent)
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
// Snake.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
_startStopRosBridge();
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
_initStartSnakeWorker();
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
......@@ -701,13 +701,9 @@ void WimaController::_checkBatteryLevel()
void WimaController::_eventTimerHandler()
{
static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
if ( enableLowBatteryHandling->rawValue().toBool() && _batteryLevelTicker.ready() )
_checkBatteryLevel();
// Snake flight plan update necessary?
......@@ -716,78 +712,25 @@ void WimaController::_eventTimerHandler()
// }
// }
if ( nemoStatusTicker.ready() ) {
if ( _nemoTimeoutTicker.ready() && _enableSnake.rawValue().toBool() ) {
this->_nemoHeartbeat.setStatus(_fallbackStatus);
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
}
if ( snakeTicker.ready() ) {
if ( _enableSnake.rawValue().toBool()
&& _pRosBridge->connected() ) {
if ( !_bridgeSetupDone ) {
_pRosBridge->start();
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
_pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
int requiredSize = this->_snakeTilesLocal.polygons().size();
auto& progress = this->_nemoProgress;
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
|| progress.progress().size() != requiredSize ) {
progress.progress().fill(0, requiredSize);
}
emit WimaController::nemoProgressChanged();
});
_pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
return;
this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
}
nemoStatusTicker.reset();
this->_fallbackStatus = -1; /*Timeout*/
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
});
auto pOrigin = &_snakeOrigin;
auto casePacker = _pCasePacker;
_pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
[casePacker, pOrigin](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc = casePacker->pack(*pOrigin, "");
casePacker->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
pReturn->AddMember("origin", value, pReturn->GetAllocator());
return pReturn;
});
auto pTiles = &_snakeTilesLocal;
_pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
[casePacker, pTiles](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc = casePacker->pack(*pTiles, "");
casePacker->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
pReturn->AddMember("tiles", value, pReturn->GetAllocator());
return pReturn;
});
_bridgeSetupDone = true;
if ( _snakeTicker.ready() ) {
if ( _enableSnake.rawValue().toBool() ) {
if ( !_pRosBridge->isRunning() && _pRosBridge->ping() ) {
_setupTopicService();
}
if ( !_pRosBridge->ping() ){
_pRosBridge->reset();
}
} else if ( _bridgeSetupDone) {
_pRosBridge->reset();
_bridgeSetupDone = false;
} else {
if ( _pRosBridge->isRunning() )
_pRosBridge->reset();
}
}
}
......@@ -933,15 +876,6 @@ void WimaController::_snakeStoreWorkerResults()
qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
}
void WimaController::_startStopRosBridge()
{
if ( _enableSnake.rawValue().toBool() ) {
_pRosBridge->start();
} else {
_pRosBridge->reset();
}
}
void WimaController::_initStartSnakeWorker()
{
if ( !_enableSnake.rawValue().toBool() )
......@@ -971,3 +905,63 @@ void WimaController::_switchSnakeManager(QVariant variant)
_switchWaypointManager(_defaultManager);
}
}
void WimaController::_setupTopicService()
{
_pRosBridge->start();
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
_pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
int requiredSize = this->_snakeTilesLocal.polygons().size();
auto& progress = this->_nemoProgress;
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
|| progress.progress().size() != requiredSize ) {
progress.progress().fill(0, requiredSize);
}
emit WimaController::nemoProgressChanged();
});
_pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc){
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
return;
this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
}
this->_nemoTimeoutTicker.reset();
this->_fallbackStatus = -1; /*Timeout*/
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
});
auto pOrigin = &_snakeOrigin;
auto casePacker = _pCasePacker;
_pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
[casePacker, pOrigin](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc = casePacker->pack(*pOrigin, "");
casePacker->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
pReturn->AddMember("origin", value, pReturn->GetAllocator());
return pReturn;
});
auto pTiles = &_snakeTilesLocal;
_pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
[casePacker, pTiles](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc = casePacker->pack(*pTiles, "");
casePacker->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
pReturn->AddMember("tiles", value, pReturn->GetAllocator());
return pReturn;
});
}
src/Wima/WimaController.h
View file @ 4f76dc12
......@@ -43,11 +43,9 @@
#include <map>
#define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
using namespace snake;
......@@ -340,9 +338,9 @@ private slots:
// Snake.
void _setSnakeCalcInProgress (bool inProgress);
void _snakeStoreWorkerResults ();
void _startStopRosBridge ();
void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant);
void _setupTopicService ();
// Periodic tasks.
void _eventTimerHandler (void);
// Waypoint manager handling.
......@@ -415,11 +413,13 @@ private:
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
const CasePacker *_pCasePacker;
bool _bridgeSetupDone;
static StatusMap _nemoStatusMap;
// Periodic tasks.
QTimer _eventTimer;
QTimer _eventTimer;
EventTicker _batteryLevelTicker;
EventTicker _snakeTicker;
EventTicker _nemoTimeoutTicker;
};
......
src/comm/ros_bridge/include/ROSBridge.h
View file @ 4f76dc12
......@@ -40,9 +40,12 @@ public:
void reset();
//! @return Returns true if connected to the rosbridge_server, false either.
//! @note This function can block up to 100ms. However normal execution time is smaller.
bool connected();
bool ping();
bool isRunning();
private:
bool _running;
TypeFactory _typeFactory;
JsonFactory _jsonFactory;
CasePacker _casePacker;
......
src/comm/ros_bridge/include/Server.cpp
View file @ 4f76dc12
......@@ -39,9 +39,20 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
return;
}
// Extract message and call callback.
if ( !document.HasMember("args") || !document["args"].IsObject()){
std::cerr << "advertiseServiceCallback(): message has no args: "
std::cerr << "advertiseServiceCallback(): message has no args member: "
<< messagebuf << std::endl;
return;
}
if ( !document.HasMember("service") || !document["service"].IsString()){
std::cerr << "advertiseServiceCallback(): message has no service member: "
<< messagebuf << std::endl;
return;
}
if ( !document.HasMember("id") || !document["id"].IsString()){
std::cerr << "advertiseServiceCallback(): message has no id member: "
<< messagebuf << std::endl;
return;
}
......@@ -51,17 +62,10 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
JsonDocUPtr pDocResponse = userCallback(std::move(pDoc));
rapidjson::OStreamWrapper out(std::cout);
// Write document...
rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
std::cout << "Ros Server: ";
(*pDocResponse).Accept(writer);
std::cout << std::endl;
rbc->serviceResponse(
document["service"].GetString(),
document["id"].GetString(),
true,
pDocResponse->MemberCount() > 0 ? true : false,
*pDocResponse);
return;
......
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @ 4f76dc12
......@@ -89,9 +89,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
auto it = clientMap.find(hash);
if ( it == clientMap.end()) { // Need to advertise topic?
clientMap.insert(std::make_pair(hash, clientName));
std::cout << clientName << ";"
<< tag.topic() << ";"
<< tag.messageType() << ";" << std::endl;
data.rbc.addClient(clientName);
data.rbc.advertise(clientName,
tag.topic(),
......
src/comm/ros_bridge/src/ROSBridge.cpp
View file @ 4f76dc12
#include "ros_bridge/include/ROSBridge.h"
ROSBridge::ROSBridge::ROSBridge() :
_casePacker(&_typeFactory, &_jsonFactory)
_running(false)
, _casePacker(&_typeFactory, &_jsonFactory)
, _rbc("localhost:9090")
, _topicPublisher(_casePacker, _rbc)
, _topicSubscriber(_casePacker, _rbc)
......@@ -37,6 +38,7 @@ void ROSBridge::ROSBridge::start()
_topicPublisher.start();
_topicSubscriber.start();
_server.start();
_running = true;
}
void ROSBridge::ROSBridge::reset()
......@@ -44,10 +46,16 @@ void ROSBridge::ROSBridge::reset()
_topicPublisher.reset();
_topicSubscriber.reset();
_server.reset();
_running = false;
}
bool ROSBridge::ROSBridge::connected()
bool ROSBridge::ROSBridge::ping()
{
return _rbc.connected();
}
bool ROSBridge::ROSBridge::isRunning()
{
return _running;
}
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