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
50929c20
Commit
50929c20
authored
Sep 14, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
1243
parent
65679db5
Changes
17
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
967 additions
and
419 deletions
+967
-419
qgroundcontrol.pro
qgroundcontrol.pro
+4
-2
snake.cpp
src/Snake/snake.cpp
+24
-24
snake.h
src/Snake/snake.h
+1
-1
NemoInterface.cpp
src/Wima/Snake/NemoInterface.cpp
+22
-6
NemoInterface.h
src/Wima/Snake/NemoInterface.h
+3
-2
SnakeThread.cc
src/Wima/Snake/SnakeThread.cc
+543
-0
SnakeThread.h
src/Wima/Snake/SnakeThread.h
+9
-4
SnakeTile.cpp
src/Wima/Snake/SnakeTile.cpp
+8
-16
SnakeTile.h
src/Wima/Snake/SnakeTile.h
+7
-9
WimaController.cc
src/Wima/WimaController.cc
+98
-69
WimaController.h
src/Wima/WimaController.h
+11
-10
com_private.h
src/comm/ros_bridge/include/com_private.h
+3
-8
server.cpp
src/comm/ros_bridge/include/server.cpp
+56
-67
topic_publisher.cpp
src/comm/ros_bridge/include/topic_publisher.cpp
+116
-129
topic_subscriber.cpp
src/comm/ros_bridge/include/topic_subscriber.cpp
+60
-72
MainWindowInner.qml
src/ui/MainWindowInner.qml
+1
-0
MainWindowNative.qml
src/ui/MainWindowNative.qml
+1
-0
No files found.
qgroundcontrol.pro
View file @
50929c20
...
@@ -28,11 +28,13 @@ QGCROOT = $$PWD
...
@@ -28,11 +28,13 @@ QGCROOT = $$PWD
DebugBuild
{
DebugBuild
{
DESTDIR
=
$$
{
OUT_PWD
}
/
debug
DESTDIR
=
$$
{
OUT_PWD
}
/
debug
DEFINES
+=
DEBUG
DEFINES
+=
DEBUG
DEFINES
+=
SNAKE_SHOW_TIME
#
DEFINES
+=
ROS_BRIDGE_DEBUG
#
DEFINES
+=
ROS_BRIDGE_DEBUG
}
}
else
{
else
{
DESTDIR
=
$$
{
OUT_PWD
}
/
release
DESTDIR
=
$$
{
OUT_PWD
}
/
release
#
DEFINES
+=
ROS_BRIDGE_DEBUG
#
DEFINES
+=
ROS_BRIDGE_DEBUG
DEFINES
+=
SNAKE_SHOW_TIME
DEFINES
+=
NDEBUG
DEFINES
+=
NDEBUG
}
}
...
@@ -438,11 +440,11 @@ HEADERS += \
...
@@ -438,11 +440,11 @@ HEADERS += \
src
/
Wima
/
Snake
/
QNemoHeartbeat
.
h
\
src
/
Wima
/
Snake
/
QNemoHeartbeat
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
QNemoProgress
.
h
\
src
/
Wima
/
Snake
/
SnakeThread
.
h
\
src
/
Wima
/
Snake
/
SnakeTile
.
h
\
src
/
Wima
/
Snake
/
SnakeTile
.
h
\
src
/
Wima
/
Snake
/
SnakeTileLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeTileLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeTiles
.
h
\
src
/
Wima
/
Snake
/
SnakeTiles
.
h
\
src
/
Wima
/
Snake
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeTilesLocal
.
h
\
src
/
Wima
/
Snake
/
SnakeDataManager
.
h
\
src
/
Wima
/
WaypointManager
/
AreaInterface
.
h
\
src
/
Wima
/
WaypointManager
/
AreaInterface
.
h
\
src
/
Wima
/
WaypointManager
/
DefaultManager
.
h
\
src
/
Wima
/
WaypointManager
/
DefaultManager
.
h
\
src
/
Wima
/
WaypointManager
/
GenericWaypointManager
.
h
\
src
/
Wima
/
WaypointManager
/
GenericWaypointManager
.
h
\
...
@@ -503,7 +505,7 @@ SOURCES += \
...
@@ -503,7 +505,7 @@ SOURCES += \
src
/
Wima
/
Geometry
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
Geometry
/
GeoPoint3D
.
cpp
\
src
/
Wima
/
Snake
/
NemoInterface
.
cpp
\
src
/
Wima
/
Snake
/
NemoInterface
.
cpp
\
src
/
Wima
/
Snake
/
QNemoProgress
.
cc
\
src
/
Wima
/
Snake
/
QNemoProgress
.
cc
\
src
/
Wima
/
Snake
/
Snake
DataManager
.
cc
\
src
/
Wima
/
Snake
/
Snake
Thread
.
cc
\
src
/
Wima
/
Snake
/
SnakeTile
.
cpp
\
src
/
Wima
/
Snake
/
SnakeTile
.
cpp
\
src
/
Wima
/
WaypointManager
/
AreaInterface
.
cpp
\
src
/
Wima
/
WaypointManager
/
AreaInterface
.
cpp
\
src
/
Wima
/
WaypointManager
/
DefaultManager
.
cpp
\
src
/
Wima
/
WaypointManager
/
DefaultManager
.
cpp
\
...
...
src/Snake/snake.cpp
View file @
50929c20
...
@@ -22,7 +22,7 @@
...
@@ -22,7 +22,7 @@
using
namespace
operations_research
;
using
namespace
operations_research
;
#ifndef NDEBUG
#ifndef NDEBUG
//#define SHOW_TIME
//#define S
NAKE_S
HOW_TIME
#endif
#endif
namespace
bg
=
boost
::
geometry
;
namespace
bg
=
boost
::
geometry
;
...
@@ -104,7 +104,7 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
...
@@ -104,7 +104,7 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
//# Compute edges (x2-x1,y2-y1)
//# Compute edges (x2-x1,y2-y1)
std
::
vector
<
BoostPoint
>
edges
;
std
::
vector
<
BoostPoint
>
edges
;
auto
convex_hull_outer
=
convex_hull
.
outer
();
const
auto
&
convex_hull_outer
=
convex_hull
.
outer
();
for
(
long
i
=
0
;
i
<
long
(
convex_hull_outer
.
size
())
-
1
;
++
i
)
{
for
(
long
i
=
0
;
i
<
long
(
convex_hull_outer
.
size
())
-
1
;
++
i
)
{
BoostPoint
p1
=
convex_hull_outer
.
at
(
i
);
BoostPoint
p1
=
convex_hull_outer
.
at
(
i
);
BoostPoint
p2
=
convex_hull_outer
.
at
(
i
+
1
);
BoostPoint
p2
=
convex_hull_outer
.
at
(
i
+
1
);
...
@@ -711,13 +711,12 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
...
@@ -711,13 +711,12 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
// Rotate measurement area by angle and calculate bounding box.
// Rotate measurement area by angle and calculate bounding box.
auto
&
mArea
=
scenario
.
measurementArea
();
auto
&
mArea
=
scenario
.
measurementArea
();
BoostPolygon
mAreaRotated
;
BoostPolygon
mAreaRotated
;
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
-
angle
.
value
()
*
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
angle
.
value
()
*
180
/
M_PI
);
180
/
M_PI
);
bg
::
transform
(
mArea
,
mAreaRotated
,
rotate
);
bg
::
transform
(
mArea
,
mAreaRotated
,
rotate
);
BoostBox
box
;
BoostBox
box
;
boost
::
geometry
::
envelope
(
mAreaRotated
,
box
);
boost
::
geometry
::
envelope
(
mAreaRotated
,
box
);
double
x0
=
box
.
min_corner
().
get
<
0
>
();
double
x0
=
box
.
min_corner
().
get
<
0
>
();
double
y0
=
box
.
min_corner
().
get
<
1
>
();
double
y0
=
box
.
min_corner
().
get
<
1
>
();
double
x1
=
box
.
max_corner
().
get
<
0
>
();
double
x1
=
box
.
max_corner
().
get
<
0
>
();
...
@@ -725,8 +724,6 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
...
@@ -725,8 +724,6 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
// Generate transects and convert them to clipper path.
// Generate transects and convert them to clipper path.
size_t
num_t
=
int
(
ceil
((
y1
-
y0
)
/
distance
.
value
()));
// number of transects
size_t
num_t
=
int
(
ceil
((
y1
-
y0
)
/
distance
.
value
()));
// number of transects
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate_back
(
angle
.
value
()
*
180
/
M_PI
);
vector
<
ClipperLib
::
Path
>
transectsClipper
;
vector
<
ClipperLib
::
Path
>
transectsClipper
;
transectsClipper
.
reserve
(
num_t
);
transectsClipper
.
reserve
(
num_t
);
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
...
@@ -736,17 +733,20 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
...
@@ -736,17 +733,20 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
BoostLineString
transect
;
BoostLineString
transect
;
transect
.
push_back
(
v1
);
transect
.
push_back
(
v1
);
transect
.
push_back
(
v2
);
transect
.
push_back
(
v2
);
// transform back
// transform back
BoostLineString
temp_transect
;
BoostLineString
temp_transect
;
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate_back
(
-
angle
.
value
()
*
180
/
M_PI
);
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
// to clipper
ClipperLib
::
IntPoint
c1
{
ClipperLib
::
IntPoint
c1
{
static_cast
<
ClipperLib
::
cInt
>
(
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
temp_transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
static_cast
<
ClipperLib
::
cInt
>
(
ClipperLib
::
IntPoint
c2
{
temp_transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
ClipperLib
::
IntPoint
c2
{
static_cast
<
ClipperLib
::
cInt
>
(
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
temp_transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
temp_transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
Path
path
{
c1
,
c2
};
ClipperLib
::
Path
path
{
c1
,
c2
};
transectsClipper
.
push_back
(
path
);
transectsClipper
.
push_back
(
path
);
}
}
...
@@ -852,22 +852,22 @@ void generateRoutingModel(const BoostLineString &vertices,
...
@@ -852,22 +852,22 @@ void generateRoutingModel(const BoostLineString &vertices,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
)
{
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
)
{
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
#endif
graphFromPolygon
(
polygonOffset
,
vertices
,
graph
);
graphFromPolygon
(
polygonOffset
,
vertices
,
graph
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
<<
endl
;
#endif
#endif
Matrix
<
double
>
distanceMatrix
(
graph
);
Matrix
<
double
>
distanceMatrix
(
graph
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
#endif
toDistanceMatrix
(
distanceMatrix
);
toDistanceMatrix
(
distanceMatrix
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
...
@@ -934,12 +934,12 @@ bool flight_plan::route(const BoostPolygon &area,
...
@@ -934,12 +934,12 @@ bool flight_plan::route(const BoostPolygon &area,
// Offset joined area.
// Offset joined area.
BoostPolygon
areaOffset
;
BoostPolygon
areaOffset
;
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
#endif
generateRoutingModel
(
vertices
,
areaOffset
,
n0
,
dataModel
,
connectionGraph
);
generateRoutingModel
(
vertices
,
areaOffset
,
n0
,
dataModel
,
connectionGraph
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
<<
endl
;
...
@@ -995,11 +995,11 @@ bool flight_plan::route(const BoostPolygon &area,
...
@@ -995,11 +995,11 @@ bool flight_plan::route(const BoostPolygon &area,
searchParameters
.
set_allocated_time_limit
(
tMax
);
searchParameters
.
set_allocated_time_limit
(
tMax
);
// Solve the problem.
// Solve the problem.
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
#endif
#endif
const
Assignment
*
solution
=
routing
.
SolveWithParameters
(
searchParameters
);
const
Assignment
*
solution
=
routing
.
SolveWithParameters
(
searchParameters
);
#ifdef SHOW_TIME
#ifdef S
NAKE_S
HOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time routing.SolveWithParameters(): "
<<
delta
.
count
()
cout
<<
"Execution time routing.SolveWithParameters(): "
<<
delta
.
count
()
...
...
src/Snake/snake.h
View file @
50929c20
...
@@ -173,7 +173,7 @@ public:
...
@@ -173,7 +173,7 @@ public:
const
BoundingBox
&
measurementAreaBBox
()
const
;
const
BoundingBox
&
measurementAreaBBox
()
const
;
const
BoostPoint
&
homePositon
()
const
;
const
BoostPoint
&
homePositon
()
const
;
bool
update
()
const
;
bool
update
();
mutable
string
errorString
;
mutable
string
errorString
;
...
...
src/Wima/Snake/NemoInterface.cpp
View file @
50929c20
...
@@ -213,6 +213,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
...
@@ -213,6 +213,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
this
->
pRosBridge
->
subscribe
(
this
->
pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
"/nemo/heartbeat"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
)
{
/* callback */
[
this
](
JsonDocUPtr
pDoc
)
{
// auto start = std::chrono::high_resolution_clock::now();
UniqueLock
lk
(
this
->
heartbeatMutex
);
UniqueLock
lk
(
this
->
heartbeatMutex
);
auto
&
heartbeatMsg
=
this
->
heartbeat
;
auto
&
heartbeatMsg
=
this
->
heartbeat
;
...
@@ -223,8 +224,15 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
...
@@ -223,8 +224,15 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
this
->
nextTimeout
=
this
->
nextTimeout
=
std
::
chrono
::
high_resolution_clock
::
now
()
+
timeoutInterval
;
std
::
chrono
::
high_resolution_clock
::
now
()
+
timeoutInterval
;
}
}
lk
.
unlock
();
emit
this
->
parent
->
statusChanged
();
emit
this
->
parent
->
statusChanged
();
// auto delta =
// std::chrono::duration_cast<std::chrono::milliseconds>(
// std::chrono::high_resolution_clock::now() - start);
// std::cout << "/nemo/heartbeat callback time: " <<
// delta.count() << " ms"
// << std::endl;
});
});
// Advertise /snake/get_origin.
// Advertise /snake/get_origin.
...
@@ -289,11 +297,12 @@ void NemoInterface::Impl::loop() {
...
@@ -289,11 +297,12 @@ void NemoInterface::Impl::loop() {
}
}
// Check if heartbeat timeout occured.
// Check if heartbeat timeout occured.
if
(
this
->
running
&&
this
->
topicServiceSetupDone
&&
if
(
this
->
running
&&
this
->
topicServiceSetupDone
)
{
this
->
nextTimeout
!=
TimePoint
::
max
())
{
UniqueLock
lk
(
this
->
heartbeatMutex
);
if
(
this
->
nextTimeout
<
std
::
chrono
::
high_resolution_clock
::
now
())
{
if
(
this
->
nextTimeout
!=
TimePoint
::
max
()
&&
UniqueLock
lk
(
this
->
heartbeatMutex
);
this
->
nextTimeout
<
std
::
chrono
::
high_resolution_clock
::
now
())
{
this
->
heartbeat
.
setStatus
(
integral
(
NemoStatus
::
Timeout
));
this
->
heartbeat
.
setStatus
(
integral
(
NemoStatus
::
Timeout
));
lk
.
unlock
();
emit
this
->
parent
->
statusChanged
();
emit
this
->
parent
->
statusChanged
();
}
}
}
}
...
@@ -321,6 +330,13 @@ void NemoInterface::Impl::publishENUOrigin() {
...
@@ -321,6 +330,13 @@ void NemoInterface::Impl::publishENUOrigin() {
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
}
}
// ===============================================================
// NemoInterface
NemoInterface
::
NemoInterface
(
QObject
*
parent
)
:
QObject
(
parent
),
pImpl
(
std
::
make_unique
<
NemoInterface
::
Impl
>
(
this
))
{}
NemoInterface
::~
NemoInterface
()
{}
void
NemoInterface
::
start
()
{
this
->
pImpl
->
start
();
}
void
NemoInterface
::
start
()
{
this
->
pImpl
->
start
();
}
void
NemoInterface
::
stop
()
{
this
->
pImpl
->
stop
();
}
void
NemoInterface
::
stop
()
{
this
->
pImpl
->
stop
();
}
...
@@ -333,8 +349,8 @@ void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
...
@@ -333,8 +349,8 @@ void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
this
->
pImpl
->
setENUOrigin
(
ENUOrigin
);
this
->
pImpl
->
setENUOrigin
(
ENUOrigin
);
}
}
NemoInterface
::
NemoStatus
NemoInterface
::
status
()
{
NemoInterface
::
NemoStatus
NemoInterface
::
status
()
const
{
return
this
->
pImpl
->
status
();
return
this
->
pImpl
->
status
();
}
}
QVector
<
int
>
NemoInterface
::
progress
()
{
return
this
->
pImpl
->
progress
();
}
QVector
<
int
>
NemoInterface
::
progress
()
const
{
return
this
->
pImpl
->
progress
();
}
src/Wima/Snake/NemoInterface.h
View file @
50929c20
...
@@ -21,6 +21,7 @@ public:
...
@@ -21,6 +21,7 @@ public:
};
};
explicit
NemoInterface
(
QObject
*
parent
=
nullptr
);
explicit
NemoInterface
(
QObject
*
parent
=
nullptr
);
~
NemoInterface
()
override
;
void
start
();
void
start
();
void
stop
();
void
stop
();
...
@@ -28,8 +29,8 @@ public:
...
@@ -28,8 +29,8 @@ public:
void
setTilesENU
(
const
SnakeTilesLocal
&
tilesENU
);
void
setTilesENU
(
const
SnakeTilesLocal
&
tilesENU
);
void
setENUOrigin
(
const
QGeoCoordinate
&
ENUOrigin
);
void
setENUOrigin
(
const
QGeoCoordinate
&
ENUOrigin
);
NemoStatus
status
();
NemoStatus
status
()
const
;
QVector
<
int
>
progress
();
QVector
<
int
>
progress
()
const
;
signals:
signals:
void
statusChanged
();
void
statusChanged
();
...
...
src/Wima/Snake/Snake
DataManager
.cc
→
src/Wima/Snake/Snake
Thread
.cc
View file @
50929c20
This diff is collapsed.
Click to expand it.
src/Wima/Snake/Snake
DataManager
.h
→
src/Wima/Snake/Snake
Thread
.h
View file @
50929c20
...
@@ -15,29 +15,34 @@ using namespace boost::units;
...
@@ -15,29 +15,34 @@ using namespace boost::units;
using
Length
=
quantity
<
si
::
length
>
;
using
Length
=
quantity
<
si
::
length
>
;
using
Area
=
quantity
<
si
::
area
>
;
using
Area
=
quantity
<
si
::
area
>
;
class
Snake
DataManager
:
public
QThread
{
class
Snake
Thread
:
public
QThread
{
Q_OBJECT
Q_OBJECT
class
Impl
;
class
Impl
;
using
PImpl
=
std
::
unique_ptr
<
Impl
>
;
using
PImpl
=
std
::
unique_ptr
<
Impl
>
;
public:
public:
Snake
DataManager
(
QObject
*
parent
=
nullptr
);
Snake
Thread
(
QObject
*
parent
=
nullptr
);
~
Snake
DataManager
()
override
;
~
Snake
Thread
()
override
;
void
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
);
void
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
);
void
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
);
void
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
);
void
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
);
void
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
);
void
setProgress
(
const
QVector
<
int
>
&
progress
);
SnakeTiles
tiles
()
const
;
SnakeTiles
tiles
()
const
;
SnakeTilesLocal
tilesENU
()
const
;
SnakeTilesLocal
tilesENU
()
const
;
QGeoCoordinate
ENUOrigin
()
const
;
QGeoCoordinate
ENUOrigin
()
const
;
QVariantList
tileCenterPoints
()
const
;
QVariantList
tileCenterPoints
()
const
;
Q
NemoProgress
progress
()
const
;
Q
Vector
<
int
>
progress
()
const
;
bool
calcInProgress
()
const
;
bool
calcInProgress
()
const
;
QString
errorMessage
()
const
;
QString
errorMessage
()
const
;
bool
success
()
const
;
bool
success
()
const
;
bool
tilesUpdated
();
bool
waypointsUpdated
();
bool
progressUpdated
();
QVector
<
QGeoCoordinate
>
waypoints
()
const
;
QVector
<
QGeoCoordinate
>
waypoints
()
const
;
QVector
<
QGeoCoordinate
>
arrivalPath
()
const
;
QVector
<
QGeoCoordinate
>
arrivalPath
()
const
;
QVector
<
QGeoCoordinate
>
returnPath
()
const
;
QVector
<
QGeoCoordinate
>
returnPath
()
const
;
...
...
src/Wima/Snake/SnakeTile.cpp
View file @
50929c20
#include "SnakeTile.h"
#include "SnakeTile.h"
SnakeTile
::
SnakeTile
()
:
WimaAreaData
()
SnakeTile
::
SnakeTile
()
:
WimaAreaData
()
{}
{
SnakeTile
::
SnakeTile
(
const
SnakeTile
&
other
,
QObject
*
parent
)
:
WimaAreaData
(
parent
)
{
*
this
=
other
;
}
}
SnakeTile
::
SnakeTile
(
const
SnakeTile
&
other
)
:
WimaAreaData
()
SnakeTile
&
SnakeTile
::
operator
=
(
const
SnakeTile
&
other
)
{
{
this
->
assign
(
other
);
*
this
=
other
;
return
*
this
;
}
}
SnakeTile
&
SnakeTile
::
operator
=
(
const
SnakeTile
&
other
)
void
SnakeTile
::
assign
(
const
SnakeTile
&
other
)
{
WimaAreaData
::
assign
(
other
);
}
{
this
->
assign
(
other
);
return
*
this
;
}
void
SnakeTile
::
assign
(
const
SnakeTile
&
other
)
{
WimaAreaData
::
assign
(
other
);
}
src/Wima/Snake/SnakeTile.h
View file @
50929c20
...
@@ -2,18 +2,16 @@
...
@@ -2,18 +2,16 @@
#include "Wima/Geometry/WimaAreaData.h"
#include "Wima/Geometry/WimaAreaData.h"
class
SnakeTile
:
public
WimaAreaData
class
SnakeTile
:
public
WimaAreaData
{
{
public:
public:
SnakeTile
();
SnakeTile
();
SnakeTile
(
const
SnakeTile
&
othe
r
);
SnakeTile
(
const
SnakeTile
&
other
,
QObject
*
parent
=
nullpt
r
);
QString
type
()
const
{
return
"Tile"
;
}
QString
type
()
const
{
return
"Tile"
;
}
SnakeTile
*
Clone
()
const
{
return
new
SnakeTile
(
*
this
);
}
SnakeTile
*
Clone
()
const
{
return
new
SnakeTile
(
*
this
);
}
SnakeTile
&
operator
=
(
const
SnakeTile
&
other
);
SnakeTile
&
operator
=
(
const
SnakeTile
&
other
);
protected:
protected:
void
assign
(
const
SnakeTile
&
other
);
void
assign
(
const
SnakeTile
&
other
);
};
};
src/Wima/WimaController.cc
View file @
50929c20
This diff is collapsed.
Click to expand it.
src/Wima/WimaController.h
View file @
50929c20
...
@@ -27,7 +27,8 @@
...
@@ -27,7 +27,8 @@
#include "SurveyComplexItem.h"
#include "SurveyComplexItem.h"
#include "Geometry/GeoPoint3D.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/NemoInterface.h"
#include "Snake/SnakeThread.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
#include "Snake/SnakeTilesLocal.h"
...
@@ -237,19 +238,16 @@ private slots:
...
@@ -237,19 +238,16 @@ private slots:
void
_initSmartRTL
();
void
_initSmartRTL
();
void
_smartRTLCleanUp
(
bool
flying
);
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
// Snake.
void
_DMFinishedHandler
();
void
_threadFinishedHandler
();
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
void
_switchToSnakeWaypointManager
(
QVariant
variant
);
void
_switchToSnakeWaypointManager
(
QVariant
variant
);
void
_switch
DataManager
(
SnakeDataManager
&
dataManager
);
void
_switch
ThreadObject
(
SnakeThread
&
thread
);
void
_progressChangedHandler
();
void
_progressChangedHandler
();
void
_enableSnakeChangedHandler
();
void
_enableSnakeChangedHandler
();
// Periodic tasks.
// Periodic tasks.
void
_eventTimerHandler
(
void
);
void
_eventTimerHandler
(
void
);
// Waypoint manager handling.
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
private:
private:
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
// Controllers.
// Controllers.
PlanMasterController
*
_masterController
;
PlanMasterController
*
_masterController
;
MissionController
*
_missionController
;
MissionController
*
_missionController
;
...
@@ -310,9 +308,12 @@ private:
...
@@ -310,9 +308,12 @@ private:
double
_measurementPathLength
;
// the lenght of the phase in meters
double
_measurementPathLength
;
// the lenght of the phase in meters
// Snake
// Snake
SnakeDataManager
_snakeDM
;
// Snake Data Manager
QmlObjectListModel
tiles
;
SnakeDataManager
_emptyDM
;
SnakeThread
_snakeThread
;
// Snake Data Manager
SnakeDataManager
*
_currentDM
;
SnakeThread
_emptyThread
;
SnakeThread
*
_currentThread
;
NemoInterface
_nemoInterface
;
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
static
StatusMap
_nemoStatusMap
;
static
StatusMap
_nemoStatusMap
;
// Periodic tasks.
// Periodic tasks.
...
...
src/comm/ros_bridge/include/com_private.h
View file @
50929c20
...
@@ -2,8 +2,8 @@
...
@@ -2,8 +2,8 @@
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <memory>
#include <deque>
#include <deque>
#include <memory>
#include <unordered_map>
#include <unordered_map>
namespace
ros_bridge
{
namespace
ros_bridge
{
...
@@ -16,11 +16,6 @@ typedef std::size_t HashType;
...
@@ -16,11 +16,6 @@ typedef std::size_t HashType;
using
ClientMap
=
std
::
unordered_map
<
HashType
,
std
::
string
>
;
using
ClientMap
=
std
::
unordered_map
<
HashType
,
std
::
string
>
;
static
const
char
*
_topicAdvertiserKey
=
"topic_advertiser"
;
static
const
char
*
_topicPublisherKey
=
"topic_publisher"
;
static
const
char
*
_serviceAdvertiserKey
=
"service_advertiser"
;
static
const
char
*
_topicSubscriberKey
=
"topic_subscriber"
;
std
::
size_t
getHash
(
const
std
::
string
&
str
);
std
::
size_t
getHash
(
const
std
::
string
&
str
);
std
::
size_t
getHash
(
const
char
*
str
);
std
::
size_t
getHash
(
const
char
*
str
);
...
@@ -30,5 +25,5 @@ bool getType(const JsonDoc &doc, std::string &type);
...
@@ -30,5 +25,5 @@ bool getType(const JsonDoc &doc, std::string &type);
bool
removeTopic
(
JsonDoc
&
doc
);
bool
removeTopic
(
JsonDoc
&
doc
);
bool
removeType
(
JsonDoc
&
doc
);
bool
removeType
(
JsonDoc
&
doc
);
}
}
// namespace com_private
}
}
// namespace ros_bridge
src/comm/ros_bridge/include/server.cpp
View file @
50929c20
...
@@ -2,98 +2,87 @@
...
@@ -2,98 +2,87 @@
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
#include "rapidjson/include/rapidjson/ostreamwrapper.h"
ros_bridge
::
com_private
::
Server
::
Server
(
RosbridgeWsClient
&
rbc
)
:
static
const
char
*
serviceAdvertiserKey
=
"service_advertiser"
;
_rbc
(
rbc
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
}
ros_bridge
::
com_private
::
Server
::
Server
(
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
),
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{}
void
ros_bridge
::
com_private
::
Server
::
advertiseService
(
const
std
::
string
&
service
,
void
ros_bridge
::
com_private
::
Server
::
advertiseService
(
const
std
::
string
&
type
,
const
std
::
string
&
service
,
const
std
::
string
&
type
,
const
Server
::
CallbackType
&
userCallback
)
const
Server
::
CallbackType
&
userCallback
)
{
{
std
::
string
clientName
=
serviceAdvertiserKey
+
service
;
std
::
string
clientName
=
_serviceAdvertiserKey
+
service
;
auto
it
=
_serviceMap
.
find
(
clientName
);
auto
it
=
_serviceMap
.
find
(
clientName
);
if
(
it
!=
_serviceMap
.
end
())
if
(
it
!=
_serviceMap
.
end
())
return
;
// Service allready advertised.
return
;
// Service allready advertised.
_serviceMap
.
insert
(
std
::
make_pair
(
clientName
,
service
));
_serviceMap
.
insert
(
std
::
make_pair
(
clientName
,
service
));
_rbc
.
addClient
(
clientName
);
_rbc
.
addClient
(
clientName
);
auto
rbc
=
&
_rbc
;
auto
rbc
=
&
_rbc
;
_rbc
.
advertiseService
(
clientName
,
service
,
type
,
_rbc
.
advertiseService
(
[
userCallback
,
rbc
](
clientName
,
service
,
type
,
std
::
shared_ptr
<
WsClient
::
Connection
>
,
[
userCallback
,
rbc
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
){
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
// message->string() is destructive, so we have to buffer it first
// message->string() is destructive, so we have to buffer it first
std
::
string
messagebuf
=
in_message
->
string
();
std
::
string
messagebuf
=
in_message
->
string
();
//std::cout << "advertiseServiceCallback(): Message Received: "
//
std::cout << "advertiseServiceCallback(): Message Received: "
// << messagebuf << std::endl;
// << messagebuf << std::endl;
rapidjson
::
Document
document
;
rapidjson
::
Document
document
;
if
(
document
.
Parse
(
messagebuf
.
c_str
()).
HasParseError
())
if
(
document
.
Parse
(
messagebuf
.
c_str
()).
HasParseError
())
{
{
std
::
cerr
<<
"advertiseServiceCallback(): Error in parsing service "
std
::
cerr
<<
"advertiseServiceCallback(): Error in parsing service
request message: "
"
request message: "
<<
messagebuf
<<
std
::
endl
;
<<
messagebuf
<<
std
::
endl
;
return
;
return
;
}
}
if
(
!
document
.
HasMember
(
"args"
)
||
!
document
[
"args"
].
IsObject
()){
if
(
!
document
.
HasMember
(
"args"
)
||
!
document
[
"args"
].
IsObject
())
{
std
::
cerr
<<
"advertiseServiceCallback(): message has no args member: "
std
::
cerr
<<
messagebuf
<<
std
::
endl
;
<<
"advertiseServiceCallback(): message has no args member: "
return
;
<<
messagebuf
<<
std
::
endl
;
return
;
}
}
if
(
!
document
.
HasMember
(
"service"
)
||
!
document
[
"service"
].
IsString
()){
if
(
!
document
.
HasMember
(
"service"
)
||
!
document
[
"service"
].
IsString
())
{
std
::
cerr
<<
"advertiseServiceCallback(): message has no service member: "
std
::
cerr
<<
messagebuf
<<
std
::
endl
;
<<
"advertiseServiceCallback(): message has no service member: "
return
;
<<
messagebuf
<<
std
::
endl
;
return
;
}
}
if
(
!
document
.
HasMember
(
"id"
)
||
!
document
[
"id"
].
IsString
())
{
if
(
!
document
.
HasMember
(
"id"
)
||
!
document
[
"id"
].
IsString
())
{
std
::
cerr
<<
"advertiseServiceCallback(): message has no id member: "
std
::
cerr
<<
"advertiseServiceCallback(): message has no id member: "
<<
messagebuf
<<
std
::
endl
;
<<
messagebuf
<<
std
::
endl
;
return
;
return
;
}
}
JsonDocUPtr
pDoc
(
new
JsonDoc
());
JsonDocUPtr
pDoc
(
new
JsonDoc
());
std
::
cout
<<
"args member count: "
<<
document
[
"args"
].
MemberCount
();
std
::
cout
<<
"args member count: "
<<
document
[
"args"
].
MemberCount
();
if
(
document
[
"args"
].
MemberCount
()
>
0
)
if
(
document
[
"args"
].
MemberCount
()
>
0
)
pDoc
->
CopyFrom
(
document
[
"args"
].
Move
(),
document
.
GetAllocator
());
pDoc
->
CopyFrom
(
document
[
"args"
].
Move
(),
document
.
GetAllocator
());
JsonDocUPtr
pDocResponse
=
userCallback
(
std
::
move
(
pDoc
));
JsonDocUPtr
pDocResponse
=
userCallback
(
std
::
move
(
pDoc
));
rbc
->
serviceResponse
(
rbc
->
serviceResponse
(
document
[
"service"
].
GetString
(),
document
[
"service"
].
GetString
(),
document
[
"id"
].
GetString
(),
document
[
"id"
].
GetString
(),
pDocResponse
->
MemberCount
()
>
0
?
true
:
false
,
*
pDocResponse
);
pDocResponse
->
MemberCount
()
>
0
?
true
:
false
,
*
pDocResponse
);
return
;
return
;
});
});
}
}
ros_bridge
::
com_private
::
Server
::~
Server
()
ros_bridge
::
com_private
::
Server
::~
Server
()
{
this
->
reset
();
}
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
Server
::
start
()
void
ros_bridge
::
com_private
::
Server
::
start
()
{
_stopped
->
store
(
false
);
}
{
_stopped
->
store
(
false
);
}
void
ros_bridge
::
com_private
::
Server
::
reset
()
void
ros_bridge
::
com_private
::
Server
::
reset
()
{
{
if
(
_stopped
->
load
())
if
(
_stopped
->
load
()
)
return
;
return
;
std
::
cout
<<
"Server: _stopped->store(true)"
<<
std
::
endl
;
std
::
cout
<<
"Server: _stopped->store(true)"
<<
std
::
endl
;
_stopped
->
store
(
true
);
_stopped
->
store
(
true
);
for
(
const
auto
&
item
:
_serviceMap
)
{
for
(
const
auto
&
item
:
_serviceMap
){
std
::
cout
<<
"Server: unadvertiseService "
<<
item
.
second
<<
std
::
endl
;
std
::
cout
<<
"Server: unadvertiseService "
<<
item
.
second
<<
std
::
endl
;
_rbc
.
unadvertiseService
(
item
.
second
);
_rbc
.
unadvertiseService
(
item
.
second
);
std
::
cout
<<
"Server: removeClient "
<<
item
.
first
<<
std
::
endl
;
std
::
cout
<<
"Server: removeClient "
<<
item
.
first
<<
std
::
endl
;
_rbc
.
removeClient
(
item
.
first
);
_rbc
.
removeClient
(
item
.
first
);
}
}
std
::
cout
<<
"Server: _serviceMap cleared "
<<
std
::
endl
;
std
::
cout
<<
"Server: _serviceMap cleared "
<<
std
::
endl
;
_serviceMap
.
clear
();
_serviceMap
.
clear
();
}
}
src/comm/ros_bridge/include/topic_publisher.cpp
View file @
50929c20
#include "topic_publisher.h"
#include "topic_publisher.h"
#include <unordered_map>
static
const
char
*
topicAdvertiserKey
=
"topic_advertiser"
;
ros_bridge
::
com_private
::
TopicPublisher
::
TopicPublisher
(
RosbridgeWsClient
&
rbc
)
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
,
_rbc
(
rbc
)
{
}
ros_bridge
::
com_private
::
TopicPublisher
::~
TopicPublisher
()
#include <unordered_map>
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
start
()
ros_bridge
::
com_private
::
TopicPublisher
::
TopicPublisher
(
RosbridgeWsClient
&
rbc
)
{
:
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
)),
_rbc
(
rbc
)
{}
if
(
!
_stopped
->
load
()
)
// start called while thread running.
return
;
ros_bridge
::
com_private
::
TopicPublisher
::~
TopicPublisher
()
{
this
->
reset
();
}
_stopped
->
store
(
false
);
_pThread
=
std
::
make_unique
<
std
::
thread
>
([
this
]{
void
ros_bridge
::
com_private
::
TopicPublisher
::
start
()
{
// Main Loop.
if
(
!
_stopped
->
load
())
// start called while thread running.
while
(
!
this
->
_stopped
->
load
()
){
return
;
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
_stopped
->
store
(
false
);
// Check if new data available, wait if not.
_pThread
=
std
::
make_unique
<
std
::
thread
>
([
this
]
{
if
(
this
->
_queue
.
empty
()){
// Main Loop.
if
(
_stopped
->
load
()
)
// Check condition again while holding the lock.
while
(
!
this
->
_stopped
->
load
())
{
break
;
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
this
->
_cv
.
wait
(
lk
);
// Wait for condition, spurious wakeups don't matter in this case.
// Check if new data available, wait if not.
continue
;
if
(
this
->
_queue
.
empty
())
{
}
if
(
_stopped
->
load
())
// Check condition again while holding the lock.
// Pop message from queue.
break
;
JsonDocUPtr
pJsonDoc
=
std
::
move
(
this
->
_queue
.
front
());
this
->
_cv
.
wait
(
lk
);
// Wait for condition, spurious wakeups don't matter
this
->
_queue
.
pop_front
();
// in this case.
lk
.
unlock
();
continue
;
}
// Get topic and type from Json message and remove it.
// Pop message from queue.
std
::
string
topic
;
JsonDocUPtr
pJsonDoc
=
std
::
move
(
this
->
_queue
.
front
());
bool
ret
=
com_private
::
getTopic
(
*
pJsonDoc
,
topic
);
this
->
_queue
.
pop_front
();
assert
(
ret
);
lk
.
unlock
();
(
void
)
ret
;
// Get topic and type from Json message and remove it.
std
::
string
topic
;
// Debug
bool
ret
=
com_private
::
getTopic
(
*
pJsonDoc
,
topic
);
rapidjson
::
StringBuffer
sb
;
assert
(
ret
);
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
sb
);
(
void
)
ret
;
pJsonDoc
->
Accept
(
writer
);
std
::
cout
<<
"message: "
<<
sb
.
GetString
()
<<
std
::
endl
;
// Debug
std
::
cout
<<
"topic: "
<<
topic
<<
std
::
endl
;
rapidjson
::
StringBuffer
sb
;
rapidjson
::
Writer
<
rapidjson
::
StringBuffer
>
writer
(
sb
);
pJsonDoc
->
Accept
(
writer
);
ret
=
com_private
::
removeTopic
(
*
pJsonDoc
);
std
::
cout
<<
"message: "
<<
sb
.
GetString
()
<<
std
::
endl
;
assert
(
ret
);
std
::
cout
<<
"topic: "
<<
topic
<<
std
::
endl
;
(
void
)
ret
;
ret
=
com_private
::
removeTopic
(
*
pJsonDoc
);
// Wait for topic (Rosbridge needs some time to process a advertise() request).
assert
(
ret
);
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]{
(
void
)
ret
;
return
this
->
_stopped
->
load
();
});
// Wait for topic (Rosbridge needs some time to process a advertise()
// request).
// Publish Json message.
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]
{
return
this
->
_stopped
->
load
();
});
if
(
!
this
->
_stopped
->
load
()
)
this
->
_rbc
.
publish
(
topic
,
*
pJsonDoc
);
// Publish Json message.
}
// while loop
if
(
!
this
->
_stopped
->
load
())
this
->
_rbc
.
publish
(
topic
,
*
pJsonDoc
);
}
// while loop
#ifdef ROS_BRIDGE_DEBUG
#ifdef ROS_BRIDGE_DEBUG
std
::
cout
<<
"TopicPublisher: publisher thread terminated."
<<
std
::
endl
;
std
::
cout
<<
"TopicPublisher: publisher thread terminated."
<<
std
::
endl
;
#endif
#endif
});
});
}
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
reset
()
void
ros_bridge
::
com_private
::
TopicPublisher
::
reset
()
{
{
if
(
_stopped
->
load
())
// stop called while thread not running.
if
(
_stopped
->
load
()
)
// stop called while thread not running.
return
;
return
;
std
::
unique_lock
<
std
::
mutex
>
lk
(
_mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
_mutex
);
_stopped
->
store
(
true
);
_stopped
->
store
(
true
);
_cv
.
notify_one
();
// Wake publisher thread.
_cv
.
notify_one
();
// Wake publisher thread.
lk
.
unlock
();
lk
.
unlock
();
if
(
!
_pThread
)
if
(
!
_pThread
)
return
;
return
;
_pThread
->
join
();
_pThread
->
join
();
lk
.
lock
();
lk
.
lock
();
// Tidy up.
// Tidy up.
for
(
auto
&
it
:
this
->
_topicMap
)
{
for
(
auto
&
it
:
this
->
_topicMap
){
this
->
_rbc
.
unadvertise
(
it
.
first
);
this
->
_rbc
.
unadvertise
(
it
.
first
);
this
->
_rbc
.
removeClient
(
it
.
second
);
this
->
_rbc
.
removeClient
(
it
.
second
);
}
}
this
->
_topicMap
.
clear
();
this
->
_topicMap
.
clear
();
_queue
.
clear
();
_queue
.
clear
();
}
}
void
ros_bridge
::
com_private
::
TopicPublisher
::
publish
(
void
ros_bridge
::
com_private
::
TopicPublisher
::
publish
(
ros_bridge
::
com_private
::
JsonDocUPtr
docPtr
,
ros_bridge
::
com_private
::
JsonDocUPtr
docPtr
,
const
char
*
topic
)
{
const
char
*
topic
)
// Check if topic was advertised.
{
std
::
string
t
(
topic
);
// Check if topic was advertised.
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
std
::
string
t
(
topic
);
auto
it
=
this
->
_topicMap
.
find
(
t
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
if
(
it
==
this
->
_topicMap
.
end
())
{
// Not yet advertised?
auto
it
=
this
->
_topicMap
.
find
(
t
);
lk
.
unlock
();
if
(
it
==
this
->
_topicMap
.
end
())
{
// Not yet advertised?
lk
.
unlock
();
#ifdef ROS_BRIDGE_DEBUG
#ifdef ROS_BRIDGE_DEBUG
std
::
cerr
<<
"TopicPublisher: topic
"
std
::
cerr
<<
"TopicPublisher: topic "
<<
t
<<
" not yet advertised
"
<<
t
<<
" not yet advertised"
<<
std
::
endl
;
<<
std
::
endl
;
#endif
#endif
return
;
return
;
}
}
lk
.
unlock
();
lk
.
unlock
();
// Add topic information to json doc.
// Add topic information to json doc.
auto
&
allocator
=
docPtr
->
GetAllocator
();
auto
&
allocator
=
docPtr
->
GetAllocator
();
rapidjson
::
Value
key
(
"topic"
,
allocator
);
rapidjson
::
Value
key
(
"topic"
,
allocator
);
rapidjson
::
Value
value
(
topic
,
allocator
);
rapidjson
::
Value
value
(
topic
,
allocator
);
docPtr
->
AddMember
(
key
,
value
,
allocator
);
docPtr
->
AddMember
(
key
,
value
,
allocator
);
lk
.
lock
();
lk
.
lock
();
_queue
.
push_back
(
std
::
move
(
docPtr
));
_queue
.
push_back
(
std
::
move
(
docPtr
));
lk
.
unlock
();
lk
.
unlock
();
_cv
.
notify_one
();
// Wake publisher thread.
_cv
.
notify_one
();
// Wake publisher thread.
}
}
bool
ros_bridge
::
com_private
::
TopicPublisher
::
advertise
(
const
char
*
topic
,
const
char
*
type
)
bool
ros_bridge
::
com_private
::
TopicPublisher
::
advertise
(
const
char
*
topic
,
{
const
char
*
type
)
{
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
std
::
unique_lock
<
std
::
mutex
>
lk
(
this
->
_mutex
);
std
::
string
t
(
topic
);
std
::
string
t
(
topic
);
auto
it
=
this
->
_topicMap
.
find
(
t
);
auto
it
=
this
->
_topicMap
.
find
(
t
);
if
(
it
==
this
->
_topicMap
.
end
())
{
// Need to advertise topic?
if
(
it
==
this
->
_topicMap
.
end
())
{
// Need to advertise topic?
std
::
string
clientName
=
std
::
string
clientName
=
std
::
string
(
topicAdvertiserKey
)
+
t
;
std
::
string
(
ros_bridge
::
com_private
::
_topicAdvertiserKey
)
this
->
_topicMap
.
insert
(
std
::
make_pair
(
t
,
clientName
));
+
t
;
lk
.
unlock
();
this
->
_topicMap
.
insert
(
std
::
make_pair
(
t
,
clientName
));
this
->
_rbc
.
addClient
(
clientName
);
lk
.
unlock
();
this
->
_rbc
.
advertise
(
clientName
,
t
,
type
);
this
->
_rbc
.
addClient
(
clientName
);
return
true
;
this
->
_rbc
.
advertise
(
clientName
,
t
,
type
);
}
else
{
return
true
;
lk
.
unlock
();
}
else
{
lk
.
unlock
();
#if ROS_BRIDGE_DEBUG
#if ROS_BRIDGE_DEBUG
std
::
cerr
<<
"TopicPublisher: topic "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
std
::
cerr
<<
"TopicPublisher: topic "
<<
topic
<<
" already advertised"
<<
std
::
endl
;
#endif
#endif
return
false
;
return
false
;
}
}
}
}
src/comm/ros_bridge/include/topic_subscriber.cpp
View file @
50929c20
...
@@ -2,98 +2,86 @@
...
@@ -2,98 +2,86 @@
#include <thread>
#include <thread>
ros_bridge
::
com_private
::
TopicSubscriber
::
TopicSubscriber
(
RosbridgeWsClient
&
rbc
)
:
static
const
char
*
topicSubscriberKey
=
"topic_subscriber"
;
_rbc
(
rbc
)
,
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{
}
ros_bridge
::
com_private
::
TopicSubscriber
::
TopicSubscriber
(
RosbridgeWsClient
&
rbc
)
:
_rbc
(
rbc
),
_stopped
(
std
::
make_shared
<
std
::
atomic_bool
>
(
true
))
{}
ros_bridge
::
com_private
::
TopicSubscriber
::~
TopicSubscriber
()
ros_bridge
::
com_private
::
TopicSubscriber
::~
TopicSubscriber
()
{
this
->
reset
();
}
{
this
->
reset
();
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
start
()
void
ros_bridge
::
com_private
::
TopicSubscriber
::
start
()
{
{
_stopped
->
store
(
false
);
_stopped
->
store
(
false
);
}
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
reset
()
void
ros_bridge
::
com_private
::
TopicSubscriber
::
reset
()
{
{
if
(
!
_stopped
->
load
())
{
if
(
!
_stopped
->
load
()
){
_stopped
->
store
(
true
);
_stopped
->
store
(
true
);
{
{
for
(
auto
&
item
:
_topicMap
)
{
for
(
auto
&
item
:
_topicMap
){
_rbc
.
unsubscribe
(
item
.
second
);
_rbc
.
unsubscribe
(
item
.
second
);
_rbc
.
removeClient
(
item
.
first
);
_rbc
.
removeClient
(
item
.
first
);
}
}
}
_topicMap
.
clear
();
}
}
_topicMap
.
clear
();
}
}
}
void
ros_bridge
::
com_private
::
TopicSubscriber
::
subscribe
(
void
ros_bridge
::
com_private
::
TopicSubscriber
::
subscribe
(
const
char
*
topic
,
const
char
*
topic
,
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
{
const
std
::
function
<
void
(
JsonDocUPtr
)
>
&
callback
)
if
(
_stopped
->
load
())
{
return
;
if
(
_stopped
->
load
()
)
return
;
std
::
string
clientName
=
ros_bridge
::
com_private
::
_topicSubscriberKey
std
::
string
clientName
=
topicSubscriberKey
+
std
::
string
(
topic
);
+
std
::
string
(
topic
);
auto
it
=
_topicMap
.
find
(
clientName
);
auto
it
=
_topicMap
.
find
(
clientName
);
if
(
it
!=
_topicMap
.
end
())
{
// Topic already subscribed?
if
(
it
!=
_topicMap
.
end
()
){
// Topic already subscribed?
return
;
return
;
}
}
_topicMap
.
insert
(
std
::
make_pair
(
clientName
,
std
::
string
(
topic
)));
_topicMap
.
insert
(
std
::
make_pair
(
clientName
,
std
::
string
(
topic
)));
// Wrap callback.
// Wrap callback.
auto
callbackWrapper
=
[
callback
](
auto
callbackWrapper
=
std
::
shared_ptr
<
WsClient
::
Connection
>
,
[
callback
](
std
::
shared_ptr
<
WsClient
::
Connection
>
,
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
std
::
shared_ptr
<
WsClient
::
InMessage
>
in_message
)
{
{
// Parse document.
// Parse document.
JsonDoc
docFull
;
JsonDoc
docFull
;
docFull
.
Parse
(
in_message
->
string
().
c_str
());
docFull
.
Parse
(
in_message
->
string
().
c_str
());
if
(
docFull
.
HasParseError
()
)
{
if
(
docFull
.
HasParseError
())
{
std
::
cout
<<
"Json document has parse error: "
std
::
cout
<<
"Json document has parse error: "
<<
in_message
->
string
()
<<
in_message
->
string
()
<<
std
::
endl
;
<<
std
::
endl
;
return
;
return
;
}
else
if
(
!
docFull
.
HasMember
(
"msg"
))
{
}
else
if
(
!
docFull
.
HasMember
(
"msg"
))
{
std
::
cout
<<
"Json document does not contain a message (
\"
msg
\"
): "
std
::
cout
<<
"Json document does not contain a message (
\"
msg
\"
): "
<<
in_message
->
string
()
<<
in_message
->
string
()
<<
std
::
endl
;
<<
std
::
endl
;
return
;
return
;
}
}
// Extract message and call callback.
// Extract message and call callback.
JsonDocUPtr
pDoc
(
new
JsonDoc
());
JsonDocUPtr
pDoc
(
new
JsonDoc
());
pDoc
->
CopyFrom
(
docFull
[
"msg"
].
Move
(),
docFull
.
GetAllocator
());
pDoc
->
CopyFrom
(
docFull
[
"msg"
].
Move
(),
docFull
.
GetAllocator
());
callback
(
std
::
move
(
pDoc
));
callback
(
std
::
move
(
pDoc
));
};
};
if
(
!
_rbc
.
topicAvailable
(
topic
)
){
if
(
!
_rbc
.
topicAvailable
(
topic
))
{
// Wait until topic is available.
// Wait until topic is available.
std
::
cout
<<
"TopicSubscriber: Starting wait thread, "
<<
clientName
<<
std
::
endl
;
std
::
cout
<<
"TopicSubscriber: Starting wait thread, "
<<
clientName
std
::
thread
t
([
this
,
clientName
,
topic
,
callbackWrapper
]{
<<
std
::
endl
;
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]{
std
::
thread
t
([
this
,
clientName
,
topic
,
callbackWrapper
]
{
return
this
->
_stopped
->
load
();
this
->
_rbc
.
waitForTopic
(
topic
,
[
this
]
{
return
this
->
_stopped
->
load
();
});
});
if
(
!
this
->
_stopped
->
load
())
{
if
(
!
this
->
_stopped
->
load
()
){
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
addClient
(
clientName
);
this
->
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
this
->
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
std
::
cout
<<
"TopicSubscriber: wait thread subscription successfull: "
std
::
cout
<<
"TopicSubscriber: wait thread subscription successfull: "
<<
clientName
<<
std
::
endl
;
<<
clientName
<<
std
::
endl
;
}
}
std
::
cout
<<
"TopicSubscriber: wait thread end, "
<<
clientName
<<
std
::
endl
;
std
::
cout
<<
"TopicSubscriber: wait thread end, "
<<
clientName
});
<<
std
::
endl
;
t
.
detach
();
});
}
else
{
t
.
detach
();
_rbc
.
addClient
(
clientName
);
}
else
{
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
_rbc
.
addClient
(
clientName
);
std
::
cout
<<
"TopicSubscriber: subscription successfull: "
<<
clientName
<<
std
::
endl
;
_rbc
.
subscribe
(
clientName
,
topic
,
callbackWrapper
);
}
std
::
cout
<<
"TopicSubscriber: subscription successfull: "
<<
clientName
<<
std
::
endl
;
}
}
}
src/ui/MainWindowInner.qml
View file @
50929c20
...
@@ -29,6 +29,7 @@ Item {
...
@@ -29,6 +29,7 @@ Item {
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
QGCPalette
{
id
:
qgcPal
;
colorGroupEnabled
:
true
}
property
real
fps
:
0.0
property
var
currentPopUp
:
null
property
var
currentPopUp
:
null
property
real
currentCenterX
:
0
property
real
currentCenterX
:
0
property
var
activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
property
var
activeVehicle
:
QGroundControl
.
multiVehicleManager
.
activeVehicle
...
...
src/ui/MainWindowNative.qml
View file @
50929c20
...
@@ -39,6 +39,7 @@ Window {
...
@@ -39,6 +39,7 @@ Window {
Loader
{
Loader
{
id
:
mainWindowInner
id
:
mainWindowInner
anchors.fill
:
parent
anchors.fill
:
parent
anchors.margins
:
10
source
:
"
MainWindowInner.qml
"
source
:
"
MainWindowInner.qml
"
Connections
{
Connections
{
...
...
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