Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
d02c3c46
Commit
d02c3c46
authored
Jul 21, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
refactoring WimaController, adding waypoint managers.
parent
8b365d4f
Changes
24
Show whitespace changes
Inline
Side-by-side
Showing
24 changed files
with
607 additions
and
219 deletions
+607
-219
qgroundcontrol.pro
qgroundcontrol.pro
+6
-2
FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewMap.qml
+3
-0
MissionController.cc
src/MissionManager/MissionController.cc
+94
-18
MissionController.h
src/MissionManager/MissionController.h
+10
-2
snake_geometry.cpp
src/Snake/snake_geometry.cpp
+2
-1
QNemoProgress.cc
src/Wima/QNemoProgress.cc
+0
-26
QNemoProgress.h
src/Wima/QNemoProgress.h
+1
-17
SnakeWorker.cc
src/Wima/SnakeWorker.cc
+4
-3
SnakeWorker.h
src/Wima/SnakeWorker.h
+1
-3
GenericPathSlicer.cpp
src/Wima/WaypointManager/GenericPathSlicer.cpp
+1
-0
GenericPathSlicer.h
src/Wima/WaypointManager/GenericPathSlicer.h
+245
-0
GenericWaypointManager.cpp
src/Wima/WaypointManager/GenericWaypointManager.cpp
+6
-0
GenericWaypointManager.h
src/Wima/WaypointManager/GenericWaypointManager.h
+8
-0
WimaController.cc
src/Wima/WimaController.cc
+77
-61
WimaController.h
src/Wima/WimaController.h
+24
-23
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+2
-2
CasePacker.h
src/comm/ros_bridge/include/CasePacker.h
+1
-1
GenericMessages.h
src/comm/ros_bridge/include/GenericMessages.h
+1
-0
ROSBridge.h
src/comm/ros_bridge/include/ROSBridge.h
+4
-2
TopicPublisher.cpp
src/comm/ros_bridge/include/TopicPublisher.cpp
+46
-42
TopicPublisher.h
src/comm/ros_bridge/include/TopicPublisher.h
+7
-3
TopicSubscriber.cpp
src/comm/ros_bridge/include/TopicSubscriber.cpp
+49
-7
TopicSubscriber.h
src/comm/ros_bridge/include/TopicSubscriber.h
+12
-3
ROSBridge.cpp
src/comm/ros_bridge/src/ROSBridge.cpp
+3
-3
No files found.
qgroundcontrol.pro
View file @
d02c3c46
...
@@ -437,7 +437,9 @@ HEADERS += \
...
@@ -437,7 +437,9 @@ HEADERS += \
src
/
Wima
/
QtROSTypeFactory
.
h
\
src
/
Wima
/
QtROSTypeFactory
.
h
\
src
/
Wima
/
SnakeTiles
.
h
\
src
/
Wima
/
SnakeTiles
.
h
\
src
/
Wima
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
WimaControllerDetail
.
h
\
src
/
Wima
/
SnakeWorker
.
h
\
src
/
Wima
/
WaypointManager
/
GenericPathSlicer
.
h
\
src
/
Wima
/
WaypointManager
/
GenericWaypointManager
.
h
\
src
/
Wima
/
WimaPolygonArray
.
h
\
src
/
Wima
/
WimaPolygonArray
.
h
\
src
/
Wima
/
snaketile
.
h
\
src
/
Wima
/
snaketile
.
h
\
src
/
api
/
QGCCorePlugin
.
h
\
src
/
api
/
QGCCorePlugin
.
h
\
...
@@ -493,12 +495,14 @@ SOURCES += \
...
@@ -493,12 +495,14 @@ SOURCES += \
src
/
Wima
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
PolygonArray
.
cc
\
src
/
Wima
/
PolygonArray
.
cc
\
src
/
Wima
/
QNemoProgress
.
cc
\
src
/
Wima
/
QNemoProgress
.
cc
\
src
/
Wima
/
SnakeWorker
.
cc
\
src
/
Wima
/
WaypointManager
/
GenericPathSlicer
.
cpp
\
src
/
Wima
/
WaypointManager
/
GenericWaypointManager
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
ComPrivateInclude
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
ComPrivateInclude
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
MessageTag
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
MessageTag
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicPublisher
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
cpp
\
src
/
comm
/
ros_bridge
/
include
/
TopicSubscriber
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
cpp
\
src
/
comm
/
ros_bridge
/
src
/
CasePacker
.
cpp
\
src
/
Wima
/
WimaControllerDetail
.
cc
\
src
/
Wima
/
snaketile
.
cpp
\
src
/
Wima
/
snaketile
.
cpp
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
...
...
src/FlightDisplay/FlightDisplayViewMap.qml
View file @
d02c3c46
...
@@ -244,9 +244,12 @@ FlightMap {
...
@@ -244,9 +244,12 @@ FlightMap {
// Add Snake tiles center points to the map
// Add Snake tiles center points to the map
MapItemView
{
MapItemView
{
property
bool
_lengthMatching
:
wimaController
.
snakeTileCenterPoints
.
length
===
wimaController
.
snakeProgress
.
length
property
bool
_enable
:
wimaController
.
enableWimaController
.
value
property
bool
_enable
:
wimaController
.
enableWimaController
.
value
&&
wimaController
.
enableSnake
.
value
&&
wimaController
.
enableSnake
.
value
&&
_lengthMatching
model
:
_enable
?
wimaController
.
snakeTileCenterPoints
:
0
model
:
_enable
?
wimaController
.
snakeTileCenterPoints
:
0
...
...
src/MissionManager/MissionController.cc
View file @
d02c3c46
...
@@ -378,7 +378,7 @@ int MissionController::_nextSequenceNumber(void)
...
@@ -378,7 +378,7 @@ int MissionController::_nextSequenceNumber(void)
}
}
}
}
int
MissionController
::
insertSimpleMissionItem
(
QGeoCoordinate
coordinate
,
int
i
)
int
MissionController
::
insertSimpleMissionItem
(
const
QGeoCoordinate
&
coordinate
,
int
i
)
{
{
int
sequenceNumber
=
_nextSequenceNumber
();
int
sequenceNumber
=
_nextSequenceNumber
();
SimpleMissionItem
*
newItem
=
new
SimpleMissionItem
(
_controllerVehicle
,
_flyView
,
this
);
SimpleMissionItem
*
newItem
=
new
SimpleMissionItem
(
_controllerVehicle
,
_flyView
,
this
);
...
@@ -410,6 +410,58 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
...
@@ -410,6 +410,58 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
return
newItem
->
sequenceNumber
();
return
newItem
->
sequenceNumber
();
}
}
int
MissionController
::
insertSimpleMissionItems
(
const
QList
<
QGeoCoordinate
>
&
coordinates
,
int
idx
)
{
MAV_CMD
mavCmd
=
MAV_CMD_NAV_WAYPOINT
;
if
(
_visualItems
->
count
()
==
1
&&
(
_controllerVehicle
->
fixedWing
()
||
_controllerVehicle
->
vtol
()
||
_controllerVehicle
->
multiRotor
())
)
{
mavCmd
=
_controllerVehicle
->
vtol
()
?
MAV_CMD_NAV_VTOL_TAKEOFF
:
MAV_CMD_NAV_TAKEOFF
;
if
(
!
_controllerVehicle
->
firmwarePlugin
()
->
supportedMissionCommands
().
contains
(
mavCmd
))
{
mavCmd
=
MAV_CMD_NAV_WAYPOINT
;
}
}
int
sequenceNumber
=
_nextSequenceNumber
();
int
size
=
coordinates
.
size
();
SimpleMissionItem
*
newItem
=
nullptr
;
bool
firstItem
=
true
;
double
prevAltitude
=
0.0
;
int
prevAltitudeMode
=
0
;
for
(
int
i
=
0
;
i
<
size
;
++
i
)
{
newItem
=
new
SimpleMissionItem
(
_controllerVehicle
,
_flyView
,
this
);
newItem
->
setSequenceNumber
(
sequenceNumber
);
newItem
->
setCoordinate
(
coordinates
[
i
]);
newItem
->
setCommand
(
mavCmd
);
_initSimpleItem
(
newItem
);
if
(
!
firstItem
||
(
newItem
->
specifiesAltitude
()
&&
_findPreviousAltitude
(
i
,
&
prevAltitude
,
&
prevAltitudeMode
))
)
{
newItem
->
altitude
()
->
setRawValue
(
prevAltitude
);
newItem
->
setAltitudeMode
(
static_cast
<
QGroundControlQmlGlobal
::
AltitudeMode
>
(
prevAltitudeMode
));
}
newItem
->
setMissionFlightStatus
(
_missionFlightStatus
);
_visualItems
->
insert
(
idx
++
,
newItem
);
sequenceNumber
=
newItem
->
lastSequenceNumber
()
+
1
;
mavCmd
=
MAV_CMD_NAV_WAYPOINT
;
firstItem
=
false
;
}
_recalcSequence
();
_recalcChildItems
();
_recalcWaypointLines
();
_updateTimer
.
start
(
UPDATE_TIMEOUT
);
if
(
newItem
==
nullptr
)
return
-
1
;
return
newItem
->
sequenceNumber
();
}
int
MissionController
::
insertSimpleMissionItem
(
const
MissionItem
&
missionItem
,
int
i
)
int
MissionController
::
insertSimpleMissionItem
(
const
MissionItem
&
missionItem
,
int
i
)
{
{
int
sequenceNumber
=
_nextSequenceNumber
();
int
sequenceNumber
=
_nextSequenceNumber
();
...
@@ -1674,7 +1726,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo
...
@@ -1674,7 +1726,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo
}
}
/// @param clickCoordinate The location of the user click when inserting a new item
/// @param clickCoordinate The location of the user click when inserting a new item
void
MissionController
::
_recalcAllWithClickCoordinate
(
QGeoCoordinate
&
clickCoordinate
)
void
MissionController
::
_recalcAllWithClickCoordinate
(
const
QGeoCoordinate
&
clickCoordinate
)
{
{
if
(
!
_flyView
)
{
if
(
!
_flyView
)
{
_setPlannedHomePositionFromFirstCoordinate
(
clickCoordinate
);
_setPlannedHomePositionFromFirstCoordinate
(
clickCoordinate
);
...
@@ -1770,37 +1822,61 @@ void MissionController::_deinitAllVisualItems(void)
...
@@ -1770,37 +1822,61 @@ void MissionController::_deinitAllVisualItems(void)
void
MissionController
::
_initVisualItem
(
VisualMissionItem
*
visualItem
)
void
MissionController
::
_initVisualItem
(
VisualMissionItem
*
visualItem
)
{
{
setDirty
(
false
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiesCoordinateChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
coordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
exitCoordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedFlightSpeedChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedGimbalYawChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedGimbalPitchChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
terrainAltitudeChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
additionalTimeDelayChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
lastSequenceNumberChanged
,
this
,
&
MissionController
::
_recalcSequence
);
if
(
visualItem
->
isSimpleItem
())
{
if
(
visualItem
->
isSimpleItem
())
{
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
SimpleMissionItem
*
simpleItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItem
);
SimpleMissionItem
*
simpleItem
=
qobject_cast
<
SimpleMissionItem
*>
(
visualItem
);
if
(
simpleItem
)
{
if
(
simpleItem
)
{
connect
(
&
simpleItem
->
missionItem
().
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionController
::
_itemCommandChanged
);
_initSimpleItem
(
simpleItem
);
}
else
{
}
else
{
qWarning
()
<<
"isSimpleItem == true, yet not SimpleMissionItem"
;
qWarning
()
<<
"isSimpleItem == true, yet not SimpleMissionItem"
;
}
}
}
else
{
}
else
{
ComplexMissionItem
*
complexItem
=
qobject_cast
<
ComplexMissionItem
*>
(
visualItem
);
ComplexMissionItem
*
complexItem
=
qobject_cast
<
ComplexMissionItem
*>
(
visualItem
);
if
(
complexItem
)
{
if
(
complexItem
)
{
connect
(
complexItem
,
&
ComplexMissionItem
::
complexDistanceChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
_initComplexItem
(
complexItem
);
connect
(
complexItem
,
&
ComplexMissionItem
::
greatestDistanceToChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
}
else
{
}
else
{
qWarning
()
<<
"ComplexMissionItem not found"
;
qWarning
()
<<
"ComplexMissionItem not found"
;
}
}
}
}
}
}
void
MissionController
::
_initVisualItemCommon
(
VisualMissionItem
*
visualItem
)
{
setDirty
(
false
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiesCoordinateChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
coordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
exitCoordinateHasRelativeAltitudeChanged
,
this
,
&
MissionController
::
_recalcWaypointLines
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedFlightSpeedChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedGimbalYawChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
specifiedGimbalPitchChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
terrainAltitudeChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
additionalTimeDelayChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
visualItem
,
&
VisualMissionItem
::
lastSequenceNumberChanged
,
this
,
&
MissionController
::
_recalcSequence
);
}
void
MissionController
::
_initSimpleItem
(
SimpleMissionItem
*
item
)
{
_initVisualItemCommon
(
item
);
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
connect
(
&
item
->
missionItem
().
_commandFact
,
&
Fact
::
valueChanged
,
this
,
&
MissionController
::
_itemCommandChanged
);
}
void
MissionController
::
_initComplexItem
(
ComplexMissionItem
*
item
)
{
_initVisualItemCommon
(
item
);
connect
(
item
,
&
ComplexMissionItem
::
complexDistanceChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
connect
(
item
,
&
ComplexMissionItem
::
greatestDistanceToChanged
,
this
,
&
MissionController
::
_recalcMissionFlightStatus
);
}
void
MissionController
::
_deinitVisualItem
(
VisualMissionItem
*
visualItem
)
void
MissionController
::
_deinitVisualItem
(
VisualMissionItem
*
visualItem
)
{
{
// Disconnect all signals
// Disconnect all signals
...
...
src/MissionManager/MissionController.h
View file @
d02c3c46
...
@@ -109,7 +109,12 @@ public:
...
@@ -109,7 +109,12 @@ public:
/// Add a new simple mission item to the list
/// Add a new simple mission item to the list
/// @param i: index to insert at
/// @param i: index to insert at
/// @return Sequence number for new item
/// @return Sequence number for new item
Q_INVOKABLE
int
insertSimpleMissionItem
(
QGeoCoordinate
coordinate
,
int
i
);
Q_INVOKABLE
int
insertSimpleMissionItem
(
const
QGeoCoordinate
&
coordinate
,
int
i
);
/// Add a list of new simple mission item to the list.
/// @param i: index to insert at
/// @return Sequence number of last item.
Q_INVOKABLE
int
insertSimpleMissionItems
(
const
QList
<
QGeoCoordinate
>
&
coordinates
,
int
i
);
/// Add a new simple mission item to the list
/// Add a new simple mission item to the list
/// @param i: index to insert at
/// @param i: index to insert at
...
@@ -289,10 +294,13 @@ private:
...
@@ -289,10 +294,13 @@ private:
void
_init
(
void
);
void
_init
(
void
);
void
_recalcSequence
(
void
);
void
_recalcSequence
(
void
);
void
_recalcChildItems
(
void
);
void
_recalcChildItems
(
void
);
void
_recalcAllWithClickCoordinate
(
QGeoCoordinate
&
clickCoordinate
);
void
_recalcAllWithClickCoordinate
(
const
QGeoCoordinate
&
clickCoordinate
);
void
_initAllVisualItems
(
void
);
void
_initAllVisualItems
(
void
);
void
_deinitAllVisualItems
(
void
);
void
_deinitAllVisualItems
(
void
);
void
_initVisualItem
(
VisualMissionItem
*
item
);
void
_initVisualItem
(
VisualMissionItem
*
item
);
void
_initVisualItemCommon
(
VisualMissionItem
*
visualItem
);
// Don't call this function, it is used by _initSimpleItem and _initComplexItem.
void
_initSimpleItem
(
SimpleMissionItem
*
item
);
void
_initComplexItem
(
ComplexMissionItem
*
item
);
void
_deinitVisualItem
(
VisualMissionItem
*
item
);
void
_deinitVisualItem
(
VisualMissionItem
*
item
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_setupActiveVehicle
(
Vehicle
*
activeVehicle
,
bool
forceLoadFromVehicle
);
void
_calcPrevWaypointValues
(
double
homeAlt
,
VisualMissionItem
*
currentItem
,
VisualMissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
);
void
_calcPrevWaypointValues
(
double
homeAlt
,
VisualMissionItem
*
currentItem
,
VisualMissionItem
*
prevItem
,
double
*
azimuth
,
double
*
distance
,
double
*
altDifference
);
...
...
src/Snake/snake_geometry.cpp
View file @
d02c3c46
...
@@ -274,6 +274,7 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, dou
...
@@ -274,6 +274,7 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, dou
bg
::
buffer
(
polygon
,
result
,
distance_strategy
,
side_strategy
,
join_strategy
,
end_strategy
,
point_strategy
);
bg
::
buffer
(
polygon
,
result
,
distance_strategy
,
side_strategy
,
join_strategy
,
end_strategy
,
point_strategy
);
if
(
result
.
size
()
>
0
)
polygonOffset
=
result
[
0
];
polygonOffset
=
result
[
0
];
}
}
...
...
src/Wima/QNemoProgress.cc
View file @
d02c3c46
#include "QNemoProgress.h"
#include "QNemoProgress.h"
QNemoProgress
::
QNemoProgress
(
QObject
*
parent
)
:
ProgressBase
()
,
QObject
(
parent
)
{
}
QNemoProgress
::
QNemoProgress
(
const
QNemoProgress
&
other
,
QObject
*
parent
)
:
ProgressBase
(
other
)
,
QObject
(
parent
)
{
}
QNemoProgress
*
QNemoProgress
::
Clone
()
const
{
return
new
QNemoProgress
(
*
this
,
this
->
parent
());
}
QVector
<
int
>
&
QNemoProgress
::
progress
()
{
emit
QNemoProgress
::
progressChanged
();
return
_progress
;
}
const
QVector
<
int
>
&
QNemoProgress
::
progress
()
const
{
return
_progress
;
}
src/Wima/QNemoProgress.h
View file @
d02c3c46
...
@@ -5,22 +5,6 @@
...
@@ -5,22 +5,6 @@
#include "ros_bridge/include/GenericMessages.h"
#include "ros_bridge/include/GenericMessages.h"
namespace
NemoMsgs
=
ROSBridge
::
GenericMessages
::
NemoMsgs
;
namespace
NemoMsgs
=
ROSBridge
::
GenericMessages
::
NemoMsgs
;
typedef
NemoMsgs
::
GenericProgress
<
int
,
QVector
>
ProgressBase
;
typedef
NemoMsgs
::
GenericProgress
<
int
,
QVector
>
QNemoProgress
;
class
QNemoProgress
:
public
ProgressBase
,
public
QObject
{
public:
QNemoProgress
(
QObject
*
parent
=
nullptr
);
QNemoProgress
(
const
QNemoProgress
&
other
,
QObject
*
parent
=
nullptr
);
virtual
QNemoProgress
*
Clone
()
const
override
;
virtual
const
QVector
<
int
>
&
progress
(
void
)
const
override
;
virtual
QVector
<
int
>
&
progress
(
void
)
override
;
signals:
void
progressChanged
();
};
src/Wima/
WimaControllerDetail
.cc
→
src/Wima/
SnakeWorker
.cc
View file @
d02c3c46
#include "
WimaControllerDetail
.h"
#include "
SnakeWorker
.h"
#include <QGeoCoordinate>
#include <QGeoCoordinate>
...
@@ -6,7 +6,10 @@ SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
...
@@ -6,7 +6,10 @@ SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
,
_lineDistance
(
3
)
// meter
,
_lineDistance
(
3
)
// meter
,
_minTransectLength
(
2
)
// meter
,
_minTransectLength
(
2
)
// meter
{
{
}
SnakeWorker
::~
SnakeWorker
()
{
}
}
void
SnakeWorker
::
setScenario
(
const
Scenario
&
scenario
)
void
SnakeWorker
::
setScenario
(
const
Scenario
&
scenario
)
...
@@ -80,6 +83,4 @@ void SnakeWorker::run()
...
@@ -80,6 +83,4 @@ void SnakeWorker::run()
}
}
}
}
emit
resultReady
();
}
}
src/Wima/
WimaControllerDetail
.h
→
src/Wima/
SnakeWorker
.h
View file @
d02c3c46
...
@@ -28,6 +28,7 @@ class SnakeWorker : public QThread{
...
@@ -28,6 +28,7 @@ class SnakeWorker : public QThread{
public:
public:
SnakeWorker
(
QObject
*
parent
=
nullptr
);
SnakeWorker
(
QObject
*
parent
=
nullptr
);
~
SnakeWorker
()
override
;
void
setScenario
(
const
Scenario
&
scenario
);
void
setScenario
(
const
Scenario
&
scenario
);
void
setProgress
(
const
QVector
<
int
>
&
progress
);
void
setProgress
(
const
QVector
<
int
>
&
progress
);
...
@@ -36,9 +37,6 @@ public:
...
@@ -36,9 +37,6 @@ public:
const
WorkerResult_t
&
getResult
()
const
;
const
WorkerResult_t
&
getResult
()
const
;
signals:
void
resultReady
();
protected:
protected:
void
run
()
override
;
void
run
()
override
;
...
...
src/Wima/WaypointManager/GenericPathSlicer.cpp
0 → 100644
View file @
d02c3c46
#include "GenericPathSlicer.h"
src/Wima/WaypointManager/GenericPathSlicer.h
0 → 100644
View file @
d02c3c46
#pragma once
#include <assert.h>
#include <iostream>
//! @brief Base class for all waypoint managers.
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
class
GenericPathSlicer
{
public:
typedef
Container
<
ElementType
>
ContainerType
;
GenericPathSlicer
();
const
ContainerType
&
path
()
const
;
// Waypoint editing.
void
setPath
(
const
ContainerType
&
path
);
void
push_back
(
const
ElementType
&
wp
);
void
push_front
(
const
ElementType
&
wp
);
void
clear
();
void
insert
(
int
i
,
const
ElementType
&
wp
);
uint32_t
size
();
ElementType
&
at
(
unsigned
int
i
);
// Slicing.
void
slice
(
ContainerType
&
slice
);
void
next
(
ContainerType
&
slice
);
void
previous
(
ContainerType
&
slice
);
void
reset
(
ContainerType
&
slice
);
// Slicing parameters.
//! @brief Sets the overlap.
//!
//! @param overlap The number of overlapping vertices
//! between on and the next slice.
void
setOverlap
(
uint32_t
overlap
);
//! @brief Sets the number of vertices per slice.
//!
//! @param N The number of vertices per slice.
void
setN
(
std
::
uint32_t
N
);
//! @brief Sets the start index.
//!
//! @param idxStart The start index.
void
setStartIndex
(
int
idxStart
);
//! @return Returns the overlap.
uint32_t
overlap
();
//! @return Returns the number of vertices per slice N.
uint32_t
N
();
//! @return Returns the start index.
int
startIndex
();
//! @return Returns the end index.
int
endIndex
();
//! @return Returns the start index of the next slice.
int
nextIndex
();
private:
void
_updateIdx
();
ContainerType
_path
;
long
_idxStart
;
long
_idxEnd
;
long
_idxNext
;
long
_idxPrevious
;
uint32_t
_overlap
;
uint32_t
_N
;
bool
_idxValid
;
bool
_atEnd
;
};
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
GenericPathSlicer
<
ElementType
,
Container
>::
GenericPathSlicer
()
:
_idxValid
(
false
)
,
_atEnd
(
false
)
{}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
const
Container
<
ElementType
>
&
GenericPathSlicer
<
ElementType
,
Container
>::
path
()
const
{
return
_path
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
setPath
(
const
ContainerType
&
waypoints
)
{
_idxValid
=
false
;
_path
=
waypoints
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
push_back
(
const
ElementType
&
wp
)
{
_idxValid
=
false
;
_path
.
push_back
(
wp
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
push_front
(
const
ElementType
&
wp
)
{
_idxValid
=
false
;
_path
.
push_front
(
wp
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
clear
(){
_idxValid
=
false
;
_path
.
clear
();
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
insert
(
int
i
,
const
ElementType
&
wp
){
_idxValid
=
false
;
_path
.
insert
(
i
,
wp
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
std
::
uint32_t
GenericPathSlicer
<
ElementType
,
Container
>::
size
()
{
return
std
::
uint32_t
(
_path
.
size
());
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
ElementType
&
GenericPathSlicer
<
ElementType
,
Container
>::
at
(
unsigned
int
i
)
{
return
_path
.
at
(
i
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
setOverlap
(
std
::
uint32_t
overlap
)
{
_idxValid
=
false
;
_overlap
=
overlap
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
setN
(
uint32_t
N
)
{
_idxValid
=
false
;
_N
=
N
>
0
?
N
:
1
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
setStartIndex
(
int
idxStart
)
{
_idxValid
=
false
;
_idxStart
=
idxStart
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
uint32_t
GenericPathSlicer
<
ElementType
,
Container
>::
overlap
()
{
return
_overlap
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
uint32_t
GenericPathSlicer
<
ElementType
,
Container
>::
N
()
{
return
_N
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
int
GenericPathSlicer
<
ElementType
,
Container
>::
startIndex
()
{
if
(
!
_idxValid
)
_updateIdx
();
return
_idxStart
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
int
GenericPathSlicer
<
ElementType
,
Container
>::
endIndex
()
{
if
(
!
_idxValid
)
_updateIdx
();
return
_idxEnd
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
int
GenericPathSlicer
<
ElementType
,
Container
>::
nextIndex
()
{
if
(
!
_idxValid
)
_updateIdx
();
return
_idxNext
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
_updateIdx
()
{
_idxValid
=
true
;
_atEnd
=
false
;
std
::
uint64_t
size
=
_path
.
size
();
if
(
_idxStart
>=
size
-
1
)
{
_idxStart
=
size
-
1
;
_idxEnd
=
_idxStart
;
_idxNext
=
_idxStart
;
_atEnd
=
true
;
return
;
}
_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
;
_idxPrevious
=
_idxStart
-
1
+
_overlap
;
_idxPrevious
=
_idxPrevious
<
0
?
0
:
_idxPrevious
;
_idxPrevious
=
_idxPrevious
<
size
?
_idxPrevious
:
size
-
1
;
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
slice
(
Container
<
ElementType
>
&
c
){
if
(
!
_idxValid
)
_updateIdx
();
(
void
)
c
;
assert
(
false
);
//ToDo
// extract waypoints
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
next
(
Container
<
ElementType
>
&
c
){
setStartIndex
(
_idxNext
);
slice
(
c
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
previous
(
Container
<
ElementType
>
&
c
){
setStartIndex
(
_idxPrevious
);
slice
(
c
);
}
template
<
class
ElementType
,
template
<
class
,
class
...
>
class
Container
/*e.g. QVector*/
>
void
GenericPathSlicer
<
ElementType
,
Container
>::
reset
(
Container
<
ElementType
>
&
c
){
setStartIndex
(
0
);
slice
(
c
);
}
src/Wima/WaypointManager/GenericWaypointManager.cpp
0 → 100644
View file @
d02c3c46
#include "GenericWaypointManager.h"
GenericWaypointManager
::
GenericWaypointManager
()
{
}
src/Wima/WaypointManager/GenericWaypointManager.h
0 → 100644
View file @
d02c3c46
#pragma once
class
GenericWaypointManager
{
public:
GenericWaypointManager
();
};
src/Wima/WimaController.cc
View file @
d02c3c46
...
@@ -17,7 +17,7 @@
...
@@ -17,7 +17,7 @@
#include <QScopedPointer>
#include <QScopedPointer>
const
char
*
WimaController
::
wimaFileExtension
=
"wima"
;
//
const char* WimaController::wimaFileExtension = "wima";
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController"
;
...
@@ -101,29 +101,32 @@ WimaController::WimaController(QObject *parent)
...
@@ -101,29 +101,32 @@ WimaController::WimaController(QObject *parent)
_enableDisableLowBatteryHandling
(
enableLowBatteryHandling
->
rawValue
());
_enableDisableLowBatteryHandling
(
enableLowBatteryHandling
->
rawValue
());
// Snake Worker Thread.
// Snake Worker Thread.
connect
(
&
_snakeWorker
,
&
SnakeWorker
::
resultReady
,
this
,
&
WimaController
::
_snakeStoreWorkerResults
);
connect
(
&
_snakeWorker
,
&
SnakeWorker
::
finished
,
this
,
&
WimaController
::
_snakeStoreWorkerResults
);
connect
(
this
,
&
WimaController
::
nemoProgressChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeWorker
,
&
SnakeWorker
::
quit
);
// Start, stop RosBridge.
// Start, stop RosBridge.
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_startStopRosBridge
);
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_startStopRosBridge
);
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
_startStopRosBridge
();
_startStopRosBridge
();
}
}
QStringList
WimaController
::
loadNameFilters
()
const
//
QStringList WimaController::loadNameFilters() const
{
//
{
QStringList
filters
;
//
QStringList filters;
filters
<<
tr
(
"Supported types (*.%1 *.%2)"
).
arg
(
wimaFileExtension
).
arg
(
AppSettings
::
planFileExtension
)
<<
//
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
tr
(
"All Files (*.*)"
);
//
tr("All Files (*.*)");
return
filters
;
//
return filters;
}
//
}
QStringList
WimaController
::
saveNameFilters
()
const
//
QStringList WimaController::saveNameFilters() const
{
//
{
QStringList
filters
;
//
QStringList filters;
filters
<<
tr
(
"Supported types (*.%1 *.%2)"
).
arg
(
wimaFileExtension
).
arg
(
AppSettings
::
planFileExtension
);
//
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
return
filters
;
//
return filters;
}
//
}
bool
WimaController
::
uploadOverrideRequired
()
const
bool
WimaController
::
uploadOverrideRequired
()
const
{
{
...
@@ -331,37 +334,32 @@ void WimaController::removeVehicleTrajectoryHistory()
...
@@ -331,37 +334,32 @@ void WimaController::removeVehicleTrajectoryHistory()
managerVehicle
->
trajectoryPoints
()
->
clear
();
managerVehicle
->
trajectoryPoints
()
->
clear
();
}
}
void
WimaController
::
saveToCurrent
()
//void WimaController::saveToFile(const QString& filename)
{
//{
// QString file = filename;
//}
}
//bool WimaController::loadFromCurrent()
//{
void
WimaController
::
saveToFile
(
const
QString
&
filename
)
// return true;
{
//}
QString
file
=
filename
;
}
bool
WimaController
::
loadFromCurrent
()
{
return
true
;
}
bool
WimaController
::
loadFromFile
(
const
QString
&
filename
)
//
bool WimaController::loadFromFile(const QString &filename)
{
//
{
QString
file
=
filename
;
//
QString file = filename;
return
true
;
//
return true;
}
//
}
QJsonDocument
WimaController
::
saveToJson
(
FileType
fileType
)
//
QJsonDocument WimaController::saveToJson(FileType fileType)
{
//
{
if
(
fileType
)
//
if(fileType)
{
//
{
}
//
}
return
QJsonDocument
();
//
return QJsonDocument();
}
//
}
bool
WimaController
::
calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
bool
WimaController
::
calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
{
{
...
@@ -1002,31 +1000,21 @@ void WimaController::_eventTimerHandler()
...
@@ -1002,31 +1000,21 @@ void WimaController::_eventTimerHandler()
_checkBatteryLevel
();
_checkBatteryLevel
();
// Snake flight plan update necessary?
// Snake flight plan update necessary?
if
(
snakeEventLoopTicker
.
ready
()
)
{
// if ( snakeEventLoopTicker.ready() ) {
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_localPlanDataValid
&&
!
_snakeCalcInProgress
&&
_scenarioDefinedBool
)
{
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
// }
_snakeWorker
.
setScenario
(
_scenario
);
// }
_snakeWorker
.
setProgress
(
_snakeProgress
.
progress
());
_snakeWorker
.
setLineDistance
(
_snakeLineDistance
.
rawValue
().
toDouble
());
_snakeWorker
.
setMinTransectLength
(
_snakeMinTransectLength
.
rawValue
().
toDouble
());
_setSnakeCalcInProgress
(
true
);
_snakeWorker
.
start
();
emit
snakeProgressChanged
();
}
}
if
(
rosBridgeTicker
.
ready
())
{
if
(
rosBridgeTicker
.
ready
())
{
// using namespace ros_bridge;
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
_pRosBridge
->
publish
(
_snakeTilesLocal
,
"/snake/tiles"
);
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
_pRosBridge
->
publish
(
_snakeOrigin
,
"/snake/origin"
);
using
namespace
std
::
placeholders
;
using
namespace
std
::
placeholders
;
auto
callBack
=
std
::
bind
(
P
rogressFromJson
,
auto
callBack
=
std
::
bind
(
&
WimaController
::
_p
rogressFromJson
,
std
::
ref
(
*
(
_pRosBridge
->
casePacker
()))
,
this
,
_1
,
_1
,
std
::
ref
(
_
snake
Progress
));
std
::
ref
(
_
nemo
Progress
));
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
callBack
);
_pRosBridge
->
subscribe
(
"/nemo/progress"
,
callBack
);
}
}
}
}
...
@@ -1372,11 +1360,32 @@ void WimaController::_snakeStoreWorkerResults()
...
@@ -1372,11 +1360,32 @@ void WimaController::_snakeStoreWorkerResults()
void
WimaController
::
_startStopRosBridge
()
void
WimaController
::
_startStopRosBridge
()
{
{
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
if
(
_enableSnake
.
rawValue
().
toBool
()
)
{
_pRosBridge
->
start
();
_pRosBridge
->
start
();
}
else
{
}
else
{
_pRosBridge
->
stop
();
_pRosBridge
->
reset
();
}
}
void
WimaController
::
_initStartSnakeWorker
()
{
if
(
!
_enableSnake
.
rawValue
().
toBool
()
)
return
;
// Stop worker thread if running.
if
(
_snakeWorker
.
isRunning
()
)
{
_snakeWorker
.
quit
();
}
}
// Initialize _snakeWorker.
_snakeWorker
.
setScenario
(
_scenario
);
_snakeWorker
.
setProgress
(
_nemoProgress
.
progress
());
_snakeWorker
.
setLineDistance
(
_snakeLineDistance
.
rawValue
().
toDouble
());
_snakeWorker
.
setMinTransectLength
(
_snakeMinTransectLength
.
rawValue
().
toDouble
());
_setSnakeCalcInProgress
(
true
);
// Start worker thread.
_snakeWorker
.
start
();
}
}
void
WimaController
::
_loadCurrentMissionItemsFromBuffer
()
void
WimaController
::
_loadCurrentMissionItemsFromBuffer
()
...
@@ -1398,7 +1407,14 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
...
@@ -1398,7 +1407,14 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
_missionItemsBuffer
.
append
(
_currentMissionItems
.
removeAt
(
0
));
_missionItemsBuffer
.
append
(
_currentMissionItems
.
removeAt
(
0
));
}
}
void
ProgressFromJson
(
const
ROSBridge
::
CasePacker
&
casePacker
,
JsonDocUPtr
pDoc
,
QNemoProgress
&
progress
)
void
WimaController
::
_progressFromJson
(
JsonDocUPtr
pDoc
,
QNemoProgress
&
progress
)
{
{
casePacker
.
unpack
(
pDoc
,
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 @
d02c3c46
...
@@ -27,7 +27,7 @@
...
@@ -27,7 +27,7 @@
#include "SettingsManager.h"
#include "SettingsManager.h"
#include "snake.h"
#include "snake.h"
#include "
WimaControllerDetail
.h"
#include "
SnakeWorker
.h"
#include "SnakeTiles.h"
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
#include "SnakeTilesLocal.h"
#include "GeoPoint3D.h"
#include "GeoPoint3D.h"
...
@@ -65,10 +65,10 @@ public:
...
@@ -65,10 +65,10 @@ public:
Q_PROPERTY
(
PlanMasterController
*
masterController
READ
masterController
WRITE
setMasterController
NOTIFY
masterControllerChanged
)
Q_PROPERTY
(
PlanMasterController
*
masterController
READ
masterController
WRITE
setMasterController
NOTIFY
masterControllerChanged
)
Q_PROPERTY
(
MissionController
*
missionController
READ
missionController
WRITE
setMissionController
NOTIFY
missionControllerChanged
)
Q_PROPERTY
(
MissionController
*
missionController
READ
missionController
WRITE
setMissionController
NOTIFY
missionControllerChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
visualItemsChanged
)
Q_PROPERTY
(
QString
currentFile
READ
currentFile
NOTIFY
currentFileChanged
)
//
Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged)
Q_PROPERTY
(
QStringList
loadNameFilters
READ
loadNameFilters
CONSTANT
)
//
Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT)
Q_PROPERTY
(
QStringList
saveNameFilters
READ
saveNameFilters
CONSTANT
)
//
Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT)
Q_PROPERTY
(
QString
fileExtension
READ
fileExtension
CONSTANT
)
//
Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT)
Q_PROPERTY
(
WimaDataContainer
*
dataContainer
READ
dataContainer
WRITE
setDataContainer
NOTIFY
dataContainerChanged
)
Q_PROPERTY
(
WimaDataContainer
*
dataContainer
READ
dataContainer
WRITE
setDataContainer
NOTIFY
dataContainerChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
NOTIFY
missionItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
NOTIFY
missionItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
currentMissionItems
READ
currentMissionItems
NOTIFY
currentMissionItemsChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
currentMissionItems
READ
currentMissionItems
NOTIFY
currentMissionItemsChanged
)
...
@@ -100,7 +100,7 @@ public:
...
@@ -100,7 +100,7 @@ public:
Q_PROPERTY
(
Fact
*
snakeMinTransectLength
READ
snakeMinTransectLength
CONSTANT
)
Q_PROPERTY
(
Fact
*
snakeMinTransectLength
READ
snakeMinTransectLength
CONSTANT
)
Q_PROPERTY
(
QmlObjectListModel
*
snakeTiles
READ
snakeTiles
NOTIFY
snakeTilesChanged
)
Q_PROPERTY
(
QmlObjectListModel
*
snakeTiles
READ
snakeTiles
NOTIFY
snakeTilesChanged
)
Q_PROPERTY
(
QVariantList
snakeTileCenterPoints
READ
snakeTileCenterPoints
NOTIFY
snakeTileCenterPointsChanged
)
Q_PROPERTY
(
QVariantList
snakeTileCenterPoints
READ
snakeTileCenterPoints
NOTIFY
snakeTileCenterPointsChanged
)
Q_PROPERTY
(
QVector
<
int
>
*
snakeProgress
READ
snakeProgress
NOTIFY
snake
ProgressChanged
)
Q_PROPERTY
(
QVector
<
int
>
nemoProgress
READ
nemoProgress
NOTIFY
nemo
ProgressChanged
)
...
@@ -108,10 +108,10 @@ public:
...
@@ -108,10 +108,10 @@ public:
PlanMasterController
*
masterController
(
void
)
{
return
_masterController
;
}
PlanMasterController
*
masterController
(
void
)
{
return
_masterController
;
}
MissionController
*
missionController
(
void
)
{
return
_missionController
;
}
MissionController
*
missionController
(
void
)
{
return
_missionController
;
}
QmlObjectListModel
*
visualItems
(
void
)
{
return
&
_visualItems
;
}
QmlObjectListModel
*
visualItems
(
void
)
{
return
&
_visualItems
;
}
QString
currentFile
(
void
)
const
{
return
_currentFile
;
}
//
QString currentFile (void) const { return _currentFile; }
QStringList
loadNameFilters
(
void
)
const
;
//
QStringList loadNameFilters (void) const;
QStringList
saveNameFilters
(
void
)
const
;
//
QStringList saveNameFilters (void) const;
QString
fileExtension
(
void
)
const
{
return
wimaFileExtension
;
}
//
QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon
joinedArea
(
void
)
const
;
QGCMapPolygon
joinedArea
(
void
)
const
;
WimaDataContainer
*
dataContainer
(
void
)
{
return
_container
;
}
WimaDataContainer
*
dataContainer
(
void
)
{
return
_container
;
}
QmlObjectListModel
*
missionItems
(
void
)
{
return
&
_missionItems
;
}
QmlObjectListModel
*
missionItems
(
void
)
{
return
&
_missionItems
;
}
...
@@ -137,7 +137,7 @@ public:
...
@@ -137,7 +137,7 @@ public:
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;}
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;}
QmlObjectListModel
*
snakeTiles
(
void
)
{
return
_snakeTiles
.
QmlObjectListModel
();}
QmlObjectListModel
*
snakeTiles
(
void
)
{
return
_snakeTiles
.
QmlObjectListModel
();}
QVariantList
snakeTileCenterPoints
(
void
)
{
return
_snakeTileCenterPoints
;}
QVariantList
snakeTileCenterPoints
(
void
)
{
return
_snakeTileCenterPoints
;}
QVector
<
int
>
*
snakeProgress
(
void
)
{
return
&
_snake
Progress
.
progress
();}
QVector
<
int
>
nemoProgress
(
void
)
{
return
_nemo
Progress
.
progress
();}
bool
uploadOverrideRequired
(
void
)
const
;
bool
uploadOverrideRequired
(
void
)
const
;
double
phaseDistance
(
void
)
const
;
double
phaseDistance
(
void
)
const
;
...
@@ -168,14 +168,14 @@ public:
...
@@ -168,14 +168,14 @@ public:
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
void
saveToCurrent
();
//
Q_INVOKABLE void saveToCurrent ();
Q_INVOKABLE
void
saveToFile
(
const
QString
&
filename
);
//
Q_INVOKABLE void saveToFile (const QString& filename);
Q_INVOKABLE
bool
loadFromCurrent
();
//
Q_INVOKABLE bool loadFromCurrent();
Q_INVOKABLE
bool
loadFromFile
(
const
QString
&
filename
);
//
Q_INVOKABLE bool loadFromFile (const QString& filename);
// static Members
// static Members
static
const
char
*
wimaFileExtension
;
//
static const char* wimaFileExtension;
static
const
char
*
areaItemsName
;
static
const
char
*
areaItemsName
;
static
const
char
*
missionItemsName
;
static
const
char
*
missionItemsName
;
static
const
char
*
settingsGroup
;
static
const
char
*
settingsGroup
;
...
@@ -213,7 +213,7 @@ signals:
...
@@ -213,7 +213,7 @@ signals:
void
masterControllerChanged
(
void
);
void
masterControllerChanged
(
void
);
void
missionControllerChanged
(
void
);
void
missionControllerChanged
(
void
);
void
visualItemsChanged
(
void
);
void
visualItemsChanged
(
void
);
void
currentFileChanged
();
//
void currentFileChanged ();
void
dataContainerChanged
();
void
dataContainerChanged
();
void
readyForSaveSendChanged
(
bool
ready
);
void
readyForSaveSendChanged
(
bool
ready
);
void
missionItemsChanged
(
void
);
void
missionItemsChanged
(
void
);
...
@@ -230,7 +230,7 @@ signals:
...
@@ -230,7 +230,7 @@ signals:
void
snakeCalcInProgressChanged
(
void
);
void
snakeCalcInProgressChanged
(
void
);
void
snakeTilesChanged
(
void
);
void
snakeTilesChanged
(
void
);
void
snakeTileCenterPointsChanged
(
void
);
void
snakeTileCenterPointsChanged
(
void
);
void
snakeProgressChanged
(
void
);
void
nemoProgressChanged
(
void
);
private:
private:
enum
SRTL_Reason
{
BatteryLow
,
UserRequest
};
enum
SRTL_Reason
{
BatteryLow
,
UserRequest
};
private
slots
:
private
slots
:
...
@@ -257,7 +257,7 @@ private slots:
...
@@ -257,7 +257,7 @@ private slots:
bool
_verifyScenarioDefinedWithErrorMessage
(
void
);
bool
_verifyScenarioDefinedWithErrorMessage
(
void
);
void
_snakeStoreWorkerResults
();
void
_snakeStoreWorkerResults
();
void
_startStopRosBridge
();
void
_startStopRosBridge
();
void
_initStartSnakeWorker
();
private:
private:
void
_setPhaseDistance
(
double
distance
);
void
_setPhaseDistance
(
double
distance
);
...
@@ -268,9 +268,12 @@ private:
...
@@ -268,9 +268,12 @@ private:
void
_loadCurrentMissionItemsFromBuffer
();
void
_loadCurrentMissionItemsFromBuffer
();
void
_saveCurrentMissionItemsToBuffer
();
void
_saveCurrentMissionItemsToBuffer
();
void
_progressFromJson
(
JsonDocUPtr
pDoc
,
QNemoProgress
&
progress
);
PlanMasterController
*
_masterController
;
PlanMasterController
*
_masterController
;
MissionController
*
_missionController
;
MissionController
*
_missionController
;
QString
_currentFile
;
// file for saveing
//
QString _currentFile; // file for saveing
WimaDataContainer
*
_container
;
// container for data exchange with WimaController
WimaDataContainer
*
_container
;
// container for data exchange with WimaController
QmlObjectListModel
_visualItems
;
// contains all visible areas
QmlObjectListModel
_visualItems
;
// contains all visible areas
WimaJoinedAreaData
_joinedArea
;
// joined area fromed by opArea, serArea, _corridor
WimaJoinedAreaData
_joinedArea
;
// joined area fromed by opArea, serArea, _corridor
...
@@ -339,14 +342,12 @@ private:
...
@@ -339,14 +342,12 @@ private:
SnakeTiles
_snakeTiles
;
// tiles
SnakeTiles
_snakeTiles
;
// tiles
SnakeTilesLocal
_snakeTilesLocal
;
// tiles local coordinate system
SnakeTilesLocal
_snakeTilesLocal
;
// tiles local coordinate system
QVariantList
_snakeTileCenterPoints
;
QVariantList
_snakeTileCenterPoints
;
QNemoProgress
_
snake
Progress
;
// measurement progress
QNemoProgress
_
nemo
Progress
;
// measurement progress
ROSBridgePtr
_pRosBridge
;
ROSBridgePtr
_pRosBridge
;
};
};
void
ProgressFromJson
(
const
ROSBridge
::
CasePacker
&
casePacker
,
JsonDocUPtr
pDoc
,
QNemoProgress
&
progress
);
/*
/*
* The following explains the structure of
* The following explains the structure of
* _missionController.visualItems().
* _missionController.visualItems().
...
...
src/Wima/WimaPlaner.cc
View file @
d02c3c46
...
@@ -562,7 +562,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
...
@@ -562,7 +562,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
_arrivalPathLength
=
path
.
size
()
-
1
;
_arrivalPathLength
=
path
.
size
()
-
1
;
int
sequenceNumber
=
0
;
int
sequenceNumber
=
0
;
for
(
int
i
=
1
;
i
<
path
.
count
()
-
1
;
i
++
)
{
for
(
int
i
=
1
;
i
<
path
.
count
()
-
1
;
i
++
)
{
sequenceNumber
=
_missionController
->
insertSimpleMissionItem
(
path
.
value
(
i
)
,
missionItems
->
count
()
-
1
);
sequenceNumber
=
_missionController
->
insertSimpleMissionItem
(
path
[
i
]
,
missionItems
->
count
()
-
1
);
_missionController
->
setCurrentPlanViewIndex
(
sequenceNumber
,
true
);
_missionController
->
setCurrentPlanViewIndex
(
sequenceNumber
,
true
);
}
}
...
@@ -576,7 +576,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
...
@@ -576,7 +576,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
}
}
_returnPathLength
=
path
.
size
()
-
1
;
// -1: fist item is last measurement point
_returnPathLength
=
path
.
size
()
-
1
;
// -1: fist item is last measurement point
for
(
int
i
=
1
;
i
<
path
.
count
()
-
1
;
i
++
)
{
for
(
int
i
=
1
;
i
<
path
.
count
()
-
1
;
i
++
)
{
sequenceNumber
=
_missionController
->
insertSimpleMissionItem
(
path
.
value
(
i
)
,
missionItems
->
count
());
sequenceNumber
=
_missionController
->
insertSimpleMissionItem
(
path
[
i
]
,
missionItems
->
count
());
_missionController
->
setCurrentPlanViewIndex
(
sequenceNumber
,
true
);
_missionController
->
setCurrentPlanViewIndex
(
sequenceNumber
,
true
);
}
}
...
...
src/comm/ros_bridge/include/CasePacker.h
View file @
d02c3c46
...
@@ -30,7 +30,7 @@ public:
...
@@ -30,7 +30,7 @@ public:
template
<
class
T
>
template
<
class
T
>
bool
unpack
(
JsonDocUPtr
&
pDoc
,
T
&
msg
)
const
{
bool
unpack
(
JsonDocUPtr
&
pDoc
,
T
&
msg
)
const
{
removeTag
(
pDoc
);
removeTag
(
pDoc
);
return
_typeFactory
->
create
(
pDoc
,
msg
);
return
_typeFactory
->
create
(
*
pDoc
.
get
()
,
msg
);
}
}
...
...
src/comm/ros_bridge/include/GenericMessages.h
View file @
d02c3c46
...
@@ -331,6 +331,7 @@ public:
...
@@ -331,6 +331,7 @@ public:
GenericProgress
(){}
GenericProgress
(){}
GenericProgress
(
const
ContainterType
<
IntType
>
&
progress
)
:
_progress
(
progress
){}
GenericProgress
(
const
ContainterType
<
IntType
>
&
progress
)
:
_progress
(
progress
){}
GenericProgress
(
const
GenericProgress
&
p
)
:
_progress
(
p
.
progress
()){}
GenericProgress
(
const
GenericProgress
&
p
)
:
_progress
(
p
.
progress
()){}
~
GenericProgress
()
{}
virtual
GenericProgress
*
Clone
()
const
override
{
return
new
GenericProgress
(
*
this
);
}
virtual
GenericProgress
*
Clone
()
const
override
{
return
new
GenericProgress
(
*
this
);
}
...
...
src/comm/ros_bridge/include/ROSBridge.h
View file @
d02c3c46
...
@@ -28,12 +28,14 @@ public:
...
@@ -28,12 +28,14 @@ public:
}
}
void
publish
(
JsonDocUPtr
doc
);
void
publish
(
JsonDocUPtr
doc
);
void
subscribe
(
const
char
*
topic
,
const
std
::
function
<
(
JsonDocUPtr
)
>
&
callBack
);
void
subscribe
(
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callBack
);
const
CasePacker
*
casePacker
()
const
;
const
CasePacker
*
casePacker
()
const
;
//! @brief Start ROS bridge.
void
start
();
void
start
();
void
stop
();
//! @brief Reset ROS bridge.
void
reset
();
private:
private:
TypeFactory
_typeFactory
;
TypeFactory
_typeFactory
;
...
...
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @
d02c3c46
#include "TopicPublisher.h"
#include "TopicPublisher.h"
ROSBridge
::
ComPrivate
::
TopicPublisher
::
TopicPublisher
(
CasePacker
*
casePacker
,
RosbridgeWsClient
*
rbc
)
:
_running
(
false
)
,
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
{
}
ROSBridge
::
ComPrivate
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
reset
();
}
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
)));
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
reset
()
{
if
(
!
_running
.
load
()
)
// stop called while thread not running.
return
;
_running
.
store
(
false
);
if
(
!
_pThread
)
return
;
_pThread
->
join
();
_pThread
.
reset
();
_rbc
->
removeClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
_queue
.
clear
();
_advertisedTopicsHashList
.
clear
();
}
void
ROSBridge
::
ComPrivate
::
transmittLoop
(
const
ROSBridge
::
CasePacker
&
casePacker
,
void
ROSBridge
::
ComPrivate
::
transmittLoop
(
const
ROSBridge
::
CasePacker
&
casePacker
,
RosbridgeWsClient
&
rbc
,
RosbridgeWsClient
&
rbc
,
ROSBridge
::
ComPrivate
::
JsonQueue
&
queue
,
ROSBridge
::
ComPrivate
::
JsonQueue
&
queue
,
std
::
mutex
&
queueMutex
,
std
::
mutex
&
queueMutex
,
HashSet
&
advertisedTopicsHashList
,
HashSet
&
advertisedTopicsHashList
,
const
std
::
atomic
<
bool
>
&
stopFla
g
)
const
std
::
atomic
<
bool
>
&
runnin
g
)
{
{
rbc
.
addClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
rbc
.
addClient
(
ROSBridge
::
ComPrivate
::
_topicPublisherKey
);
while
(
!
stopFla
g
.
load
()){
while
(
runnin
g
.
load
()){
// Pop message from queue.
// Pop message from queue.
queueMutex
.
lock
();
queueMutex
.
lock
();
//std::cout << "Queue size: " << queue.size() << std::endl;
//std::cout << "Queue size: " << queue.size() << std::endl;
...
@@ -55,43 +99,3 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
...
@@ -55,43 +99,3 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
}
// while loop
}
// while loop
}
}
ROSBridge
::
ComPrivate
::
TopicPublisher
::
TopicPublisher
(
CasePacker
*
casePacker
,
RosbridgeWsClient
*
rbc
)
:
_stopFlag
(
true
)
,
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
{
}
ROSBridge
::
ComPrivate
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
stop
();
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
start
()
{
if
(
!
_stopFlag
.
load
()
)
// start called while thread running.
return
;
_stopFlag
.
store
(
false
);
_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
(
_stopFlag
)));
}
void
ROSBridge
::
ComPrivate
::
TopicPublisher
::
stop
()
{
if
(
_stopFlag
.
load
()
)
// start called while thread not running.
return
;
_stopFlag
.
store
(
true
);
if
(
!
_pThread
)
return
;
_pThread
->
join
();
_pThread
.
reset
();
_rbc
->
removeClient
(
ROSBridge
::
ComPrivate
::
_topicAdvertiserKey
);
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @
d02c3c46
...
@@ -25,8 +25,12 @@ public:
...
@@ -25,8 +25,12 @@ public:
~
TopicPublisher
();
~
TopicPublisher
();
//! @brief Starts the publisher.
void
start
();
void
start
();
void
stop
();
//! @brief Resets the publisher.
void
reset
();
template
<
class
T
>
template
<
class
T
>
void
publish
(
const
T
&
msg
,
const
std
::
string
&
topic
){
void
publish
(
const
T
&
msg
,
const
std
::
string
&
topic
){
JsonDocUPtr
docPtr
(
_casePacker
->
pack
(
msg
,
topic
));
JsonDocUPtr
docPtr
(
_casePacker
->
pack
(
msg
,
topic
));
...
@@ -40,7 +44,7 @@ public:
...
@@ -40,7 +44,7 @@ public:
private:
private:
JsonQueue
_queue
;
JsonQueue
_queue
;
std
::
mutex
_queueMutex
;
std
::
mutex
_queueMutex
;
std
::
atomic
<
bool
>
_
stopFla
g
;
std
::
atomic
<
bool
>
_
runnin
g
;
CasePacker
*
_casePacker
;
CasePacker
*
_casePacker
;
ThreadPtr
_pThread
;
ThreadPtr
_pThread
;
RosbridgeWsClient
*
_rbc
;
RosbridgeWsClient
*
_rbc
;
...
@@ -54,7 +58,7 @@ void transmittLoop(const ROSBridge::CasePacker &casePacker,
...
@@ -54,7 +58,7 @@ void transmittLoop(const ROSBridge::CasePacker &casePacker,
ROSBridge
::
ComPrivate
::
JsonQueue
&
queue
,
ROSBridge
::
ComPrivate
::
JsonQueue
&
queue
,
std
::
mutex
&
queueMutex
,
std
::
mutex
&
queueMutex
,
HashSet
&
advertisedTopicsHashList
,
HashSet
&
advertisedTopicsHashList
,
const
std
::
atomic
<
bool
>
&
stopFla
g
);
const
std
::
atomic
<
bool
>
&
runnin
g
);
}
// namespace CommunicatorDetail
}
// namespace CommunicatorDetail
...
...
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @
d02c3c46
...
@@ -6,32 +6,64 @@ ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
...
@@ -6,32 +6,64 @@ ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
RosbridgeWsClient
*
rbc
)
:
RosbridgeWsClient
*
rbc
)
:
_casePacker
(
casePacker
)
_casePacker
(
casePacker
)
,
_rbc
(
rbc
)
,
_rbc
(
rbc
)
,
_running
(
false
)
{
{
}
}
ROSBridge
::
ComPrivate
::
TopicSubscriber
::~
TopicSubscriber
()
{
this
->
reset
();
}
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
start
()
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
start
()
{
{
_r
bc
->
addClient
(
ROSBridge
::
ComPrivate
::
_topicSubscriberKey
)
;
_r
unning
=
true
;
}
}
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
stop
()
void
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
reset
()
{
{
_rbc
->
removeClient
(
ROSBridge
::
ComPrivate
::
_topicSubscriberKey
);
if
(
_running
){
for
(
std
::
string
clientName
:
_clientList
)
_rbc
->
removeClient
(
clientName
);
_running
=
false
;
_callbackMap
.
clear
();
_clientList
.
clear
();
}
}
}
bool
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
subscribe
(
bool
ROSBridge
::
ComPrivate
::
TopicSubscriber
::
subscribe
(
const
char
*
topic
,
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
{
{
using
namespace
std
::
placeholders
;
if
(
!
_running
)
HashType
hash
=
getHash
(
topic
);
return
false
;
std
::
string
clientName
=
ROSBridge
::
ComPrivate
::
_topicSubscriberKey
+
std
::
string
(
topic
);
_clientList
.
push_back
(
clientName
);
HashType
hash
=
getHash
(
clientName
);
auto
ret
=
_callbackMap
.
insert
(
std
::
make_pair
(
hash
,
callback
));
//
auto
ret
=
_callbackMap
.
insert
(
std
::
make_pair
(
hash
,
callback
));
//
if
(
!
ret
.
second
)
if
(
!
ret
.
second
)
return
false
;
// Topic subscription already present.
return
false
;
// Topic subscription already present.
auto
f
=
std
::
bind
(
&
ROSBridge
::
ComPrivate
::
subscriberCallback
,
hash
,
_callbackMap
,
_1
,
_2
);
{
_rbc
->
subscribe
(
ROSBridge
::
ComPrivate
::
_topicSubscriberKey
,
topic
,
f
);
using
namespace
std
::
placeholders
;
auto
f
=
std
::
bind
(
&
ROSBridge
::
ComPrivate
::
subscriberCallback
,
hash
,
std
::
cref
(
_callbackMap
),
_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
);
}
return
true
;
return
true
;
}
}
...
@@ -78,3 +110,13 @@ void ROSBridge::ComPrivate::subscriberCallback(
...
@@ -78,3 +110,13 @@ void ROSBridge::ComPrivate::subscriberCallback(
return
;
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 @
d02c3c46
...
@@ -10,15 +10,20 @@ namespace ComPrivate {
...
@@ -10,15 +10,20 @@ namespace ComPrivate {
class
TopicSubscriber
class
TopicSubscriber
{
{
typedef
std
::
vector
<
std
::
string
>
ClientList
;
public:
public:
typedef
std
::
function
<
void
(
JsonDocUPtr
)
>
CallbackType
;
typedef
std
::
function
<
void
(
JsonDocUPtr
)
>
CallbackType
;
typedef
std
::
map
<
HashType
,
CallbackType
>
CallbackMap
;
typedef
std
::
map
<
HashType
,
CallbackType
>
CallbackMap
;
TopicSubscriber
()
=
delete
;
TopicSubscriber
()
=
delete
;
TopicSubscriber
(
CasePacker
*
casePacker
,
RosbridgeWsClient
*
rbc
);
TopicSubscriber
(
CasePacker
*
casePacker
,
RosbridgeWsClient
*
rbc
);
~
TopicSubscriber
();
//! @brief Starts the subscriber.
void
start
();
void
start
();
void
stop
();
//! @brief Resets the subscriber.
void
reset
();
//! @return Returns false if a subscription to this topic allready exists.
//! @return Returns false if a subscription to this topic allready exists.
//!
//!
...
@@ -29,6 +34,8 @@ private:
...
@@ -29,6 +34,8 @@ private:
CasePacker
*
_casePacker
;
CasePacker
*
_casePacker
;
RosbridgeWsClient
*
_rbc
;
RosbridgeWsClient
*
_rbc
;
CallbackMap
_callbackMap
;
CallbackMap
_callbackMap
;
ClientList
_clientList
;
bool
_running
;
};
};
void
subscriberCallback
(
const
HashType
&
hash
,
void
subscriberCallback
(
const
HashType
&
hash
,
...
@@ -36,5 +43,7 @@ void subscriberCallback(const HashType &hash,
...
@@ -36,5 +43,7 @@ void subscriberCallback(const HashType &hash,
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
Connection
>
/*connection*/
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
);
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 @
d02c3c46
...
@@ -30,9 +30,9 @@ void ROSBridge::ROSBridge::start()
...
@@ -30,9 +30,9 @@ void ROSBridge::ROSBridge::start()
_topicSubscriber
.
start
();
_topicSubscriber
.
start
();
}
}
void
ROSBridge
::
ROSBridge
::
stop
()
void
ROSBridge
::
ROSBridge
::
reset
()
{
{
_topicPublisher
.
stop
();
_topicPublisher
.
reset
();
_topicSubscriber
.
stop
();
_topicSubscriber
.
reset
();
}
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment