Commit b4b4aede authored by Valentin Platzgummer's avatar Valentin Platzgummer

before restoring src/comm/ros_bridge/include

parent 07a48251
......@@ -489,7 +489,7 @@ HEADERS += \
src/comm/ros_bridge/include/MessageTag.h \
src/comm/ros_bridge/include/MessageTraits.h \
src/comm/ros_bridge/include/RosBridgeClient.h \
src/comm/ros_bridge/include/ThreadSafeQueue.h \
src/comm/ros_bridge/include/Server.h \
src/comm/ros_bridge/include/TopicPublisher.h \
src/comm/ros_bridge/include/TopicSubscriber.h \
src/comm/ros_bridge/include/TypeFactory.h \
......@@ -513,6 +513,7 @@ SOURCES += \
src/Wima/WimaBridge.cc \
src/comm/ros_bridge/include/ComPrivateInclude.cpp \
src/comm/ros_bridge/include/MessageTag.cpp \
src/comm/ros_bridge/include/Server.cpp \
src/comm/ros_bridge/include/TopicPublisher.cpp \
src/comm/ros_bridge/include/TopicSubscriber.cpp \
src/comm/ros_bridge/src/CasePacker.cpp \
......
......@@ -35,7 +35,8 @@ const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength";
WimaController::StatusMap WimaController::_nemoStatusMap{std::make_pair<int, QString>(0, "No Heartbeat"),
WimaController::StatusMap WimaController::_nemoStatusMap{
std::make_pair<int, QString>(0, "No Heartbeat"),
std::make_pair<int, QString>(1, "Connected"),
std::make_pair<int, QString>(-1, "Timeout")};
......@@ -55,7 +56,9 @@ WimaController::WimaController(QObject *parent)
, _snakeManager (_managerSettings, _areaInterface)
, _rtlManager (_managerSettings, _areaInterface)
, _currentManager (&_defaultManager)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _managerList {&_defaultManager, &_snakeManager, &_rtlManager}
, _metaDataMap (FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
, _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
......@@ -68,7 +71,6 @@ WimaController::WimaController(QObject *parent)
, _measurementPathLength (-1)
, _lowBatteryHandlingTriggered (false)
, _snakeCalcInProgress (false)
, _scenarioDefinedBool (false)
, _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName])
, _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName])
, _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName])
......@@ -89,9 +91,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);
......@@ -343,17 +356,8 @@ 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
// reset visual items
_areas.clear();
_defaultManager.clear();
......@@ -779,17 +783,6 @@ void WimaController::_eventTimerHandler()
_pRosBridge->reset();
_bridgeSetupDone = false;
}
//qWarning() << "Connectable: " << _pRosBridge->connected() << "\n";
//auto start = std::chrono::high_resolution_clock::now();
//auto end = std::chrono::high_resolution_clock::now();
//qWarning() << "Duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
//<< " ms\n";
}
}
......@@ -838,14 +831,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();
......@@ -888,33 +890,13 @@ 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()
{
_setSnakeCalcInProgress(false);
auto start = std::chrono::high_resolution_clock::now();
_snakeManager.clear();
const WorkerResult_t &r = _snakeWorker.getResult();
_setSnakeCalcInProgress(false);
if (!r.success) {
//qgcApp()->showMessage(r.errorMessage);
return;
......
......@@ -338,8 +338,6 @@ private slots:
void _smartRTLCleanUp (bool flying);
// Snake.
void _setSnakeCalcInProgress (bool inProgress);
bool _isScenarioDefined (void);
bool _isScenarioDefinedErrorMessage (void);
void _snakeStoreWorkerResults ();
void _startStopRosBridge ();
void _initStartSnakeWorker ();
......@@ -357,7 +355,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 +369,8 @@ private:
WaypointManager::DefaultManager _snakeManager;
WaypointManager::RTLManager _rtlManager;
WaypointManager::ManagerBase *_currentManager;
using ManagerList = QList<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts.
QMap<QString, FactMetaData*> _metaDataMap;
......
This diff is collapsed.
This diff is collapsed.
#include "Server.h"
Server::Server()
{
}
#ifndef SERVER_H
#define SERVER_H
class Server
{
public:
Server();
};
#endif // SERVER_H
#pragma once
#include <queue>
#include <mutex>
#include <condition_variable>
namespace ROSBridge {
template <class T>
class ThreadSafeQueue
{
public:
ThreadSafeQueue();
~ThreadSafeQueue();
T pop_front();
void push_back(const T& item);
void push_back(T&& item);
int size();
bool empty();
private:
std::deque<T> _queue;
std::mutex _mutex;
std::condition_variable _cond;
};
template <typename T>
ThreadSafeQueue<T>::ThreadSafeQueue(){}
template <typename T>
ThreadSafeQueue<T>::~ThreadSafeQueue(){}
template <typename T>
T ThreadSafeQueue<T>::pop_front()
{
std::unique_lock<std::mutex> mlock(_mutex);
while (_queue.empty())
{
_cond.wait(mlock);
}
T t = std::move(_queue.front());
_queue.pop_front();
return t;
}
template <typename T>
void ThreadSafeQueue<T>::push_back(const T& item)
{
std::unique_lock<std::mutex> mlock(_mutex);
_queue.push_back(item);
mlock.unlock(); // unlock before notificiation to minimize mutex con
_cond.notify_one(); // notify one waiting thread
}
template <typename T>
void ThreadSafeQueue<T>::push_back(T&& item)
{
std::unique_lock<std::mutex> mlock(_mutex);
_queue.push_back(std::move(item));
mlock.unlock(); // unlock before notificiation to minimize mutex con
_cond.notify_one(); // notify one waiting thread
}
template <typename T>
int ThreadSafeQueue<T>::size()
{
std::unique_lock<std::mutex> mlock(_mutex);
int size = _queue.size();
mlock.unlock();
return size;
}
} // namespace
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