Skip to content

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

Merge branch 'master' of ssh://squid.nt.tuwien.ac.at:22103/platzgummer/qgroundcontrol

parents 4bb718fe 96d3005b
Hide whitespace changes
Inline Side-by-side
Showing with 652 additions and 413 deletions
qgroundcontrol.pro
View file @ a0ede0e6
......@@ -27,6 +27,8 @@ QGCROOT = $$PWD
DebugBuild {
DESTDIR = $${OUT_PWD}/debug
#DEFINES += DEBUG
#DEFINES += ROS_BRIDGE_CLIENT_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ a0ede0e6
......@@ -251,7 +251,8 @@ FlightMap {
delegate: MapCircle{
center: modelData
border.color: "transparent"
border.color: "black"
border.width: 1
color: getColor(wimaController.nemoProgress[index])
radius: 0.6
opacity: 1
......
src/Snake/snake.cpp
View file @ a0ede0e6
......@@ -4,6 +4,13 @@
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace operations_research;
#ifndef NDEBUG
//#define SHOW_TIME
#endif
......@@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area)
return false;
}
bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea)
bool Scenario::update(double tileWidth, double tileHeight, double minTileArea)
{
if (!_areas2enu())
return false;
......@@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea()
return true;
}
struct FlightPlan::RoutingDataModel{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
FlightPlan::FlightPlan()
{
......@@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel_t dataModel;
RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1);
#ifdef SHOW_TIME
......@@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
FlightPlan::RoutingDataModel_t &dataModel,
FlightPlan::RoutingDataModel &dataModel,
Matrix<double> &graph)
{
#ifdef SHOW_TIME
......
src/Snake/snake.h
View file @ a0ede0e6
......@@ -6,16 +6,9 @@
#include "snake_geometry.h"
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace std;
using namespace snake_geometry;
using namespace operations_research;
namespace snake {
enum AreaType {MeasurementArea, ServiceArea, Corridor};
......@@ -60,7 +53,7 @@ namespace snake {
const vector<GeoPoint3D> &getTileCenterPoints() {return _tileCenterPoints;}
const GeoPoint3D &getHomePositon() {return _homePosition;}
bool defined(double tileWidth, double tileHeight, double minTileArea);
bool update(double tileWidth, double tileHeight, double minTileArea);
string errorString;
......@@ -104,8 +97,8 @@ namespace snake {
public:
FlightPlan();
void setScenario(const Scenario &scenario) {_scenario = scenario;}
void setProgress(const vector<int8_t> &progress) {_progress = progress;}
void setScenario(Scenario &scenario) {_scenario = scenario;}
void setProgress(vector<int> &progress) {_progress = progress;}
const Scenario &getScenario(void) const {return _scenario;}
const BoostLineString &getWaypointsENU(void) const {return _waypointsENU;}
......@@ -124,28 +117,22 @@ namespace snake {
private:
// Search Filter to speed up routing.SolveWithParameters(...);
// Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
class SearchFilter : public LocalSearchFilter{
};
class SearchFilter;
typedef struct{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
}RoutingDataModel_t;
struct RoutingDataModel;
bool _generateTransects(double lineDistance, double minTransectLength);
void _generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
RoutingDataModel_t &dataModel,
RoutingDataModel &dataModel,
Matrix<double> &graph);
Scenario _scenario;
BoostLineString _waypointsENU;
GeoPoint2DList _waypoints;
vector<BoostLineString> _transects;
vector<int8_t> _progress;
vector<int> _progress;
vector<uint64_t> _arrivalPathIdx;
vector<uint64_t> _returnPathIdx;
......
src/Wima/Geometry/GeoPoint3D.cpp
View file @ a0ede0e6
#include "GeoPoint3D.h"
GeoPoint3D::GeoPoint3D(QObject *parent)
GeoPoint::GeoPoint(QObject *parent)
: QObject(parent), ROSGeoPoint() {}
GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent)
: QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
{}
GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent)
: QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
{}
GeoPoint3D *GeoPoint3D::Clone() const
GeoPoint *GeoPoint::Clone() const
{
return new GeoPoint3D(*this);
return new GeoPoint(*this);
}
GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
GeoPoint &GeoPoint::operator=(const GeoPoint &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
......@@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
return *this;
}
GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p)
{
this->setLatitude(p.latitude());
this->setLongitude(p.longitude());
......
src/Wima/Geometry/GeoPoint3D.h
View file @ a0ede0e6
......@@ -11,27 +11,27 @@
typedef ROSBridge::MessageBaseClass ROSMsg;
typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
namespace MsgGroups = ROSBridge::MessageGroups;
class GeoPoint3D : public QObject, public ROSGeoPoint
class GeoPoint : public QObject, public ROSGeoPoint
{
Q_OBJECT
public:
typedef MsgGroups::GeoPointGroup Group;
GeoPoint3D(QObject *parent = nullptr);
GeoPoint3D(double latitude,
GeoPoint(QObject *parent = nullptr);
GeoPoint(double latitude,
double longitude,
double altitude,
QObject *parent = nullptr);
GeoPoint3D(const GeoPoint3D& p,
GeoPoint(const GeoPoint& p,
QObject *parent = nullptr);
GeoPoint3D(const ROSGeoPoint& p,
GeoPoint(const ROSGeoPoint& p,
QObject *parent = nullptr);
GeoPoint3D(const QGeoCoordinate& p,
GeoPoint(const QGeoCoordinate& p,
QObject *parent = nullptr);
virtual GeoPoint3D *Clone() const override;
GeoPoint3D &operator=(const GeoPoint3D&p);
GeoPoint3D &operator=(const QGeoCoordinate&p);
virtual GeoPoint *Clone() const override;
GeoPoint &operator=(const GeoPoint&p);
GeoPoint &operator=(const QGeoCoordinate&p);
};
......
src/Wima/Snake/SnakeWorker.cc
View file @ a0ede0e6
......@@ -2,58 +2,84 @@
#include <QGeoCoordinate>
#include "snake_geometry.h"
#include "snake.h"
using namespace snake;
using namespace snake_geometry;
SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
, _lineDistance (3) // meter
, _minTransectLength (2) // meter
, _lineDistance(3)
, _minTransectLength(2)
, _tileWidth(5)
, _tileHeight(5)
, _minTileArea(3)
{
}
SnakeWorker::~SnakeWorker()
{
}
void SnakeWorker::setScenario(const Scenario &scenario)
{
_scenario = scenario;
}
void SnakeWorker::setProgress(const QVector<int> &progress)
WorkerResult_t &SnakeWorker::result()
{
_progress.clear();
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
return _result;
}
void SnakeWorker::setLineDistance(double lineDistance)
bool SnakeWorker::precondition() const
{
assert(lineDistance > 0);
_lineDistance = lineDistance;
}
void SnakeWorker::setMinTransectLength(double minTransectLength)
{
assert(minTransectLength > 0);
_minTransectLength = minTransectLength;
return _mArea.size() > 0
&& _sArea.size() > 0
&& _corridor.size() > 0
&& _progress.size() > 0;
}
const WorkerResult_t &SnakeWorker::getResult() const
double SnakeWorker::lineDistance() const
{
return _result;
return _lineDistance;
}
void SnakeWorker::run()
{
// Reset _result struct.
_result.success = false;
_result.errorMessage = "";
_result.waypoints.clear();
_result.returnPathIdx.clear();
_result.arrivalPathIdx.clear();
_result.clear();
if ( !precondition())
return;
Scenario scenario;// Initialize scenario.
Area mArea;
for (auto vertex : _mArea){
mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
}
mArea.type = AreaType::MeasurementArea;
Area sArea;
for (auto vertex : _sArea){
sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
}
sArea.type = AreaType::ServiceArea;
Area corridor;
for (auto vertex : _corridor){
corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
}
corridor.type = AreaType::Corridor;
scenario.addArea(mArea);
scenario.addArea(sArea);
scenario.addArea(corridor);
if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) )
return;
FlightPlan flightplan;
flightplan.setScenario(_scenario);
flightplan.setScenario(scenario);
flightplan.setProgress(_progress);
// Trying to generate flight plan.
......@@ -67,20 +93,116 @@ void SnakeWorker::run()
_result.success = true;
// Fill result struct.
auto waypoints = flightplan.getWaypoints();
auto& waypoints = flightplan.getWaypoints();
for (auto vertex : waypoints){
_result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0)));
_result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0));
}
auto arrivalPathIdx = flightplan.getArrivalPathIdx();
auto& arrivalPathIdx = flightplan.getArrivalPathIdx();
for (auto idx : arrivalPathIdx){
_result.arrivalPathIdx.append(idx);
}
auto returnPathIdx = flightplan.getReturnPathIdx();
auto& returnPathIdx = flightplan.getReturnPathIdx();
for (auto idx : returnPathIdx){
_result.returnPathIdx.append(idx);
}
// Get tiles and origin.
auto origin = scenario.getOrigin();
_result.origin.setLatitude(origin[0]);
_result.origin.setLongitude(origin[1]);
_result.origin.setAltitude(origin[2]);
const auto &tiles = scenario.getTiles();
const auto &cps = scenario.getTileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
SnakeTile Tile;
for ( const auto &vertex : tile) {
QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
Tile.append(QVertex);
}
const auto &centerPoint = cps[i];
QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
Tile.setCenter(QCenterPoint);
_result.tiles.polygons().append(Tile);
_result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint));
}
// Get local tiles.
const auto &tilesENU = scenario.getTilesENU();
for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
const auto &tile = tilesENU[i];
Polygon2D Tile;
for ( const auto &vertex : tile.outer()) {
QPointF QVertex(vertex.get<0>(), vertex.get<1>());
Tile.path().append(QVertex);
}
_result.tilesLocal.polygons().append(Tile);
}
}
}
double SnakeWorker::minTransectLength() const
{
return _minTransectLength;
}
void SnakeWorker::setMinTransectLength(double minTransectLength)
{
if (minTransectLength > 0)
_minTransectLength = minTransectLength;
}
double SnakeWorker::minTileArea() const
{
return _minTileArea;
}
void SnakeWorker::setLineDistance(double lineDistance)
{
if (lineDistance > 0)
_lineDistance = lineDistance;
}
double SnakeWorker::tileWidth() const
{
return _tileWidth;
}
void SnakeWorker::setTileWidth(double tileWidth)
{
if (tileWidth > 0)
_tileWidth = tileWidth;
}
double SnakeWorker::tileHeight() const
{
return _tileHeight;
}
void SnakeWorker::setTileHeight(double tileHeight)
{
if (tileHeight > 0)
_tileHeight = tileHeight;
}
void SnakeWorker::setMinTileArea(double minTileArea)
{
if (minTileArea > 0)
_minTileArea = minTileArea;
}
void Result::clear()
{
this->success = false;
this->errorMessage = "";
this->waypoints.clear();
this->returnPathIdx.clear();
this->arrivalPathIdx.clear();
this->tiles.polygons().clear();
this->tilesLocal.polygons().clear();
this->tileCenterPoints.clear();
}
src/Wima/Snake/SnakeWorker.h
View file @ a0ede0e6
......@@ -4,23 +4,27 @@
#include <QVariant>
#include <QThread>
#include <QVector>
#include <QGeoCoordinate>
#include <vector>
#include "snake_geometry.h"
#include "snake.h"
using namespace snake;
using namespace snake_geometry;
using namespace std;
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
typedef QList<QVariant> QVariantList;
typedef struct Result{
QVariantList waypoints;
QVector<QGeoCoordinate> waypoints;
SnakeTiles tiles;
SnakeTilesLocal tilesLocal;
QVariantList tileCenterPoints;
QGeoCoordinate origin;
QVector<unsigned long> arrivalPathIdx;
QVector<unsigned long> returnPathIdx;
bool success;
QString errorMessage;
QString errorMessage;
void clear();
}WorkerResult_t;
class SnakeWorker : public QThread{
......@@ -30,21 +34,86 @@ public:
SnakeWorker(QObject *parent = nullptr);
~SnakeWorker() override;
void setScenario (const Scenario &scenario);
void setProgress (const QVector<int> &progress);
void setLineDistance (double lineDistance);
void setMinTransectLength (double minTransectLength);
const WorkerResult_t &getResult() const;
template<template<class, class...> class Container>
void setMeasurementArea (const Container<QGeoCoordinate> &measurementArea);
template<template<class, class...> class Container>
void setServiceArea (const Container<QGeoCoordinate> &serviceArea);
template<template<class, class...> class Container>
void setCorridor (const Container<QGeoCoordinate> &corridor);
template<template<class, class...> class Container, class IntType>
void setProgress (const Container<IntType> &progress);
WorkerResult_t &result();
double lineDistance() const;
void setLineDistance(double lineDistance);
double minTransectLength() const;
void setMinTransectLength(double minTransectLength);
double minTileArea() const;
void setMinTileArea(double minTileArea);
double tileHeight() const;
void setTileHeight(double tileHeight);
double tileWidth() const;
void setTileWidth(double tileWidth);
protected:
void run() override;
private:
Scenario _scenario;
vector<int8_t> _progress;
double _lineDistance;
double _minTransectLength;
WorkerResult_t _result;
bool precondition() const;
std::vector<QGeoCoordinate> _mArea;
std::vector<QGeoCoordinate> _sArea;
std::vector<QGeoCoordinate> _corridor;
std::vector<int> _progress;
double _lineDistance;
double _minTransectLength;
double _tileWidth;
double _tileHeight;
double _minTileArea;
WorkerResult_t _result;
};
template<template<class, class...> class Container>
void SnakeWorker::setCorridor(const Container<QGeoCoordinate> &corridor)
{
_corridor.clear();
for (auto vertex : corridor) {
_corridor.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setMeasurementArea(const Container<QGeoCoordinate> &measurementArea)
{
_mArea.clear();
for (auto vertex : measurementArea) {
_mArea.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setServiceArea(const Container<QGeoCoordinate> &serviceArea)
{
_sArea.clear();
for (auto vertex : serviceArea) {
_sArea.push_back(vertex);
}
}
template<template<class, class...> class Container, class IntType>
void SnakeWorker::setProgress(const Container<IntType> &progress)
{
_progress.clear();
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
}
src/Wima/WaypointManager/Slicer.cpp
View file @ a0ede0e6
......@@ -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 @ a0ede0e6
......@@ -55,6 +55,7 @@ WimaController::WimaController(QObject *parent)
, _snakeManager (_managerSettings, _areaInterface)
, _rtlManager (_managerSettings, _areaInterface)
, _currentManager (&_defaultManager)
, _managerList {&_defaultManager, &_snakeManager, &_rtlManager}
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
......@@ -68,14 +69,14 @@ WimaController::WimaController(QObject *parent)
, _measurementPathLength (-1)
, _lowBatteryHandlingTriggered (false)
, _snakeCalcInProgress (false)
, _scenarioDefinedBool (false)
, _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName])
, _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName])
, _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName])
, _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName])
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _nemoHeartbeat (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _bridgeSetupDone (false)
, _pRosBridge (new ROSBridge::ROSBridge())
{
// Set up facts.
......@@ -88,9 +89,20 @@ WimaController::WimaController(QObject *parent)
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
_defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
_defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
_defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());
// Init waypoint managers.
bool value;
size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
Q_ASSERT(value);
(void)value;
for (auto manager : _managerList){
manager->setOverlap(overlap);
manager->setN(N);
manager->setStartIndex(startIdx);
}
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
......@@ -103,10 +115,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);
_switchSnakeManager(_enableSnake.rawValue());
}
......@@ -342,13 +350,6 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCo
return retVal;
}
/*!
* \fn void WimaController::containerDataValidChanged(bool valid)
* Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
* Is connected to the dataValidChanged() signal of the \c WimaDataContainer.
*
* \sa WimaDataContainer, WimaPlaner, WimaPlanData
*/
bool WimaController::setWimaPlanData(const WimaPlanData &planData)
{
// fetch only if valid, return true on success
......@@ -446,80 +447,10 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
emit waypointPathChanged();
emit currentWaypointPathChanged();
_localPlanDataValid = true;
_initStartSnakeWorker();
// Initialize _scenario.
Area mArea;
for (auto variant : _measurementArea.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
mArea.type = AreaType::MeasurementArea;
Area sArea;
for (auto variant : _serviceArea.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
sArea.type = AreaType::ServiceArea;
Area corridor;
for (auto variant : _corridor.path()){
QGeoCoordinate c{variant.value<QGeoCoordinate>()};
corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
}
corridor.type = AreaType::Corridor;
_scenario.addArea(mArea);
_scenario.addArea(sArea);
_scenario.addArea(corridor);
// Check if scenario is defined.
if ( !_isScenarioDefinedErrorMessage() ) {
Q_ASSERT(false);
return false;
}
{
// Get tiles and origin.
auto origin = _scenario.getOrigin();
_snakeOrigin.setLatitude(origin[0]);
_snakeOrigin.setLongitude(origin[1]);
_snakeOrigin.setAltitude(origin[2]);
const auto &tiles = _scenario.getTiles();
const auto &cps = _scenario.getTileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
SnakeTile Tile;
for ( const auto &vertex : tile) {
QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
Tile.append(QVertex);
}
const auto &centerPoint = cps[i];
QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
Tile.setCenter(QCenterPoint);
_snakeTiles.polygons().append(Tile);
_snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint));
}
}
{
// Get local tiles.
const auto &tiles = _scenario.getTilesENU();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
Polygon2D Tile;
for ( const auto &vertex : tile.outer()) {
QPointF QVertex(vertex.get<0>(), vertex.get<1>());
Tile.path().append(QVertex);
}
_snakeTilesLocal.polygons().append(Tile);
}
}
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
_localPlanDataValid = true;
return true;
}
......@@ -692,8 +623,7 @@ void WimaController::_checkBatteryLevel()
void WimaController::_eventTimerHandler()
{
static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000);
static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
// Battery level check necessary?
......@@ -713,29 +643,72 @@ void WimaController::_eventTimerHandler()
emit WimaController::nemoStatusStringChanged();
}
if (_enableSnake.rawValue().toBool() && rosBridgeTicker.ready()) {
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
using namespace std::placeholders;
auto progressCallBack = std::bind(&WimaController::_progressFromJson,
this,
_1,
std::ref(_nemoProgress));
_pRosBridge->subscribe("/nemo/progress", progressCallBack);
_pRosBridge->subscribe("/nemo/heartbeat", [this, &nemoStatusTicker](JsonDocUPtr pDoc){
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
return;
this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
if ( snakeTicker.ready() ) {
if ( _enableSnake.rawValue().toBool()
&& _pRosBridge->connected() ) {
if ( !_bridgeSetupDone ) {
qWarning() << "ROS Bridge setup. ";
auto start = std::chrono::high_resolution_clock::now();
_pRosBridge->start();
auto end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->start() time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_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();
});
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_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();
});
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
_bridgeSetupDone = true;
}
nemoStatusTicker.reset();
this->_fallbackStatus = -1; /*Timeout*/
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
});
} else if ( _bridgeSetupDone) {
_pRosBridge->reset();
_bridgeSetupDone = false;
}
}
}
......@@ -784,14 +757,23 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage
if (_currentManager != &manager) {
_currentManager = &manager;
bool value;
_currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
Q_ASSERT(value);
_currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
_currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
Q_ASSERT(value);
(void)value;
disconnect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
_maxWaypointsPerPhase.setRawValue(_currentManager->N());
_overlapWaypoints.setRawValue(_currentManager->overlap());
_nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
connect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
emit missionItemsChanged();
emit currentMissionItemsChanged();
......@@ -834,35 +816,15 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
}
}
bool WimaController::_isScenarioDefined()
{
_scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(),
_snakeTileHeight.rawValue().toDouble(),
_snakeMinTileArea.rawValue().toDouble());
return _scenarioDefinedBool;
}
bool WimaController::_isScenarioDefinedErrorMessage()
{
bool value = _isScenarioDefined();
if (!value){
QString errorString;
for (auto c : _scenario.errorString)
errorString.push_back(c);
qgcApp()->showMessage(errorString);
}
return value;
}
void WimaController::_snakeStoreWorkerResults()
{
auto start = std::chrono::high_resolution_clock::now();
_snakeManager.clear();
const WorkerResult_t &r = _snakeWorker.getResult();
const WorkerResult_t &r = _snakeWorker.result();
_setSnakeCalcInProgress(false);
if (!r.success) {
qgcApp()->showMessage(r.errorMessage);
//qgcApp()->showMessage(r.errorMessage);
return;
}
......@@ -876,7 +838,7 @@ void WimaController::_snakeStoreWorkerResults()
unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first();
for (unsigned long i = startIdx; i <= endIdx; ++i) {
_snakeManager.push_back(r.waypoints[int(i)].value<QGeoCoordinate>());
_snakeManager.push_back(r.waypoints[int(i)]);
}
_snakeManager.update();
......@@ -891,15 +853,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() )
......@@ -911,10 +864,15 @@ void WimaController::_initStartSnakeWorker()
}
// Initialize _snakeWorker.
_snakeWorker.setScenario(_scenario);
_snakeWorker.setMeasurementArea(_measurementArea.coordinateList());
_snakeWorker.setServiceArea(_serviceArea.coordinateList());
_snakeWorker.setCorridor(_corridor.coordinateList());
_snakeWorker.setProgress(_nemoProgress.progress());
_snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
_snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
_snakeWorker.setTileHeight(_snakeTileHeight.rawValue().toDouble());
_snakeWorker.setTileWidth(_snakeTileWidth.rawValue().toDouble());
_snakeWorker.setMinTileArea(_snakeMinTileArea.rawValue().toDouble());
_setSnakeCalcInProgress(true);
// Start worker thread.
......@@ -929,16 +887,3 @@ void WimaController::_switchSnakeManager(QVariant variant)
_switchWaypointManager(_defaultManager);
}
}
void WimaController::_progressFromJson(JsonDocUPtr pDoc,
QNemoProgress &progress)
{
int requiredSize = _snakeTilesLocal.polygons().size();
if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
|| progress.progress().size() != requiredSize ) {
progress.progress().fill(0, requiredSize);
}
emit WimaController::nemoProgressChanged();
}
src/Wima/WimaController.h
View file @ a0ede0e6
......@@ -46,7 +46,7 @@
#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 5000 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
using namespace snake;
......@@ -338,10 +338,7 @@ private slots:
void _smartRTLCleanUp (bool flying);
// Snake.
void _setSnakeCalcInProgress (bool inProgress);
bool _isScenarioDefined (void);
bool _isScenarioDefinedErrorMessage (void);
void _snakeStoreWorkerResults ();
void _startStopRosBridge ();
void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant);
// Periodic tasks.
......@@ -351,16 +348,12 @@ private slots:
private:
using StatusMap = std::map<int, QString>;
// Snake.
void _progressFromJson (JsonDocUPtr pDoc,
QNemoProgress &progress);
// Controllers.
PlanMasterController *_masterController;
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
......@@ -375,6 +368,8 @@ private:
WaypointManager::DefaultManager _snakeManager;
WaypointManager::RTLManager _rtlManager;
WaypointManager::ManagerBase *_currentManager;
using ManagerList = QList<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts.
QMap<QString, FactMetaData*> _metaDataMap;
......@@ -406,11 +401,8 @@ private:
// Snake
bool _snakeCalcInProgress;
bool _snakeRecalcNecessary;
bool _scenarioDefinedBool;
SnakeWorker _snakeWorker;
Scenario _scenario;
::GeoPoint3D _snakeOrigin;
GeoPoint _snakeOrigin;
SnakeTiles _snakeTiles; // tiles
SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system
QVariantList _snakeTileCenterPoints;
......@@ -418,6 +410,7 @@ private:
QNemoHeartbeat _nemoHeartbeat; // measurement progress
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
bool _bridgeSetupDone;
static StatusMap _nemoStatusMap;
// Periodic tasks.
......
src/comm/ros_bridge/include/ComPrivateInclude.h
View file @ a0ede0e6
......@@ -5,7 +5,7 @@
#include <memory>
#include <deque>
#include <set>
#include <unordered_map>
namespace ROSBridge {
namespace ComPrivate {
......@@ -15,7 +15,8 @@ typedef rapidjson::Document JsonDoc;
typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
typedef std::deque<JsonDocUPtr> JsonQueue;
typedef std::size_t HashType;
typedef std::set<HashType> HashSet;
using ClientMap = std::unordered_map<HashType, std::string>;
static const char* _topicAdvertiserKey = "topic_advertiser";
static const char* _topicPublisherKey = "topic_publisher";
......
src/comm/ros_bridge/include/MessageGroups.h
View file @ a0ede0e6
......@@ -79,6 +79,17 @@ struct GeometryMsgs {
};
};
struct GeographicMsgs {
static StringType label() {return "geographic_msgs";}
//!
//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
struct GeoPointGroup {
static StringType label() {return "GeoPoint";}
};
};
struct JSKRecognitionMsgs {
static StringType label() {return "jsk_recognition_msgs";}
......@@ -118,8 +129,8 @@ typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
MessageGroups::GeometryMsgs::Point32Group>
Point32Group;
typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
MessageGroups::GeometryMsgs::GeoPointGroup>
typedef MessageGroups::MessageGroup<MessageGroups::GeographicMsgs,
MessageGroups::GeographicMsgs::GeoPointGroup>
GeoPointGroup;
typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
......
src/comm/ros_bridge/include/ROSBridge.h
View file @ a0ede0e6
......@@ -34,8 +34,11 @@ public:
//! @brief Start ROS bridge.
void start();
//! @brief Reset ROS bridge.
//! @brief Stops ROS bridge.
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();
private:
TypeFactory _typeFactory;
......
src/comm/ros_bridge/include/RosBridgeClient.h
View file @ a0ede0e6
......@@ -15,6 +15,9 @@
#include <chrono>
#include <functional>
#include <thread>
#include <future>
#include <functional>
#include <mutex>
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
......@@ -23,18 +26,20 @@ class RosbridgeWsClient
{
std::string server_port_path;
std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
std::mutex map_mutex;
// Starts the client.
void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
{
if (!client->on_open)
{
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -42,7 +47,7 @@ class RosbridgeWsClient
};
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
if (!client->on_message)
{
client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
......@@ -66,14 +71,14 @@ class RosbridgeWsClient
}
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::thread client_thread([client_name, client]() {
#else
std::thread client_thread([client]() {
#endif
client->start();
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << ": Terminated" << std::endl;
#endif
client->on_open = NULL;
......@@ -83,9 +88,6 @@ class RosbridgeWsClient
});
client_thread.detach();
// This is to make sure that the thread got fully launched before we do anything to it (e.g. remove)
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
public:
......@@ -96,6 +98,8 @@ public:
~RosbridgeWsClient()
{
std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
for (auto& client : client_map)
{
client.second->stop();
......@@ -103,14 +107,44 @@ public:
}
}
// The execution can take up to 100 ms!
bool connected(){
auto p = std::make_shared<std::promise<void>>();
auto future = p->get_future();
auto callback = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<std::promise<void>> p) {
p->set_value();
connection->send_close(1000);
};
std::shared_ptr<WsClient> status_client = std::make_shared<WsClient>(server_port_path);
status_client->on_open = std::bind(callback, std::placeholders::_1, p);
std::async([status_client]{
status_client->start();
status_client->on_open = NULL;
status_client->on_message = NULL;
status_client->on_close = NULL;
status_client->on_error = NULL;
});
auto status = future.wait_for(std::chrono::milliseconds(100));
return status == std::future_status::ready;
}
// Adds a client to the client_map.
void addClient(const std::string& client_name)
{
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())
{
client_map[client_name] = std::make_shared<WsClient>(server_port_path);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << " has already been created" << std::endl;
......@@ -120,6 +154,8 @@ public:
std::shared_ptr<WsClient> getClient(const std::string& client_name)
{
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())
{
......@@ -130,6 +166,8 @@ public:
void stopClient(const std::string& client_name)
{
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())
{
......@@ -138,11 +176,11 @@ public:
it->second->on_message = NULL;
it->second->on_close = NULL;
it->second->on_error = NULL;
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << " has been stopped" << std::endl;
#endif
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
......@@ -152,17 +190,27 @@ public:
void removeClient(const std::string& client_name)
{
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())
{
it->second->stop();
it->second.reset();
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving 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);
#ifdef DEBUG
t.detach();
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << " has been removed" << std::endl;
#endif
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
......@@ -170,8 +218,12 @@ public:
#endif
}
// Gets the client from client_map and starts it. Advertising is essentially sending a message.
// 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())
{
......@@ -185,7 +237,7 @@ public:
start(client_name, it->second, message);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -210,7 +262,7 @@ public:
std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << "publish_client: Opened connection" << std::endl;
std::cout << "publish_client: Sending message: " << message << std::endl;
#endif
......@@ -225,6 +277,8 @@ public:
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::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......@@ -259,7 +313,7 @@ public:
it->second->on_message = callback;
start(client_name, it->second, message);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -269,6 +323,8 @@ 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::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
......@@ -277,7 +333,7 @@ public:
it->second->on_message = callback;
start(client_name, it->second, message);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -304,7 +360,7 @@ public:
std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << "service_response_client: Opened connection" << std::endl;
std::cout << "service_response_client: Sending message: " << message << std::endl;
#endif
......@@ -351,7 +407,7 @@ public:
else
{
call_service_client->on_message = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
#ifdef DEBUG
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << "call_service_client: Message received: " << in_message->string() << std::endl;
std::cout << "call_service_client: Sending close connection" << std::endl;
#endif
......
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @ a0ede0e6
......@@ -2,8 +2,20 @@
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc) :
struct ROSBridge::ComPrivate::ThreadData
{
const ROSBridge::CasePacker &casePacker;
RosbridgeWsClient &rbc;
ROSBridge::ComPrivate::JsonQueue &queue;
std::mutex &queueMutex;
const std::atomic<bool> &running;
std::condition_variable &cv;
};
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient &rbc) :
_running(false)
, _casePacker(casePacker)
, _rbc(rbc)
......@@ -21,14 +33,15 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
if ( _running.load() ) // start called while thread running.
return;
_running.store(true);
_rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
_pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
std::cref(*_casePacker),
std::ref(*_rbc),
std::ref(_queue),
std::ref(_queueMutex),
std::ref(_advertisedTopicsHashList),
std::cref(_running)));
ROSBridge::ComPrivate::ThreadData data{
_casePacker,
_rbc,
_queue,
_queueMutex,
_running,
_cv
};
_pThread = std::make_unique<std::thread>(&ROSBridge::ComPrivate::transmittLoop, data);
}
void ROSBridge::ComPrivate::TopicPublisher::reset()
......@@ -36,66 +49,61 @@ void ROSBridge::ComPrivate::TopicPublisher::reset()
if ( !_running.load() ) // stop called while thread not running.
return;
_running.store(false);
_cv.notify_one(); // Wake publisher thread.
if ( !_pThread )
return;
_pThread->join();
_pThread.reset();
_rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
_queue.clear();
_advertisedTopicsHashList.clear();
}
void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacker,
RosbridgeWsClient &rbc,
ROSBridge::ComPrivate::JsonQueue &queue,
std::mutex &queueMutex,
HashSet &advertisedTopicsHashList,
const std::atomic<bool> &running)
void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data)
{
rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
while(running.load()){
// Pop message from queue.
queueMutex.lock();
//std::cout << "Queue size: " << queue.size() << std::endl;
if (queue.empty()){
queueMutex.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(20));
// Init.
ClientMap clientMap;
// Main Loop.
while(data.running.load()){
std::unique_lock<std::mutex> lk(data.queueMutex);
// Check if new data available, wait if not.
if (data.queue.empty()){
data.cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
continue;
}
JsonDocUPtr pJsonDoc(std::move(queue.front()));
queue.pop_front();
queueMutex.unlock();
// Debug output.
// std::cout << "Transmitter loop json document:" << std::endl;
// rapidjson::OStreamWrapper out(std::cout);
// rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
// pJsonDoc->Accept(writer);
// std::cout << std::endl << std::endl;
// Pop message from queue.
JsonDocUPtr pJsonDoc(std::move(data.queue.front()));
data.queue.pop_front();
lk.unlock();
// Get tag from Json message and remove it.
Tag tag;
bool ret = casePacker.getTag(pJsonDoc, tag);
bool ret = data.casePacker.getTag(pJsonDoc, tag);
assert(ret); // Json message does not contain a tag;
(void)ret;
casePacker.removeTag(pJsonDoc);
data.casePacker.removeTag(pJsonDoc);
// Check if topic must be advertised.
// Advertised topics are stored in advertisedTopicsHashList as
// a hash.
HashType hash = ROSBridge::ComPrivate::getHash(tag.topic());
if ( advertisedTopicsHashList.count(hash) == 0) {
advertisedTopicsHashList.insert(hash);
rbc.advertise(ROSBridge::ComPrivate::_topicAdvertiserKey,
tag.topic(),
tag.messageType() );
std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
+ tag.topic();
HashType hash = ROSBridge::ComPrivate::getHash(clientName);
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(),
tag.messageType() );
}
// Debug output.
//std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl;
// Send Json message.
rbc.publish(tag.topic(), *pJsonDoc.get());
// Publish Json message.
data.rbc.publish(tag.topic(), *pJsonDoc.get());
} // while loop
// Tidy up.
for (auto& it : clientMap)
data.rbc.removeClient(it.second);
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @ a0ede0e6
......@@ -10,18 +10,22 @@
#include <atomic>
#include <mutex>
#include <set>
#include <condition_variable>
namespace ROSBridge {
namespace ComPrivate {
struct ThreadData;
class TopicPublisher
{
typedef std::unique_ptr<std::thread> ThreadPtr;
using CondVar = std::condition_variable;
public:
TopicPublisher() = delete;
TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc);
TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient &rbc);
~TopicPublisher();
......@@ -31,34 +35,32 @@ public:
//! @brief Resets the publisher.
void reset();
template<class T>
void publish(const T &msg, const std::string &topic){
JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
publish(std::move(docPtr));
}
void publish(JsonDocUPtr docPtr){
{
std::lock_guard<std::mutex> lock(_queueMutex);
_queue.push_back(std::move(docPtr));
}
_cv.notify_one(); // Wake publisher thread.
}
template<class T>
void publish(const T &msg, const std::string &topic){
JsonDocUPtr docPtr(_casePacker.pack(msg, topic));
publish(std::move(docPtr));
}
private:
JsonQueue _queue;
std::mutex _queueMutex;
std::atomic<bool> _running;
CasePacker *_casePacker;
CasePacker &_casePacker;
RosbridgeWsClient &_rbc;
CondVar _cv;
ThreadPtr _pThread;
RosbridgeWsClient *_rbc;
HashSet _advertisedTopicsHashList; // Not thread save! This container
// is manipulated by transmittLoop only!
};
void transmittLoop(const ROSBridge::CasePacker &casePacker,
RosbridgeWsClient &rbc,
ROSBridge::ComPrivate::JsonQueue &queue,
std::mutex &queueMutex,
HashSet &advertisedTopicsHashList,
const std::atomic<bool> &running);
void transmittLoop(ThreadData data);
} // namespace CommunicatorDetail
......
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @ a0ede0e6
#include "TopicSubscriber.h"
ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
ROSBridge::CasePacker *casePacker,
RosbridgeWsClient *rbc) :
ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
RosbridgeWsClient &rbc) :
_casePacker(casePacker)
, _rbc(rbc)
, _running(false)
......@@ -24,10 +23,16 @@ void ROSBridge::ComPrivate::TopicSubscriber::start()
void ROSBridge::ComPrivate::TopicSubscriber::reset()
{
if ( _running ){
for (std::string clientName : _clientList)
_rbc->removeClient(clientName);
_running = false;
_callbackMap.clear();
{
for (std::string clientName : _clientList){
_rbc.removeClient(clientName);
}
}
{
std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
_callbackMapStruct.map.clear();
}
_clientList.clear();
}
}
......@@ -44,25 +49,39 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
_clientList.push_back(clientName);
HashType hash = getHash(clientName);
auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
if ( !ret.second )
return false; // Topic subscription already present.
{
std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
if ( !ret.second )
return false; // Topic subscription already present.
}
using namespace std::placeholders;
auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
hash,
std::cref(_callbackMap),
std::ref(_callbackMapStruct),
_1, _2);
// std::cout << std::endl;
// std::cout << "topic subscription" << std::endl;
// std::cout << "client name: " << clientName << std::endl;
// std::cout << "topic: " << topic << std::endl;
_rbc->addClient(clientName);
_rbc->subscribe(clientName,
topic,
f );
{
auto start = std::chrono::high_resolution_clock::now();
_rbc.addClient(clientName);
auto end = std::chrono::high_resolution_clock::now();
std::cout << "add client time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
_rbc.subscribe(clientName,
topic,
f );
end = std::chrono::high_resolution_clock::now();
std::cout << "subscribe time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms" << std::endl;
}
return true;
......@@ -72,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback(
const HashType &hash,
const ROSBridge::ComPrivate::TopicSubscriber::CallbackMap &map,
ROSBridge::ComPrivate::MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
......@@ -97,26 +116,21 @@ void ROSBridge::ComPrivate::subscriberCallback(
// << std::endl;
// Search callback.
auto it = map.find(hash);
if (it == map.end()) {
assert(false); // callback not found
return;
CallbackType callback;
{
std::lock_guard<std::mutex> lk(mapWrapper.mutex);
auto it = mapWrapper.map.find(hash);
if (it == mapWrapper.map.end()) {
//assert(false); // callback not found
return;
}
callback = it->second;
}
// Extract message and call callback.
JsonDocUPtr pDoc(new JsonDoc());
pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
it->second(std::move(pDoc)); // Call callback.
callback(std::move(pDoc));
return;
}
void ROSBridge::ComPrivate::test(
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
std::cout << "test: Json document: "
<< in_message->string()
<< std::endl;
}
src/comm/ros_bridge/include/TopicSubscriber.h
View file @ a0ede0e6
......@@ -8,15 +8,21 @@
namespace ROSBridge {
namespace ComPrivate {
typedef std::function<void(JsonDocUPtr)> CallbackType;
struct MapStruct{
typedef std::map<HashType, CallbackType> Map;
Map map;
std::mutex mutex;
};
class TopicSubscriber
{
typedef std::vector<std::string> ClientList;
public:
typedef std::function<void(JsonDocUPtr)> CallbackType;
typedef std::map<HashType, CallbackType> CallbackMap;
public:
TopicSubscriber() = delete;
TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc);
TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
~TopicSubscriber();
//! @brief Starts the subscriber.
......@@ -31,19 +37,19 @@ public:
bool subscribe(const char* topic, const CallbackType &callback);
private:
CasePacker *_casePacker;
RosbridgeWsClient *_rbc;
CallbackMap _callbackMap;
CasePacker &_casePacker;
RosbridgeWsClient &_rbc;
MapStruct _callbackMapStruct;
ClientList _clientList;
bool _running;
};
void subscriberCallback(const HashType &hash,
const TopicSubscriber::CallbackMap &map,
MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
void test(std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
} // namespace ComPrivate
} // namespace ROSBridge
src/comm/ros_bridge/src/ROSBridge.cpp
View file @ a0ede0e6
......@@ -3,8 +3,8 @@
ROSBridge::ROSBridge::ROSBridge() :
_casePacker(&_typeFactory, &_jsonFactory)
, _rbc("localhost:9090")
, _topicPublisher(&_casePacker, &_rbc)
, _topicSubscriber(&_casePacker, &_rbc)
, _topicPublisher(_casePacker, _rbc)
, _topicSubscriber(_casePacker, _rbc)
{
}
......@@ -36,3 +36,8 @@ void ROSBridge::ROSBridge::reset()
_topicSubscriber.reset();
}
bool ROSBridge::ROSBridge::connected()
{
return _rbc.connected();
}
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