Skip to content

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

123

parent 07a48251
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 407 additions and 309 deletions
src/Snake/snake.cpp
View file @ 96d3005b
......@@ -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 @ 96d3005b
......@@ -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 @ 96d3005b
#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 @ 96d3005b
......@@ -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 @ 96d3005b
......@@ -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 @ 96d3005b
......@@ -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 @ 96d3005b
......@@ -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 @ 96d3005b
This diff is collapsed.
src/Wima/WimaController.h
View file @ 96d3005b
......@@ -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.
......@@ -357,7 +354,6 @@ private:
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
......@@ -372,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;
......@@ -403,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;
......
src/comm/ros_bridge/include/ROSBridge.h
View file @ 96d3005b
......@@ -45,7 +45,6 @@ private:
JsonFactory _jsonFactory;
CasePacker _casePacker;
RosbridgeWsClient _rbc;
std::mutex _rbcMutex;
ComPrivate::TopicPublisher _topicPublisher;
ComPrivate::TopicSubscriber _topicSubscriber;
......
src/comm/ros_bridge/include/RosBridgeClient.h
View file @ 96d3005b
......@@ -17,6 +17,7 @@
#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>)>;
......@@ -25,6 +26,7 @@ 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)
......@@ -86,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:
......@@ -99,6 +98,8 @@ public:
~RosbridgeWsClient()
{
std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
for (auto& client : client_map)
{
client.second->stop();
......@@ -136,6 +137,8 @@ public:
// 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())
{
......@@ -151,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())
{
......@@ -161,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())
{
......@@ -183,12 +190,22 @@ 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);
t.detach();
#ifdef ROS_BRIDGE_CLIENT_DEBUG
std::cout << client_name << " has been removed" << std::endl;
#endif
......@@ -205,6 +222,8 @@ 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())
{
......@@ -258,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())
{
......@@ -302,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())
{
......
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @ 96d3005b
......@@ -8,20 +8,17 @@ struct ROSBridge::ComPrivate::ThreadData
{
const ROSBridge::CasePacker &casePacker;
RosbridgeWsClient &rbc;
std::mutex &rbcMutex;
ROSBridge::ComPrivate::JsonQueue &queue;
std::mutex &queueMutex;
const std::atomic<bool> &running;
std::condition_variable &cv;
};
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc,
std::mutex *rbcMutex) :
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient &rbc) :
_running(false)
, _casePacker(casePacker)
, _rbc(rbc)
, _rbcMutex(rbcMutex)
{
}
......@@ -37,9 +34,8 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
return;
_running.store(true);
ROSBridge::ComPrivate::ThreadData data{
*_casePacker,
*_rbc,
*_rbcMutex,
_casePacker,
_rbc,
_queue,
_queueMutex,
_running,
......@@ -64,11 +60,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
{
// Init.
ClientMap clientMap;
// {
// std::lock_guard<std::mutex> lk(data.rbcMutex);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
// data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
// }
// Main Loop.
while(data.running.load()){
std::unique_lock<std::mutex> lk(data.queueMutex);
......@@ -101,27 +92,18 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
std::cout << clientName << ";"
<< tag.topic() << ";"
<< tag.messageType() << ";" << std::endl;
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
data.rbc.addClient(clientName);
data.rbc.advertise(clientName,
tag.topic(),
tag.messageType() );
}
data.rbc.addClient(clientName);
data.rbc.advertise(clientName,
tag.topic(),
tag.messageType() );
}
// Publish Json message.
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
data.rbc.publish(tag.topic(), *pJsonDoc.get());
}
data.rbc.publish(tag.topic(), *pJsonDoc.get());
} // while loop
// Tidy up.
{
std::lock_guard<std::mutex> lk(data.rbcMutex);
for (auto& it : clientMap)
data.rbc.removeClient(it.second);
}
for (auto& it : clientMap)
data.rbc.removeClient(it.second);
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @ 96d3005b
......@@ -24,9 +24,8 @@ class TopicPublisher
public:
TopicPublisher() = delete;
TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc,
std::mutex *rbcMutex);
TopicPublisher(CasePacker &casePacker,
RosbridgeWsClient &rbc);
~TopicPublisher();
......@@ -46,7 +45,7 @@ public:
template<class T>
void publish(const T &msg, const std::string &topic){
JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
JsonDocUPtr docPtr(_casePacker.pack(msg, topic));
publish(std::move(docPtr));
}
......@@ -54,9 +53,8 @@ private:
JsonQueue _queue;
std::mutex _queueMutex;
std::atomic<bool> _running;
CasePacker *_casePacker;
RosbridgeWsClient *_rbc;
std::mutex *_rbcMutex;
CasePacker &_casePacker;
RosbridgeWsClient &_rbc;
CondVar _cv;
ThreadPtr _pThread;
};
......
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @ 96d3005b
#include "TopicSubscriber.h"
ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(ROSBridge::CasePacker *casePacker,
RosbridgeWsClient *rbc, std::mutex *rbcMutex) :
ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
RosbridgeWsClient &rbc) :
_casePacker(casePacker)
, _rbc(rbc)
, _rbcMutex(rbcMutex)
, _running(false)
{
......@@ -26,14 +25,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::reset()
if ( _running ){
_running = false;
{
std::lock_guard<std::mutex> lk(*_rbcMutex);
for (std::string clientName : _clientList){
_rbc->removeClient(clientName);
_rbc.removeClient(clientName);
}
}
{
std::lock_guard<std::mutex> lk(_callbackMap.mutex);
_callbackMap.map.clear();
std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
_callbackMapStruct.map.clear();
}
_clientList.clear();
}
......@@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
HashType hash = getHash(clientName);
{
std::lock_guard<std::mutex> lk(_callbackMap.mutex);
auto ret = _callbackMap.map.insert(std::make_pair(hash, callback)); //
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.
......@@ -62,7 +60,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using namespace std::placeholders;
auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
hash,
std::ref(_callbackMap),
std::ref(_callbackMapStruct),
_1, _2);
// std::cout << std::endl;
......@@ -70,11 +68,20 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
// std::cout << "client name: " << clientName << std::endl;
// std::cout << "topic: " << topic << std::endl;
{
std::lock_guard<std::mutex> lk(*_rbcMutex);
_rbc->addClient(clientName);
_rbc->subscribe(clientName,
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;
......@@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
void ROSBridge::ComPrivate::subscriberCallback(
const HashType &hash,
ROSBridge::ComPrivate::CallbackMapWrapper &mapWrapper,
ROSBridge::ComPrivate::MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
......
src/comm/ros_bridge/include/TopicSubscriber.h
View file @ 96d3005b
......@@ -10,7 +10,7 @@ namespace ComPrivate {
typedef std::function<void(JsonDocUPtr)> CallbackType;
struct CallbackMapWrapper{
struct MapStruct{
typedef std::map<HashType, CallbackType> Map;
Map map;
std::mutex mutex;
......@@ -22,7 +22,7 @@ class TopicSubscriber
public:
TopicSubscriber() = delete;
TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc, std::mutex *rbcMutex);
TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
~TopicSubscriber();
//! @brief Starts the subscriber.
......@@ -40,16 +40,15 @@ private:
CasePacker *_casePacker;
RosbridgeWsClient *_rbc;
std::mutex *_rbcMutex;
CallbackMapWrapper _callbackMap;
CasePacker &_casePacker;
RosbridgeWsClient &_rbc;
MapStruct _callbackMapStruct;
ClientList _clientList;
bool _running;
};
void subscriberCallback(const HashType &hash,
CallbackMapWrapper &mapWrapper,
MapStruct &mapWrapper,
std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
} // namespace ComPrivate
......
src/comm/ros_bridge/src/ROSBridge.cpp
View file @ 96d3005b
......@@ -3,8 +3,8 @@
ROSBridge::ROSBridge::ROSBridge() :
_casePacker(&_typeFactory, &_jsonFactory)
, _rbc("localhost:9090")
, _topicPublisher(&_casePacker, &_rbc, &_rbcMutex)
, _topicSubscriber(&_casePacker, &_rbc, &_rbcMutex)
, _topicPublisher(_casePacker, _rbc)
, _topicSubscriber(_casePacker, _rbc)
{
}
......@@ -38,7 +38,6 @@ void ROSBridge::ROSBridge::reset()
bool ROSBridge::ROSBridge::connected()
{
std::lock_guard<std::mutex> lk(_rbcMutex);
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