Commit 571dc451 authored by Valentin Platzgummer's avatar Valentin Platzgummer

Ros Bridge tested with remote connection.

parent fd49a04a
......@@ -32,7 +32,7 @@ DebugBuild {
}
else {
DESTDIR = $${OUT_PWD}/release
DEFINES += DEBUG
#DEFINES += DEBUG
DEFINES += NDEBUG
}
......
......@@ -18,7 +18,7 @@
"type": "uint64",
"units": "s",
"defaultValue": 15
}
},
{
"name": "rosbridgeConnectionString",
"shortDescription": "Ros Bridge Connection String (host:port).",
......
......@@ -22,7 +22,6 @@
// const char* WimaController::wimaFileExtension = "wima";
const char* WimaController::areaItemsName = "AreaItems";
const char* WimaController::missionItemsName = "MissionItems";
const char* WimaController::settingsGroup = "WimaController";
......@@ -63,8 +62,9 @@ WimaController::WimaController(QObject *parent)
, _rtlManager (_managerSettings, _areaInterface)
, _currentManager (&_defaultManager)
, _managerList {&_defaultManager, &_snakeManager, &_rtlManager}
, _metaDataMap (FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _metaDataMap (
FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
, _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
......@@ -84,12 +84,26 @@ WimaController::WimaController(QObject *parent)
, _snakeCalcInProgress (false)
, _nemoHeartbeat (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _pRosBridge (new ROSBridge::ROSBridge())
, _pCasePacker (_pRosBridge->casePacker())
, _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
, _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/)
, _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
{
{
// ROS Bridge.
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
auto setConnectionString = [connectionStringFact, this]{
auto connectionString = connectionStringFact->rawValue().toString();
if ( ROSBridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
this->_pRosBridge.reset(new ROSBridge::ROSBridge(connectionString.toLocal8Bit().data()));
} else {
qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
this->_pRosBridge.reset(new ROSBridge::ROSBridge("localhost:9090"));
}
};
setConnectionString();
connect(wimaSettings->rosbridgeConnectionString(), &SettingsFact::rawValueChanged, setConnectionString);
// Set up facts.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
......@@ -964,11 +978,11 @@ void WimaController::_setupTopicService()
[this](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc;
if ( this->_snakeTilesLocal.polygons().size() > 0){
pDoc = this->_pCasePacker->pack(this->_snakeOrigin, "");
pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeOrigin, "");
} else {
pDoc = this->_pCasePacker->pack(::GeoPoint3D(0,0,0), "");
pDoc = this->_pRosBridge->casePacker()->pack(::GeoPoint3D(0,0,0), "");
}
this->_pCasePacker->removeTag(pDoc);
this->_pRosBridge->casePacker()->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
......@@ -979,8 +993,8 @@ void WimaController::_setupTopicService()
_pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr {
JsonDocUPtr pDoc = this->_pCasePacker->pack(this->_snakeTilesLocal, "");
this->_pCasePacker->removeTag(pDoc);
JsonDocUPtr pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeTilesLocal, "");
this->_pRosBridge->casePacker()->removeTag(pDoc);
rapidjson::Document value(rapidjson::kObjectType);
JsonDocUPtr pReturn(new rapidjson::Document(rapidjson::kObjectType));
value.CopyFrom(*pDoc, pReturn->GetAllocator());
......
......@@ -406,7 +406,6 @@ private:
QNemoHeartbeat _nemoHeartbeat; // measurement progress
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
const CasePacker *_pCasePacker;
static StatusMap _nemoStatusMap;
// Periodic tasks.
......
......@@ -20,6 +20,7 @@ public:
typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
explicit ROSBridge();
explicit ROSBridge(const char* connectionString);
template<class T>
void publish(T &msg, const std::string &topic){
......@@ -55,4 +56,6 @@ private:
ComPrivate::Server _server;
};
bool isValidConnectionString(const char* connectionString);
}
#include "RosBridgeClient.h"
#include <chrono>
#include <functional>
#include <thread>
#include <future>
#include <regex>
struct Task{
......@@ -120,8 +122,10 @@ void RosbridgeWsClient::run()
// ====================================================================================
if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
#ifdef DEBUG
std::cout << "Starting new poll." << std::endl;
std::cout << "connected: " << this->isConnected->load() << std::endl;
#endif
std::string reset_status_task_name = "reset_status_task";
// Add status task if necessary.
auto const it = std::find_if(task_list.begin(), task_list.end(),
......@@ -129,12 +133,16 @@ void RosbridgeWsClient::run()
return t.name == reset_status_task_name;
});
if ( it == task_list.end() ){
#ifdef DEBUG
std::cout << "Adding status_task" << std::endl;
#endif
// Check connection status.
auto status_set = std::make_shared<std::atomic_bool>(false);
auto status_client = std::make_shared<WsClient>(this->server_port_path);
status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
#ifdef DEBUG
std::cout << "status_client opened" << std::endl;
#endif
this->isConnected->store(true);
status_set->store(true);
};
......@@ -182,12 +190,16 @@ void RosbridgeWsClient::run()
});
if ( topics_it == task_list.end() ){
// Call /rosapi/topics service.
#ifdef DEBUG
std::cout << "Adding reset_topics_task" << std::endl;
#endif
auto topics_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/topics", [topics_set, this](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
//std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
#ifdef DEBUG
std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
#endif
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics = in_message->string();
lk.unlock();
......@@ -229,12 +241,16 @@ void RosbridgeWsClient::run()
});
if ( services_it == task_list.end() ){
// Call /rosapi/services service.
#ifdef DEBUG
std::cout << "Adding reset_services_task" << std::endl;
#endif
auto services_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/services", [this, services_set](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
//std::cout << "/rosapi/services: " << in_message->string() << std::endl;
#ifdef DEBUG
std::cout << "/rosapi/services: " << in_message->string() << std::endl;
#endif
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services = in_message->string();
lk.unlock();
......@@ -279,22 +295,29 @@ void RosbridgeWsClient::run()
// Process tasks.
// ====================================================================================
for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
//std::cout << "processing task: " << task_it->name << std::endl;
#ifdef DEBUG
std::cout << "processing task: " << task_it->name << std::endl;
#endif
if ( !task_it->expired() ){
if ( task_it->ready() ){
//std::cout << "executing task: " << task_it->name << std::endl;
#ifdef DEBUG
std::cout << "executing task: " << task_it->name << std::endl;
#endif
task_it->execute();
task_it = task_list.erase(task_it);
} else {
//std::cout << "noting to do for task: " << task_it->name << std::endl;
#ifdef DEBUG
std::cout << "noting to do for task: " << task_it->name << std::endl;
#endif
++task_it;
}
} else {
//std::cout << "task expired: " << task_it->name << std::endl;
#ifdef DEBUG
std::cout << "task expired: " << task_it->name << std::endl;
#endif
task_it->clear_up();
task_it = task_list.erase(task_it);
}
//std::cout << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
......@@ -305,7 +328,9 @@ void RosbridgeWsClient::run()
task_it->clear_up();
}
task_list.clear();
#ifdef DEBUG
std::cout << "periodic thread end" << std::endl;
#endif
});
}
......@@ -571,7 +596,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
#ifdef DEBUG
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;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message);
......@@ -949,3 +974,11 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::functi
<< counter << std::endl;
#endif
}
bool is_valid_port_path(std::string server_port_path)
{
std::regex url_regex("^(((([a-z]|[A-z])+([0-9]|_)*\\.*([a-z]|[A-z])+([0-9]|_)*))"
"|(((1?[0-9]{1,2}|2[0-4][0-9]|25[0-5])\\.){3}(1?[0-9]{1,2}|2[0-4][0-9]|25[0-5]){1}))"
":[0-9]+$");
return std::regex_match(server_port_path, url_regex);
}
......@@ -17,6 +17,8 @@
#include <deque>
#include <thread>
bool is_valid_port_path(std::string server_port_path);
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
......
#include "ros_bridge/include/ROSBridge.h"
ROSBridge::ROSBridge::ROSBridge() :
ROSBridge::ROSBridge::ROSBridge(const char *connectionString) :
_stopped(std::make_shared<std::atomic_bool>(true))
, _casePacker(&_typeFactory, &_jsonFactory)
, _rbc("localhost:9090", false /*run*/)
, _rbc(connectionString, false /*run*/)
, _topicPublisher(_casePacker, _rbc)
, _topicSubscriber(_casePacker, _rbc)
, _server(_casePacker, _rbc)
{
}
ROSBridge::ROSBridge::ROSBridge() :
ROSBridge::ROSBridge::ROSBridge("localhost:9090")
{
}
void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc)
{
_topicPublisher.publish(std::move(doc));
......@@ -64,3 +69,8 @@ bool ROSBridge::ROSBridge::isRunning()
return !_stopped->load();
}
bool ROSBridge::isValidConnectionString(const char *connectionString)
{
return is_valid_port_path(connectionString);
}
......@@ -983,13 +983,8 @@ QGCView {
visible: _userBrandImageIndoor.visible
}
FactTextField {
Layout.preferredWidth: _valueFieldWidth
fact: QGroundControl.settingsManager.wimaSettings.rosbridgeConnectionString
Layout.columnSpan: 2
}
Item{
// dummy
width: 1
Layout.columnSpan: 3
}
}
}
......
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