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
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/RosBridgeClient.h
View file @ 4f76dc12
......@@ -16,15 +16,36 @@
#include <thread>
#include <future>
#include <mutex>
#include <tuple>
#include <deque>
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value)
{
return static_cast<typename std::underlying_type<T>::type>(value);
}
class RosbridgeWsClient
{
using TopicData = std::tuple<std::string /*topic*/,
std::string /*client name*/,
std::shared_ptr<WsClient> /*client*/,
std::shared_ptr<std::atomic_bool> /*topic ready*/>;
enum class TopicEnum{
TopicName = 0,
ClientName = 1,
Client = 2,
Ready = 3
};
std::string server_port_path;
std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
std::mutex map_mutex;
std::unordered_map<std::string /*client name*/,
std::shared_ptr<WsClient> /*client*/> client_map;
std::deque<TopicData> advertised_topics_list;
std::mutex mutex;
void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
{
......@@ -66,8 +87,8 @@ class RosbridgeWsClient
std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
};
}
#endif
#endif
#ifdef DEBUG
std::thread client_thread([client_name, client]() {
#else
......@@ -95,11 +116,13 @@ public:
~RosbridgeWsClient()
{
std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
for (auto& topicData : advertised_topics_list){
unadvertise(std::get<integral(TopicEnum::ClientName)>(topicData),
std::get<integral(TopicEnum::TopicName)>(topicData));
}
for (auto& client : client_map)
{
client.second->stop();
client.second.reset();
removeClient(client.first);
}
}
......@@ -133,7 +156,7 @@ public:
// Adds a client to the client_map.
void addClient(const std::string& client_name)
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it == client_map.end())
{
......@@ -149,7 +172,7 @@ public:
std::shared_ptr<WsClient> getClient(const std::string& client_name)
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......@@ -160,7 +183,7 @@ public:
void stopClient(const std::string& client_name)
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......@@ -183,23 +206,26 @@ public:
void removeClient(const std::string& client_name)
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
std::thread t([](std::shared_ptr<WsClient> client){
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client->stop();
client.reset();
},
it->second /*lambda param*/ );
client_map.erase(it);
t.detach();
std::shared_ptr<WsClient> client = it->second;
#ifdef DEBUG
std::thread t([client, client_name](){
#else
std::thread t([client](){
#endif
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client->stop();
#ifdef DEBUG
std::cout << client_name << " has been removed" << std::endl;
#endif
});
client_map.erase(it);
t.detach();
}
#ifdef DEBUG
else
......@@ -213,19 +239,51 @@ public:
// One client per topic!
void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
{
std::lock_guard<std::mutex> lk(map_mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
if (it_client != client_map.end())
{
std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
auto it_topic = std::find_if(advertised_topics_list.begin(),
advertised_topics_list.end(),
[topic](const TopicData &td){
return topic == std::get<integral(TopicEnum::TopicName)>(td);
});
if ( it_topic != advertised_topics_list.end()){
#ifdef DEBUG
std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
return;
}
auto client = it_client->second;
auto ready = std::make_shared<std::atomic_bool>(false);
advertised_topics_list.push_back(std::make_tuple(topic, client_name, client, ready));
std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message = "{" + message + "}";
start(client_name, it->second, message);
#ifdef DEBUG
client->on_open = [client_name, message, ready](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message, ready](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message);
// Wait for rosbridge_server to process the request and mark topic as "ready".
// This could be avoided by waiting for a status message. However, at the time of
// writing rosbridge_server status messages are still experimental.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
ready->store(true);
};
start(client_name, client, message);
}
#ifdef DEBUG
else
......@@ -235,12 +293,83 @@ public:
#endif
}
void unadvertise(const std::string& client_name,
const std::string& topic,
const std::string& id = ""){
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
if (it_client != client_map.end())
{
// Topic advertised?
auto it_topic = std::find_if(advertised_topics_list.begin(),
advertised_topics_list.end(),
[topic](const TopicData &td){
return topic == std::get<integral(TopicEnum::TopicName)>(td);
});
if ( it_topic == advertised_topics_list.end()){
#ifdef DEBUG
std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
return;
}
std::string message = "\"op\":\"unadvertise\"";
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message += ", \"topic\":\"" + topic + "\"";
message = "{" + message + "}";
auto client = it_client->second;
auto ready = std::get<integral(TopicEnum::Ready)>(*it_topic);
#ifdef DEBUG
client->on_open = [client_name, message, ready](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message, ready](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
while ( !ready->load() ){ // Wait for topic to be advertised.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
connection->send(message);
};
start(client_name, client, message);
advertised_topics_list.erase(it_topic);
}
#ifdef DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
}
#endif
}
void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "")
{
std::lock_guard<std::mutex> lk(mutex);
auto it_topic = std::find_if(advertised_topics_list.begin(),
advertised_topics_list.end(),
[&topic](const TopicData &td){
return topic == std::get<integral(TopicEnum::TopicName)>(td);
});
if ( it_topic == advertised_topics_list.end() ){
#ifdef DEBUG
std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
#endif
return;
}
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
msg.Accept(writer);
std::string client_name = "publish_client" + topic;
std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();
if (id.compare("") != 0)
......@@ -250,24 +379,28 @@ public:
message = "{" + message + "}";
std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
auto ready = std::get<integral(TopicEnum::Ready)>(*it_topic);
publish_client->on_open = [message, client_name, ready](std::shared_ptr<WsClient::Connection> connection) {
#ifdef DEBUG
std::cout << "publish_client: Opened connection" << std::endl;
std::cout << "publish_client: Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message." << std::endl;
//std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
while ( !ready->load() ){ // Wait for the topic to be advertised.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
connection->send(message);
// TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
connection->send_close(1000);
};
start("publish_client", publish_client, message);
start(client_name, publish_client, message);
}
void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "")
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......@@ -312,7 +445,7 @@ public:
void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
{
std::lock_guard<std::mutex> lk(map_mutex);
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......
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;
}