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
c6e6a519
Commit
c6e6a519
authored
Sep 08, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
project compiles, not tested
parent
cf53d945
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
2521 additions
and
2799 deletions
+2521
-2799
snake.cpp
src/Snake/snake.cpp
+879
-912
snake.h
src/Snake/snake.h
+195
-204
SnakeDataManager.cc
src/Wima/Snake/SnakeDataManager.cc
+495
-494
SnakeDataManager.h
src/Wima/Snake/SnakeDataManager.h
+47
-49
WimaController.cc
src/Wima/WimaController.cc
+625
-779
WimaController.h
src/Wima/WimaController.h
+279
-360
heartbeat.h
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+1
-1
No files found.
src/Snake/snake.cpp
View file @
c6e6a519
...
@@ -2,13 +2,13 @@
...
@@ -2,13 +2,13 @@
#include "snake.h"
#include "snake.h"
#include <mapbox/polylabel.hpp>
#include <mapbox/geometry.hpp>
#include <mapbox/geometry.hpp>
#include <mapbox/polylabel.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "clipper/clipper.hpp"
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000
#define CLIPPER_SCALE 10000
...
@@ -34,467 +34,434 @@ namespace snake {
...
@@ -34,467 +34,434 @@ namespace snake {
// Geometry stuff.
// Geometry stuff.
//=========================================================================
//=========================================================================
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
)
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
)
{
{
using
namespace
mapbox
;
using
namespace
mapbox
;
if
(
polygon
.
outer
().
empty
())
if
(
polygon
.
outer
().
empty
())
return
;
return
;
geometry
::
polygon
<
double
>
p
;
geometry
::
polygon
<
double
>
p
;
geometry
::
linear_ring
<
double
>
lr1
;
geometry
::
linear_ring
<
double
>
lr1
;
for
(
size_t
i
=
0
;
i
<
polygon
.
outer
().
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
<
polygon
.
outer
().
size
();
++
i
)
{
geometry
::
point
<
double
>
vertex
(
polygon
.
outer
()[
i
].
get
<
0
>
(),
polygon
.
outer
()[
i
].
get
<
1
>
());
geometry
::
point
<
double
>
vertex
(
polygon
.
outer
()[
i
].
get
<
0
>
(),
polygon
.
outer
()[
i
].
get
<
1
>
());
lr1
.
push_back
(
vertex
);
lr1
.
push_back
(
vertex
);
}
}
p
.
push_back
(
lr1
);
p
.
push_back
(
lr1
);
geometry
::
point
<
double
>
c
=
polylabel
(
p
);
geometry
::
point
<
double
>
c
=
polylabel
(
p
);
center
.
set
<
0
>
(
c
.
x
);
center
.
set
<
0
>
(
c
.
x
);
center
.
set
<
1
>
(
c
.
y
);
center
.
set
<
1
>
(
c
.
y
);
}
}
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
)
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
)
{
{
/*
/*
Find the minimum-area bounding box of a set of 2D points
Find the minimum-area bounding box of a set of 2D points
The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates.
The first and last points points must be the same, making a closed polygon.
The first and last points points must be the same, making a closed polygon.
This program finds the rotation angles of each edge of the convex polygon,
This program finds the rotation angles of each edge of the convex polygon,
then tests the area of a bounding box aligned with the unique angles in
then tests the area of a bounding box aligned with the unique angles in
90 degrees of the 1st Quadrant.
90 degrees of the 1st Quadrant.
Returns the
Returns the
Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version)
Results verified using Matlab
Results verified using Matlab
Copyright (c) 2013, David Butterworth, University of Queensland
Copyright (c) 2013, David Butterworth, University of Queensland
All rights reserved.
All rights reserved.
Redistribution and use in source and binary forms, with or without
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
documentation and/or other materials provided with the distribution.
* Neither the name of the Willow Garage, Inc. nor the names of its
* Neither the name of the Willow Garage, Inc. nor the names of its
contributors may be used to endorse or promote products derived from
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
POSSIBILITY OF SUCH DAMAGE.
*/
*/
if
(
polygon
.
outer
().
empty
())
if
(
polygon
.
outer
().
empty
())
return
;
return
;
BoostPolygon
convex_hull
;
BoostPolygon
convex_hull
;
bg
::
convex_hull
(
polygon
,
convex_hull
);
bg
::
convex_hull
(
polygon
,
convex_hull
);
// cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
//cout << "Convex hull: " << bg::wkt<BoostPolygon2D>(convex_hull) << endl;
//# 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
();
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
);
double
edge_x
=
p2
.
get
<
0
>
()
-
p1
.
get
<
0
>
();
double
edge_x
=
p2
.
get
<
0
>
()
-
p1
.
get
<
0
>
();
double
edge_y
=
p2
.
get
<
1
>
()
-
p1
.
get
<
1
>
();
double
edge_y
=
p2
.
get
<
1
>
()
-
p1
.
get
<
1
>
();
edges
.
push_back
(
BoostPoint
{
edge_x
,
edge_y
});
edges
.
push_back
(
BoostPoint
{
edge_x
,
edge_y
});
}
}
// cout << "Edges: ";
// cout << "Edges: ";
// for (auto e : edges)
// for (auto e : edges)
// cout << e.get<0>() << " " << e.get<1>() << ",";
// cout << e.get<0>() << " " << e.get<1>() << ",";
// cout << endl;
// cout << endl;
// Calculate unique edge angles atan2(y/x)
// Calculate unique edge angles atan2(y/x)
double
angle_scale
=
1e3
;
double
angle_scale
=
1e3
;
std
::
set
<
long
>
angles_long
;
std
::
set
<
long
>
angles_long
;
for
(
auto
vertex
:
edges
)
{
for
(
auto
vertex
:
edges
)
{
double
angle
=
std
::
fmod
(
atan2
(
vertex
.
get
<
1
>
(),
vertex
.
get
<
0
>
()),
M_PI
/
2
);
double
angle
=
std
::
fmod
(
atan2
(
vertex
.
get
<
1
>
(),
vertex
.
get
<
0
>
()),
M_PI
/
2
);
angle
=
angle
=
angle
<
0
?
angle
+
M_PI
/
2
:
angle
;
// want strictly positive answers
angle
<
0
?
angle
+
M_PI
/
2
:
angle
;
// want strictly positive answers
angles_long
.
insert
(
long
(
round
(
angle
*
angle_scale
)));
angles_long
.
insert
(
long
(
round
(
angle
*
angle_scale
)));
}
}
std
::
vector
<
double
>
edge_angles
;
std
::
vector
<
double
>
edge_angles
;
for
(
auto
a
:
angles_long
)
for
(
auto
a
:
angles_long
)
edge_angles
.
push_back
(
double
(
a
)
/
angle_scale
);
edge_angles
.
push_back
(
double
(
a
)
/
angle_scale
);
// cout << "Unique angles: ";
// cout << "Unique angles: ";
// for (auto e : edge_angles)
// for (auto e : edge_angles)
// cout << e*180/M_PI << ",";
// cout << e*180/M_PI << ",";
// cout << endl;
// cout << endl;
double
min_area
=
std
::
numeric_limits
<
double
>::
infinity
();
double
min_area
=
std
::
numeric_limits
<
double
>::
infinity
();
// Test each angle to find bounding box with smallest area
// Test each angle to find bounding box with smallest area
// print "Testing", len(edge_angles), "possible rotations for bounding box...
// print "Testing", len(edge_angles), "possible rotations for bounding box... \n"
// \n"
for
(
double
angle
:
edge_angles
){
for
(
double
angle
:
edge_angles
)
{
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
angle
*
180
/
M_PI
);
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
angle
*
180
/
BoostPolygon
hull_rotated
;
M_PI
);
bg
::
transform
(
convex_hull
,
hull_rotated
,
rotate
);
BoostPolygon
hull_rotated
;
//cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
bg
::
transform
(
convex_hull
,
hull_rotated
,
rotate
);
// cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated)
bg
::
model
::
box
<
BoostPoint
>
box
;
// << endl;
bg
::
envelope
(
hull_rotated
,
box
);
// cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
bg
::
model
::
box
<
BoostPoint
>
box
;
bg
::
envelope
(
hull_rotated
,
box
);
//# print "Rotated hull points are \n", rot_points
// cout << "Bounding box: " <<
BoostPoint
min_corner
=
box
.
min_corner
();
// bg::wkt<bg::model::box<BoostPoint2D>>(box) << endl;
BoostPoint
max_corner
=
box
.
max_corner
();
double
min_x
=
min_corner
.
get
<
0
>
();
//# print "Rotated hull points are \n", rot_points
double
max_x
=
max_corner
.
get
<
0
>
();
BoostPoint
min_corner
=
box
.
min_corner
();
double
min_y
=
min_corner
.
get
<
1
>
();
BoostPoint
max_corner
=
box
.
max_corner
();
double
max_y
=
max_corner
.
get
<
1
>
();
double
min_x
=
min_corner
.
get
<
0
>
();
// cout << "min_x: " << min_x << endl;
double
max_x
=
max_corner
.
get
<
0
>
();
// cout << "max_x: " << max_x << endl;
double
min_y
=
min_corner
.
get
<
1
>
();
// cout << "min_y: " << min_y << endl;
double
max_y
=
max_corner
.
get
<
1
>
();
// cout << "max_y: " << max_y << endl;
// cout << "min_x: " << min_x << endl;
// cout << "max_x: " << max_x << endl;
// Calculate height/width/area of this bounding rectangle
// cout << "min_y: " << min_y << endl;
double
width
=
max_x
-
min_x
;
// cout << "max_y: " << max_y << endl;
double
height
=
max_y
-
min_y
;
double
area
=
width
*
height
;
// Calculate height/width/area of this bounding rectangle
// cout << "Width: " << width << endl;
double
width
=
max_x
-
min_x
;
// cout << "Height: " << height << endl;
double
height
=
max_y
-
min_y
;
// cout << "area: " << area << endl;
double
area
=
width
*
height
;
// cout << "angle: " << angle*180/M_PI << endl;
// cout << "Width: " << width << endl;
// cout << "Height: " << height << endl;
// Store the smallest rect found first (a simple convex hull might have 2 answers with same area)
// cout << "area: " << area << endl;
if
(
area
<
min_area
){
// cout << "angle: " << angle*180/M_PI << endl;
min_area
=
area
;
minBBox
.
angle
=
angle
;
// Store the smallest rect found first (a simple convex hull might have 2
minBBox
.
width
=
width
;
// answers with same area)
minBBox
.
height
=
height
;
if
(
area
<
min_area
)
{
min_area
=
area
;
minBBox
.
corners
.
clear
();
minBBox
.
angle
=
angle
;
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
min_y
});
minBBox
.
width
=
width
;
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
max_y
});
minBBox
.
height
=
height
;
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
max_x
,
max_y
});
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
max_x
,
min_y
});
minBBox
.
corners
.
clear
();
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
min_y
});
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
min_y
});
}
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
max_y
});
//cout << endl << endl;
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
max_x
,
max_y
});
}
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
max_x
,
min_y
});
minBBox
.
corners
.
outer
().
push_back
(
BoostPoint
{
min_x
,
min_y
});
}
// Transform corners of minimal bounding box.
// cout << endl << endl;
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
-
minBBox
.
angle
*
180
/
M_PI
);
}
BoostPolygon
rotated_polygon
;
bg
::
transform
(
minBBox
.
corners
,
rotated_polygon
,
rotate
);
// Transform corners of minimal bounding box.
minBBox
.
corners
=
rotated_polygon
;
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate
(
-
minBBox
.
angle
*
180
/
M_PI
);
BoostPolygon
rotated_polygon
;
bg
::
transform
(
minBBox
.
corners
,
rotated_polygon
,
rotate
);
minBBox
.
corners
=
rotated_polygon
;
}
}
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
double
offset
)
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
{
double
offset
)
{
bg
::
strategy
::
buffer
::
distance_symmetric
<
double
>
distance_strategy
(
offset
);
bg
::
strategy
::
buffer
::
distance_symmetric
<
double
>
distance_strategy
(
offset
);
bg
::
strategy
::
buffer
::
join_miter
join_strategy
(
3
);
bg
::
strategy
::
buffer
::
join_miter
join_strategy
(
3
);
bg
::
strategy
::
buffer
::
end_flat
end_strategy
;
bg
::
strategy
::
buffer
::
end_flat
end_strategy
;
bg
::
strategy
::
buffer
::
point_square
point_strategy
;
bg
::
strategy
::
buffer
::
point_square
point_strategy
;
bg
::
strategy
::
buffer
::
side_straight
side_strategy
;
bg
::
strategy
::
buffer
::
side_straight
side_strategy
;
bg
::
model
::
multi_polygon
<
BoostPolygon
>
result
;
bg
::
model
::
multi_polygon
<
BoostPolygon
>
result
;
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
];
if
(
result
.
size
()
>
0
)
polygonOffset
=
result
[
0
];
}
}
void
graphFromPolygon
(
const
BoostPolygon
&
polygon
,
const
BoostLineString
&
vertices
,
Matrix
<
double
>
&
graph
)
{
size_t
n
=
graph
.
getN
();
void
graphFromPolygon
(
const
BoostPolygon
&
polygon
,
const
BoostLineString
&
vertices
,
Matrix
<
double
>
&
graph
)
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
{
BoostPoint
v1
=
vertices
[
i
];
size_t
n
=
graph
.
getN
();
for
(
size_t
j
=
i
+
1
;
j
<
n
;
++
j
)
{
BoostPoint
v2
=
vertices
[
j
];
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
BoostLineString
path
{
v1
,
v2
};
BoostPoint
v1
=
vertices
[
i
];
for
(
size_t
j
=
i
+
1
;
j
<
n
;
++
j
){
BoostPoint
v2
=
vertices
[
j
];
BoostLineString
path
{
v1
,
v2
};
double
distance
=
0
;
double
distance
=
0
;
if
(
!
bg
::
within
(
path
,
polygon
))
if
(
!
bg
::
within
(
path
,
polygon
))
distance
=
std
::
numeric_limits
<
double
>::
infinity
();
distance
=
std
::
numeric_limits
<
double
>::
infinity
();
else
else
distance
=
bg
::
length
(
path
);
distance
=
bg
::
length
(
path
);
graph
.
set
(
i
,
j
,
distance
);
graph
.
set
(
j
,
i
,
distance
);
}
}
graph
.
set
(
i
,
j
,
distance
);
graph
.
set
(
j
,
i
,
distance
);
}
}
}
}
bool
dijkstraAlgorithm
(
const
size_t
numElements
,
bool
dijkstraAlgorithm
(
size_t
startIndex
,
const
size_t
numElements
,
size_t
startIndex
,
size_t
endIndex
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
elementPath
,
std
::
vector
<
size_t
>
&
elementPath
,
std
::
function
<
double
(
const
size_t
,
const
size_t
)
>
distanceDij
)
{
std
::
function
<
double
(
const
size_t
,
const
size_t
)
>
distanceDij
)
if
(
startIndex
>=
numElements
||
endIndex
>=
numElements
||
{
endIndex
==
startIndex
)
{
if
(
startIndex
>=
numElements
return
false
;
||
endIndex
>=
numElements
}
||
endIndex
==
startIndex
)
{
// Node struct
return
false
;
// predecessorIndex is the index of the predecessor node
}
// (nodeList[predecessorIndex]) distance is the distance between the node and
// Node struct
// the start node node number is stored by the position in nodeList
// predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
struct
Node
{
// distance is the distance between the node and the start node
int
predecessorIndex
=
-
1
;
// node number is stored by the position in nodeList
double
distance
=
std
::
numeric_limits
<
double
>::
infinity
();
struct
Node
{
};
int
predecessorIndex
=
-
1
;
double
distance
=
std
::
numeric_limits
<
double
>::
infinity
();
// The list with all Nodes (elements)
};
std
::
vector
<
Node
>
nodeList
(
numElements
);
// This list will be initalized with indices referring to the elements of
// The list with all Nodes (elements)
// nodeList. Elements will be successively remove during the execution of the
std
::
vector
<
Node
>
nodeList
(
numElements
);
// Dijkstra Algorithm.
// This list will be initalized with indices referring to the elements of nodeList.
std
::
vector
<
size_t
>
workingSet
(
numElements
);
// Elements will be successively remove during the execution of the Dijkstra Algorithm.
std
::
vector
<
size_t
>
workingSet
(
numElements
);
// append elements to node list
for
(
size_t
i
=
0
;
i
<
numElements
;
++
i
)
//append elements to node list
workingSet
[
i
]
=
i
;
for
(
size_t
i
=
0
;
i
<
numElements
;
++
i
)
workingSet
[
i
]
=
i
;
nodeList
[
startIndex
].
distance
=
0
;
nodeList
[
startIndex
].
distance
=
0
;
// Dijkstra Algorithm
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
// Dijkstra Algorithm
while
(
workingSet
.
size
()
>
0
)
{
// https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
// serach Node with minimal distance
while
(
workingSet
.
size
()
>
0
)
{
double
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
// serach Node with minimal distance
int
minDistIndex_WS
=
-
1
;
// WS = workinSet
double
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
for
(
size_t
i
=
0
;
i
<
workingSet
.
size
();
++
i
)
{
int
minDistIndex_WS
=
-
1
;
// WS = workinSet
const
int
nodeIndex
=
workingSet
.
at
(
i
);
for
(
size_t
i
=
0
;
i
<
workingSet
.
size
();
++
i
)
{
const
double
dist
=
nodeList
.
at
(
nodeIndex
).
distance
;
const
int
nodeIndex
=
workingSet
.
at
(
i
);
if
(
dist
<
minDist
)
{
const
double
dist
=
nodeList
.
at
(
nodeIndex
).
distance
;
minDist
=
dist
;
if
(
dist
<
minDist
)
{
minDistIndex_WS
=
i
;
minDist
=
dist
;
}
minDistIndex_WS
=
i
;
}
}
if
(
minDistIndex_WS
==
-
1
)
}
return
false
;
if
(
minDistIndex_WS
==
-
1
)
return
false
;
size_t
indexU_NL
=
workingSet
.
at
(
minDistIndex_WS
);
// NL = nodeList
workingSet
.
erase
(
workingSet
.
begin
()
+
minDistIndex_WS
);
size_t
indexU_NL
=
workingSet
.
at
(
minDistIndex_WS
);
// NL = nodeList
if
(
indexU_NL
==
endIndex
)
// shortest path found
workingSet
.
erase
(
workingSet
.
begin
()
+
minDistIndex_WS
);
break
;
if
(
indexU_NL
==
endIndex
)
// shortest path found
break
;
const
double
distanceU
=
nodeList
.
at
(
indexU_NL
).
distance
;
// update distance
const
double
distanceU
=
nodeList
.
at
(
indexU_NL
).
distance
;
for
(
size_t
i
=
0
;
i
<
workingSet
.
size
();
++
i
)
{
//update distance
int
indexV_NL
=
workingSet
[
i
];
// NL = nodeList
for
(
size_t
i
=
0
;
i
<
workingSet
.
size
();
++
i
)
{
Node
*
v
=
&
nodeList
[
indexV_NL
];
int
indexV_NL
=
workingSet
[
i
];
// NL = nodeList
double
dist
=
distanceDij
(
indexU_NL
,
indexV_NL
);
Node
*
v
=
&
nodeList
[
indexV_NL
];
// is ther an alternative path which is shorter?
double
dist
=
distanceDij
(
indexU_NL
,
indexV_NL
);
double
alternative
=
distanceU
+
dist
;
// is ther an alternative path which is shorter?
if
(
alternative
<
v
->
distance
)
{
double
alternative
=
distanceU
+
dist
;
v
->
distance
=
alternative
;
if
(
alternative
<
v
->
distance
)
{
v
->
predecessorIndex
=
indexU_NL
;
v
->
distance
=
alternative
;
}
v
->
predecessorIndex
=
indexU_NL
;
}
}
}
}
// end Djikstra Algorithm
}
// reverse assemble path
// end Djikstra Algorithm
int
e
=
endIndex
;
while
(
1
)
{
if
(
e
==
-
1
)
{
// reverse assemble path
if
(
elementPath
[
0
]
==
startIndex
)
// check if starting point was reached
int
e
=
endIndex
;
break
;
while
(
1
)
{
return
false
;
if
(
e
==
-
1
)
{
}
if
(
elementPath
[
0
]
==
startIndex
)
// check if starting point was reached
elementPath
.
insert
(
elementPath
.
begin
(),
e
);
break
;
return
false
;
}
elementPath
.
insert
(
elementPath
.
begin
(),
e
);
//Update Node
e
=
nodeList
[
e
].
predecessorIndex
;
}
return
true
;
}
void
toDistanceMatrix
(
Matrix
<
double
>
&
graph
)
// Update Node
{
e
=
nodeList
[
e
].
predecessorIndex
;
size_t
n
=
graph
.
getN
();
}
return
true
;
auto
distance
=
[
graph
](
size_t
i
,
size_t
j
){
return
graph
.
get
(
i
,
j
);
};
std
::
vector
<
size_t
>
path
;
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
for
(
size_t
j
=
i
+
1
;
j
<
n
;
++
j
){
double
d
=
graph
.
get
(
i
,
j
);
if
(
!
std
::
isinf
(
d
))
continue
;
path
.
clear
();
bool
ret
=
dijkstraAlgorithm
(
n
,
i
,
j
,
path
,
distance
);
assert
(
ret
);
(
void
)
ret
;
// cout << "(" << i << "," << j << ") d: " << d << endl;
// cout << "Path size: " << path.size() << endl;
// for (auto idx : path)
// cout << idx << " ";
// cout << endl;
d
=
0
;
for
(
long
k
=
0
;
k
<
long
(
path
.
size
())
-
1
;
++
k
)
{
size_t
idx0
=
path
[
k
];
size_t
idx1
=
path
[
k
+
1
];
double
d0
=
graph
.
get
(
idx0
,
idx1
);
assert
(
std
::
isinf
(
d0
)
==
false
);
d
+=
d0
;
}
graph
.
set
(
i
,
j
,
d
);
graph
.
set
(
j
,
i
,
d
);
}
}
}
}
void
shortestPathFromGraph
(
const
Matrix
<
double
>
&
graph
,
size_t
startIndex
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
pathIdx
)
void
toDistanceMatrix
(
Matrix
<
double
>
&
graph
)
{
{
size_t
n
=
graph
.
getN
();
if
(
!
std
::
isinf
(
graph
.
get
(
startIndex
,
endIndex
))){
auto
distance
=
[
graph
](
size_t
i
,
size_t
j
)
{
return
graph
.
get
(
i
,
j
);
};
pathIdx
.
push_back
(
startIndex
);
pathIdx
.
push_back
(
endIndex
);
std
::
vector
<
size_t
>
path
;
}
else
{
for
(
size_t
i
=
0
;
i
<
n
;
++
i
)
{
auto
distance
=
[
graph
](
size_t
i
,
size_t
j
){
for
(
size_t
j
=
i
+
1
;
j
<
n
;
++
j
)
{
return
graph
.
get
(
i
,
j
);
double
d
=
graph
.
get
(
i
,
j
);
};
if
(
!
std
::
isinf
(
d
))
bool
ret
=
dijkstraAlgorithm
(
graph
.
getN
(),
startIndex
,
endIndex
,
pathIdx
,
distance
);
continue
;
assert
(
ret
);
path
.
clear
();
(
void
)
ret
;
bool
ret
=
dijkstraAlgorithm
(
n
,
i
,
j
,
path
,
distance
);
}
assert
(
ret
);
(
void
)
ret
;
//=========================================================================
// cout << "(" << i << "," << j << ") d: " << d << endl;
// Scenario calculation.
// cout << "Path size: " << path.size() << endl;
//=========================================================================
// for (auto idx : path)
}
Scenario
::
Scenario
()
:
// cout << idx << " ";
_tileWidth
(
5
*
bu
::
si
::
meter
)
// cout << endl;
,
_tileHeight
(
5
*
bu
::
si
::
meter
)
,
_minTileArea
(
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
d
=
0
;
,
_needsUpdate
(
true
)
for
(
long
k
=
0
;
k
<
long
(
path
.
size
())
-
1
;
++
k
)
{
{
size_t
idx0
=
path
[
k
];
size_t
idx1
=
path
[
k
+
1
];
double
d0
=
graph
.
get
(
idx0
,
idx1
);
assert
(
std
::
isinf
(
d0
)
==
false
);
d
+=
d0
;
}
graph
.
set
(
i
,
j
,
d
);
graph
.
set
(
j
,
i
,
d
);
}
}
}
void
shortestPathFromGraph
(
const
Matrix
<
double
>
&
graph
,
size_t
startIndex
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
pathIdx
)
{
if
(
!
std
::
isinf
(
graph
.
get
(
startIndex
,
endIndex
)))
{
pathIdx
.
push_back
(
startIndex
);
pathIdx
.
push_back
(
endIndex
);
}
else
{
auto
distance
=
[
graph
](
size_t
i
,
size_t
j
)
{
return
graph
.
get
(
i
,
j
);
};
bool
ret
=
dijkstraAlgorithm
(
graph
.
getN
(),
startIndex
,
endIndex
,
pathIdx
,
distance
);
assert
(
ret
);
(
void
)
ret
;
}
//=========================================================================
// Scenario calculation.
//=========================================================================
}
}
Scenario
::
Scenario
()
:
_tileWidth
(
5
*
bu
::
si
::
meter
),
_tileHeight
(
5
*
bu
::
si
::
meter
),
_minTileArea
(
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
),
_needsUpdate
(
true
)
{}
void
Scenario
::
setMeasurementArea
(
const
BoostPolygon
&
area
)
void
Scenario
::
setMeasurementArea
(
const
BoostPolygon
&
area
)
{
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_mArea
=
area
;
_mArea
=
area
;
}
}
void
Scenario
::
setServiceArea
(
const
BoostPolygon
&
area
)
void
Scenario
::
setServiceArea
(
const
BoostPolygon
&
area
)
{
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_sArea
=
area
;
_sArea
=
area
;
}
}
void
Scenario
::
setCorridor
(
const
BoostPolygon
&
area
)
void
Scenario
::
setCorridor
(
const
BoostPolygon
&
area
)
{
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_corridor
=
area
;
_corridor
=
area
;
}
}
BoostPolygon
&
Scenario
::
measurementArea
()
{
BoostPolygon
&
Scenario
::
measurementArea
()
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
return
_mArea
;
return
_mArea
;
}
}
BoostPolygon
&
Scenario
::
serviceArea
()
{
BoostPolygon
&
Scenario
::
serviceArea
()
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
return
_sArea
;
return
_sArea
;
}
}
BoostPolygon
&
Scenario
::
corridor
()
{
BoostPolygon
&
Scenario
::
corridor
()
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
return
_corridor
;
return
_corridor
;
}
}
const
BoundingBox
&
Scenario
::
mAreaBoundingBox
()
const
const
BoundingBox
&
Scenario
::
mAreaBoundingBox
()
const
{
{
return
_mAreaBoundingBox
;
return
_mAreaBoundingBox
;
}
}
const
BoostPolygon
&
Scenario
::
measurementArea
()
const
const
BoostPolygon
&
Scenario
::
measurementArea
()
const
{
return
_mArea
;
}
{
return
_mArea
;
}
const
BoostPolygon
&
Scenario
::
serviceArea
()
const
const
BoostPolygon
&
Scenario
::
serviceArea
()
const
{
return
_sArea
;
}
{
return
_sArea
;
}
const
BoostPolygon
&
Scenario
::
corridor
()
const
const
BoostPolygon
&
Scenario
::
corridor
()
const
{
return
_corridor
;
}
{
return
_corridor
;
}
const
BoostPolygon
&
Scenario
::
joinedArea
()
const
{
const
BoostPolygon
&
Scenario
::
joinedArea
()
const
{
return
_jArea
;
}
return
_jArea
;
}
const
vector
<
BoostPolygon
>
&
Scenario
::
tiles
()
const
{
const
vector
<
BoostPolygon
>
&
Scenario
::
tiles
()
const
{
return
_tiles
;
}
return
_tiles
;
}
const
BoostLineString
&
Scenario
::
tileCenterPoints
()
const
{
const
BoostLineString
&
Scenario
::
tileCenterPoints
()
const
{
return
_tileCenterPoints
;
return
_tileCenterPoints
;
}
}
const
BoundingBox
&
Scenario
::
measurementAreaBBox
()
const
{
const
BoundingBox
&
Scenario
::
measurementAreaBBox
()
const
{
return
_mAreaBoundingBox
;
return
_mAreaBoundingBox
;
}
}
const
BoostPoint
&
Scenario
::
homePositon
()
const
{
const
BoostPoint
&
Scenario
::
homePositon
()
const
{
return
_homePosition
;
}
return
_homePosition
;
}
bool
Scenario
::
update
()
bool
Scenario
::
update
()
{
{
if
(
!
_needsUpdate
)
if
(
!
_needsUpdate
)
return
true
;
if
(
!
_calculateBoundingBox
())
return
false
;
if
(
!
_calculateTiles
())
return
false
;
if
(
!
_calculateJoinedArea
())
return
false
;
_needsUpdate
=
false
;
return
true
;
return
true
;
if
(
!
_calculateBoundingBox
())
return
false
;
if
(
!
_calculateTiles
())
return
false
;
if
(
!
_calculateJoinedArea
())
return
false
;
_needsUpdate
=
false
;
return
true
;
}
}
bool
Scenario
::
_calculateBoundingBox
()
bool
Scenario
::
_calculateBoundingBox
()
{
{
minimalBoundingBox
(
_mArea
,
_mAreaBoundingBox
);
minimalBoundingBox
(
_mArea
,
_mAreaBoundingBox
);
return
true
;
return
true
;
}
}
/**
/**
* Devides the (measurement area) bounding box into tiles and clips it to the measurement area.
* Devides the (measurement area) bounding box into tiles and clips it to the
* measurement area.
*
*
* Devides the (measurement area) bounding box into tiles of width \p tileWidth and height \p tileHeight.
* Devides the (measurement area) bounding box into tiles of width \p tileWidth
* Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
* and height \p tileHeight. Clips the resulting tiles to the measurement area.
* The function assumes that \a _mArea and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
* Tiles are rejected, if their area is smaller than \p minTileArea. The
* Scenario::_calculateBoundingBox().
* function assumes that \a _mArea and \a _mAreaBoundingBox have correct values.
* \see \ref Scenario::_areas2enu() and \ref Scenario::_calculateBoundingBox().
*
*
* @param tileWidth The width (>0) of a tile.
* @param tileWidth The width (>0) of a tile.
* @param tileHeight The heigth (>0) of a tile.
* @param tileHeight The heigth (>0) of a tile.
...
@@ -502,585 +469,585 @@ bool Scenario::_calculateBoundingBox()
...
@@ -502,585 +469,585 @@ bool Scenario::_calculateBoundingBox()
*
*
* @return Returns true if successful.
* @return Returns true if successful.
*/
*/
bool
Scenario
::
_calculateTiles
()
bool
Scenario
::
_calculateTiles
()
{
{
_tiles
.
clear
();
_tiles
.
clear
();
_tileCenterPoints
.
clear
();
_tileCenterPoints
.
clear
();
if
(
_tileWidth
<=
0
*
bu
::
si
::
meter
||
_tileHeight
<=
0
*
bu
::
si
::
meter
||
if
(
_tileWidth
<=
0
*
bu
::
si
::
meter
_minTileArea
<=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
{
||
_tileHeight
<=
0
*
bu
::
si
::
meter
errorString
=
||
_minTileArea
<=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
"Parameters tileWidth, tileHeight, minTileArea must be positive."
;
)
return
false
;
{
}
errorString
=
"Parameters tileWidth, tileHeight, minTileArea must be positive."
;
return
false
;
double
bboxWidth
=
_mAreaBoundingBox
.
width
;
}
double
bboxHeight
=
_mAreaBoundingBox
.
height
;
double
bboxWidth
=
_mAreaBoundingBox
.
width
;
BoostPoint
origin
=
_mAreaBoundingBox
.
corners
.
outer
()[
0
];
double
bboxHeight
=
_mAreaBoundingBox
.
height
;
// cout << "Origin: " << origin[0] << " " << origin[1] << endl;
BoostPoint
origin
=
_mAreaBoundingBox
.
corners
.
outer
()[
0
];
// Transform _mArea polygon to bounding box coordinate system.
trans
::
rotate_transformer
<
boost
::
geometry
::
degree
,
double
,
2
,
2
>
rotate
(
//cout << "Origin: " << origin[0] << " " << origin[1] << endl;
_mAreaBoundingBox
.
angle
*
180
/
M_PI
);
// Transform _mArea polygon to bounding box coordinate system.
trans
::
translate_transformer
<
double
,
2
,
2
>
translate
(
-
origin
.
get
<
0
>
(),
trans
::
rotate_transformer
<
boost
::
geometry
::
degree
,
double
,
2
,
2
>
rotate
(
_mAreaBoundingBox
.
angle
*
180
/
M_PI
);
-
origin
.
get
<
1
>
());
trans
::
translate_transformer
<
double
,
2
,
2
>
translate
(
-
origin
.
get
<
0
>
(),
-
origin
.
get
<
1
>
());
BoostPolygon
translated_polygon
;
BoostPolygon
translated_polygon
;
BoostPolygon
rotated_polygon
;
BoostPolygon
rotated_polygon
;
boost
::
geometry
::
transform
(
_mArea
,
translated_polygon
,
translate
);
boost
::
geometry
::
transform
(
_mArea
,
translated_polygon
,
translate
);
boost
::
geometry
::
transform
(
translated_polygon
,
rotated_polygon
,
rotate
);
boost
::
geometry
::
transform
(
translated_polygon
,
rotated_polygon
,
rotate
);
bg
::
correct
(
rotated_polygon
);
bg
::
correct
(
rotated_polygon
);
// cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
//cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t
iMax
=
ceil
(
bboxWidth
/
_tileWidth
.
value
());
size_t
iMax
=
ceil
(
bboxWidth
/
_tileWidth
.
value
());
size_t
jMax
=
ceil
(
bboxHeight
/
_tileHeight
.
value
());
size_t
jMax
=
ceil
(
bboxHeight
/
_tileHeight
.
value
());
if
(
iMax
<
1
||
jMax
<
1
)
{
if
(
iMax
<
1
||
jMax
<
1
)
{
std
::
stringstream
ss
;
std
::
stringstream
ss
;
ss
<<
"Tile width or Tile height to large. "
ss
<<
"Tile width or Tile height to large. "
<<
"tile width = "
<<
_tileWidth
<<
", "
<<
"tile width = "
<<
_tileWidth
<<
", "
<<
"tile height = "
<<
_tileHeight
<<
"."
<<
std
::
endl
;
<<
"tile height = "
<<
_tileHeight
<<
"."
<<
std
::
endl
;
errorString
=
ss
.
str
();
errorString
=
ss
.
str
();
return
false
;
return
false
;
}
}
trans
::
rotate_transformer
<
boost
::
geometry
::
degree
,
double
,
2
,
2
>
rotate_back
(
-
_mAreaBoundingBox
.
angle
*
180
/
M_PI
);
trans
::
rotate_transformer
<
boost
::
geometry
::
degree
,
double
,
2
,
2
>
rotate_back
(
-
_mAreaBoundingBox
.
angle
*
180
/
M_PI
);
trans
::
translate_transformer
<
double
,
2
,
2
>
translate_back
(
origin
.
get
<
0
>
(),
trans
::
translate_transformer
<
double
,
2
,
2
>
translate_back
(
origin
.
get
<
0
>
(),
origin
.
get
<
1
>
());
origin
.
get
<
1
>
());
for
(
size_t
i
=
0
;
i
<
iMax
;
++
i
){
for
(
size_t
i
=
0
;
i
<
iMax
;
++
i
)
{
double
x_min
=
_tileWidth
.
value
()
*
i
;
double
x_min
=
_tileWidth
.
value
()
*
i
;
double
x_max
=
x_min
+
_tileWidth
.
value
();
double
x_max
=
x_min
+
_tileWidth
.
value
();
for
(
size_t
j
=
0
;
j
<
jMax
;
++
j
){
for
(
size_t
j
=
0
;
j
<
jMax
;
++
j
)
{
double
y_min
=
_tileHeight
.
value
()
*
j
;
double
y_min
=
_tileHeight
.
value
()
*
j
;
double
y_max
=
y_min
+
_tileHeight
.
value
();
double
y_max
=
y_min
+
_tileHeight
.
value
();
BoostPolygon
tile_unclipped
;
BoostPolygon
tile_unclipped
;
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_min
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_min
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_max
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_max
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_max
,
y_max
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_max
,
y_max
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_max
,
y_min
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_max
,
y_min
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_min
});
tile_unclipped
.
outer
().
push_back
(
BoostPoint
{
x_min
,
y_min
});
std
::
deque
<
BoostPolygon
>
boost_tiles
;
std
::
deque
<
BoostPolygon
>
boost_tiles
;
if
(
!
boost
::
geometry
::
intersection
(
tile_unclipped
,
rotated_polygon
,
if
(
!
boost
::
geometry
::
intersection
(
tile_unclipped
,
rotated_polygon
,
boost_tiles
))
boost_tiles
))
continue
;
continue
;
for
(
BoostPolygon
t
:
boost_tiles
)
for
(
BoostPolygon
t
:
boost_tiles
)
{
{
if
(
bg
::
area
(
t
)
>
_minTileArea
.
value
())
{
if
(
bg
::
area
(
t
)
>
_minTileArea
.
value
()){
// Transform boost_tile to world coordinate system.
// Transform boost_tile to world coordinate system.
BoostPolygon
rotated_tile
;
BoostPolygon
rotated_tile
;
BoostPolygon
translated_tile
;
BoostPolygon
translated_tile
;
boost
::
geometry
::
transform
(
t
,
rotated_tile
,
rotate_back
);
boost
::
geometry
::
transform
(
t
,
rotated_tile
,
rotate_back
);
boost
::
geometry
::
transform
(
rotated_tile
,
translated_tile
,
boost
::
geometry
::
transform
(
rotated_tile
,
translated_tile
,
translate_back
);
translate_back
);
// Store tile and calculate center point.
// Store tile and calculate center point.
_tiles
.
push_back
(
translated_tile
);
_tiles
.
push_back
(
translated_tile
);
BoostPoint
tile_center
;
BoostPoint
tile_center
;
polygonCenter
(
translated_tile
,
tile_center
);
polygonCenter
(
translated_tile
,
tile_center
);
_tileCenterPoints
.
push_back
(
tile_center
);
_tileCenterPoints
.
push_back
(
tile_center
);
}
}
}
}
}
}
}
}
if
(
_tiles
.
size
()
<
1
){
if
(
_tiles
.
size
()
<
1
)
{
errorString
=
"No tiles calculated. Is the minTileArea parameter large enough?"
;
errorString
=
return
false
;
"No tiles calculated. Is the minTileArea parameter large enough?"
;
}
return
false
;
}
return
true
;
return
true
;
}
}
bool
Scenario
::
_calculateJoinedArea
()
bool
Scenario
::
_calculateJoinedArea
()
{
{
_jArea
.
clear
();
_jArea
.
clear
();
// Measurement area and service area overlapping?
// Measurement area and service area overlapping?
bool
overlapingSerMeas
=
bg
::
intersects
(
_mArea
,
_sArea
)
?
true
:
false
;
bool
overlapingSerMeas
=
bg
::
intersects
(
_mArea
,
_sArea
)
?
true
:
false
;
bool
corridorValid
=
_corridor
.
outer
().
size
()
>
0
?
true
:
false
;
bool
corridorValid
=
_corridor
.
outer
().
size
()
>
0
?
true
:
false
;
// Check if corridor is connecting measurement area and service area.
bool
corridor_is_connection
=
false
;
// Check if corridor is connecting measurement area and service area.
if
(
corridorValid
)
{
bool
corridor_is_connection
=
false
;
// Corridor overlaping with measurement area?
if
(
corridorValid
)
{
if
(
bg
::
intersects
(
_corridor
,
_mArea
))
{
// Corridor overlaping with measurement area?
// Corridor overlaping with service area?
if
(
bg
::
intersects
(
_corridor
,
_mArea
)
)
{
if
(
bg
::
intersects
(
_corridor
,
_sArea
))
{
// Corridor overlaping with service area?
corridor_is_connection
=
true
;
if
(
bg
::
intersects
(
_corridor
,
_sArea
)
)
{
}
corridor_is_connection
=
true
;
}
}
}
}
}
// Are areas joinable?
std
::
deque
<
BoostPolygon
>
sol
;
// Are areas joinable?
BoostPolygon
partialArea
=
_mArea
;
std
::
deque
<
BoostPolygon
>
sol
;
if
(
overlapingSerMeas
){
BoostPolygon
partialArea
=
_mArea
;
if
(
corridor_is_connection
){
if
(
overlapingSerMeas
)
{
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
if
(
corridor_is_connection
)
{
}
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
}
else
if
(
corridor_is_connection
){
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
}
else
{
errorString
=
"Areas are not overlapping"
;
return
false
;
}
if
(
sol
.
size
()
>
0
)
{
partialArea
=
sol
[
0
];
sol
.
clear
();
}
}
}
else
if
(
corridor_is_connection
)
{
// Join areas.
bg
::
union_
(
partialArea
,
_corridor
,
sol
);
bg
::
union_
(
partialArea
,
_sArea
,
sol
);
}
else
{
errorString
=
"Areas are not overlapping"
;
if
(
sol
.
size
()
>
0
)
{
return
false
;
_jArea
=
sol
[
0
];
}
}
else
{
return
false
;
if
(
sol
.
size
()
>
0
)
{
}
partialArea
=
sol
[
0
];
sol
.
clear
();
return
true
;
}
// Join areas.
bg
::
union_
(
partialArea
,
_sArea
,
sol
);
if
(
sol
.
size
()
>
0
)
{
_jArea
=
sol
[
0
];
}
else
{
return
false
;
}
return
true
;
}
}
Area
Scenario
::
minTileArea
()
const
Area
Scenario
::
minTileArea
()
const
{
return
_minTileArea
;
}
{
return
_minTileArea
;
}
void
Scenario
::
setMinTileArea
(
Area
minTileArea
)
void
Scenario
::
setMinTileArea
(
Area
minTileArea
)
{
{
if
(
minTileArea
>=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
)
{
if
(
minTileArea
>=
0
*
bu
::
si
::
meter
*
bu
::
si
::
meter
){
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_minTileArea
=
minTileArea
;
_minTileArea
=
minTileArea
;
}
}
}
}
Length
Scenario
::
tileHeight
()
const
Length
Scenario
::
tileHeight
()
const
{
return
_tileHeight
;
}
{
return
_tileHeight
;
}
void
Scenario
::
setTileHeight
(
Length
tileHeight
)
void
Scenario
::
setTileHeight
(
Length
tileHeight
)
{
{
if
(
tileHeight
>
0
*
bu
::
si
::
meter
)
{
if
(
tileHeight
>
0
*
bu
::
si
::
meter
)
{
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_tileHeight
=
tileHeight
;
_tileHeight
=
tileHeight
;
}
}
}
}
Length
Scenario
::
tileWidth
()
const
Length
Scenario
::
tileWidth
()
const
{
return
_tileWidth
;
}
{
return
_tileWidth
;
}
void
Scenario
::
setTileWidth
(
Length
tileWidth
)
void
Scenario
::
setTileWidth
(
Length
tileWidth
)
{
{
if
(
tileWidth
>
0
*
bu
::
si
::
meter
)
{
if
(
tileWidth
>
0
*
bu
::
si
::
meter
){
_needsUpdate
=
true
;
_needsUpdate
=
true
;
_tileWidth
=
tileWidth
;
_tileWidth
=
tileWidth
;
}
}
}
}
//=========================================================================
//=========================================================================
// Tile calculation.
// Tile calculation.
//=========================================================================
//=========================================================================
bool
joinAreas
(
const
std
::
vector
<
BoostPolygon
>
&
areas
,
BoostPolygon
&
joinedArea
)
bool
joinAreas
(
const
std
::
vector
<
BoostPolygon
>
&
areas
,
{
BoostPolygon
&
joinedArea
)
{
if
(
areas
.
size
()
<
1
)
if
(
areas
.
size
()
<
1
)
return
false
;
return
false
;
std
::
deque
<
std
::
size_t
>
idxList
;
std
::
deque
<
std
::
size_t
>
idxList
;
for
(
size_t
i
=
1
;
i
<
areas
.
size
();
++
i
)
for
(
size_t
i
=
1
;
i
<
areas
.
size
();
++
i
)
idxList
.
push_back
(
i
);
idxList
.
push_back
(
i
);
joinedArea
=
areas
[
0
];
joinedArea
=
areas
[
0
];
std
::
deque
<
BoostPolygon
>
sol
;
std
::
deque
<
BoostPolygon
>
sol
;
while
(
idxList
.
size
()
>
0
){
while
(
idxList
.
size
()
>
0
)
{
bool
success
=
false
;
bool
success
=
false
;
for
(
auto
it
=
idxList
.
begin
();
it
!=
idxList
.
end
();
++
it
){
for
(
auto
it
=
idxList
.
begin
();
it
!=
idxList
.
end
();
++
it
)
{
bg
::
union_
(
joinedArea
,
areas
[
*
it
],
sol
);
bg
::
union_
(
joinedArea
,
areas
[
*
it
],
sol
);
if
(
sol
.
size
()
>
0
)
{
if
(
sol
.
size
()
>
0
)
{
joinedArea
=
sol
[
0
];
joinedArea
=
sol
[
0
];
sol
.
clear
();
sol
.
clear
();
idxList
.
erase
(
it
);
idxList
.
erase
(
it
);
success
=
true
;
success
=
true
;
break
;
break
;
}
}
}
if
(
!
success
)
return
false
;
}
}
if
(
!
success
)
return
false
;
}
return
true
;
return
true
;
}
}
BoundingBox
::
BoundingBox
()
:
BoundingBox
::
BoundingBox
()
:
width
(
0
),
height
(
0
),
angle
(
0
)
{}
width
(
0
)
,
height
(
0
)
,
angle
(
0
)
{
void
BoundingBox
::
clear
()
{
width
=
0
;
height
=
0
;
angle
=
0
;
corners
.
clear
();
}
}
void
BoundingBox
::
clear
()
bool
flight_plan
::
transectsFromScenario
(
Length
distance
,
Length
minLength
,
{
Angle
angle
,
const
Scenario
&
scenario
,
width
=
0
;
height
=
0
;
angle
=
0
;
corners
.
clear
();
}
bool
flight_plan
::
transectsFromScenario
(
Length
distance
,
Length
minLength
,
Angle
angle
,
const
Scenario
&
scenario
,
const
Progress
&
p
,
const
Progress
&
p
,
flight_plan
::
Transects
&
t
,
flight_plan
::
Transects
&
t
,
string
&
errorString
)
string
&
errorString
)
{
{
// 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
>
();
double
y1
=
box
.
max_corner
().
get
<
1
>
();
double
y1
=
box
.
max_corner
().
get
<
1
>
();
// 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
);
trans
::
rotate_transformer
<
bg
::
degree
,
double
,
2
,
2
>
rotate_back
(
vector
<
ClipperLib
::
Path
>
transectsClipper
;
angle
.
value
()
*
180
/
M_PI
);
transectsClipper
.
reserve
(
num_t
);
vector
<
ClipperLib
::
Path
>
transectsClipper
;
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
transectsClipper
.
reserve
(
num_t
);
// calculate transect
for
(
size_t
i
=
0
;
i
<
num_t
;
++
i
)
{
BoostPoint
v1
{
x0
,
y0
+
i
*
distance
.
value
()};
// calculate transect
BoostPoint
v2
{
x1
,
y0
+
i
*
distance
.
value
()};
BoostPoint
v1
{
x0
,
y0
+
i
*
distance
.
value
()};
BoostLineString
transect
;
BoostPoint
v2
{
x1
,
y0
+
i
*
distance
.
value
()};
transect
.
push_back
(
v1
);
BoostLineString
transect
;
transect
.
push_back
(
v2
);
transect
.
push_back
(
v1
);
transect
.
push_back
(
v2
);
// transform back
BoostLineString
temp_transect
;
// transform back
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
BoostLineString
temp_transect
;
bg
::
transform
(
transect
,
temp_transect
,
rotate_back
);
ClipperLib
::
IntPoint
c1
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
ClipperLib
::
IntPoint
c1
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
0
>
()
*
CLIPPER_SCALE
),
ClipperLib
::
IntPoint
c2
{
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
0
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
ClipperLib
::
IntPoint
c2
{
ClipperLib
::
Path
path
{
c1
,
c2
};
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
0
>
()
*
CLIPPER_SCALE
),
transectsClipper
.
push_back
(
path
);
static_cast
<
ClipperLib
::
cInt
>
(
transect
[
1
].
get
<
1
>
()
*
CLIPPER_SCALE
)};
}
ClipperLib
::
Path
path
{
c1
,
c2
};
transectsClipper
.
push_back
(
path
);
if
(
transectsClipper
.
size
()
==
0
){
}
std
::
stringstream
ss
;
ss
<<
"Not able to generate transects. Parameter: distance = "
<<
distance
<<
std
::
endl
;
if
(
transectsClipper
.
size
()
==
0
)
{
errorString
=
ss
.
str
();
std
::
stringstream
ss
;
return
false
;
ss
<<
"Not able to generate transects. Parameter: distance = "
<<
distance
<<
std
::
endl
;
errorString
=
ss
.
str
();
return
false
;
}
// Convert measurement area to clipper path.
ClipperLib
::
Path
mAreaClipper
;
for
(
auto
vertex
:
mArea
.
outer
())
{
mAreaClipper
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
mAreaClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
transectsClipper
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
clippedTransecs
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
clippedTransecs
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
auto
&
transects
=
clippedTransecs
;
bool
ignoreProgress
=
p
.
size
()
!=
scenario
.
tiles
().
size
();
ClipperLib
::
PolyTree
clippedTransecs2
;
if
(
!
ignoreProgress
)
{
// Calculate processed tiles (_progress[i] == 100) and subtract them from
// measurement area.
size_t
numTiles
=
p
.
size
();
vector
<
BoostPolygon
>
processedTiles
;
const
auto
&
tiles
=
scenario
.
tiles
();
for
(
size_t
i
=
0
;
i
<
numTiles
;
++
i
)
{
if
(
p
[
i
]
==
100
)
{
processedTiles
.
push_back
(
tiles
[
i
]);
}
}
}
// Convert measurement area to clipper path.
if
(
processedTiles
.
size
()
!=
numTiles
)
{
ClipperLib
::
Path
mAreaClipper
;
vector
<
ClipperLib
::
Path
>
processedTilesClipper
;
for
(
auto
vertex
:
mArea
.
outer
()
){
for
(
auto
t
:
processedTiles
)
{
mAreaClipper
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
ClipperLib
::
Path
path
;
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
for
(
auto
vertex
:
t
.
outer
())
{
}
path
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
// Perform clipping.
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
// Clip transects to measurement area.
ClipperLib
::
Clipper
clipper
;
clipper
.
AddPath
(
mAreaClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
AddPaths
(
transectsClipper
,
ClipperLib
::
ptSubject
,
false
);
ClipperLib
::
PolyTree
clippedTransecs
;
clipper
.
Execute
(
ClipperLib
::
ctIntersection
,
clippedTransecs
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
auto
&
transects
=
clippedTransecs
;
bool
ignoreProgress
=
p
.
size
()
!=
scenario
.
tiles
().
size
();
ClipperLib
::
PolyTree
clippedTransecs2
;
if
(
!
ignoreProgress
)
{
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t
numTiles
=
p
.
size
();
vector
<
BoostPolygon
>
processedTiles
;
const
auto
&
tiles
=
scenario
.
tiles
();
for
(
size_t
i
=
0
;
i
<
numTiles
;
++
i
)
{
if
(
p
[
i
]
==
100
){
processedTiles
.
push_back
(
tiles
[
i
]);
}
}
if
(
processedTiles
.
size
()
!=
numTiles
){
vector
<
ClipperLib
::
Path
>
processedTilesClipper
;
for
(
auto
t
:
processedTiles
){
ClipperLib
::
Path
path
;
for
(
auto
vertex
:
t
.
outer
()){
path
.
push_back
(
ClipperLib
::
IntPoint
{
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
0
>
()
*
CLIPPER_SCALE
),
static_cast
<
ClipperLib
::
cInt
>
(
vertex
.
get
<
1
>
()
*
CLIPPER_SCALE
)});
}
processedTilesClipper
.
push_back
(
path
);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper
.
Clear
();
for
(
auto
&
child
:
clippedTransecs
.
Childs
)
clipper
.
AddPath
(
child
->
Contour
,
ClipperLib
::
ptSubject
,
false
);
clipper
.
AddPaths
(
processedTilesClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
Execute
(
ClipperLib
::
ctDifference
,
clippedTransecs2
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
transects
=
clippedTransecs2
;
}
}
processedTilesClipper
.
push_back
(
path
);
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper
.
Clear
();
for
(
auto
&
child
:
clippedTransecs
.
Childs
)
clipper
.
AddPath
(
child
->
Contour
,
ClipperLib
::
ptSubject
,
false
);
clipper
.
AddPaths
(
processedTilesClipper
,
ClipperLib
::
ptClip
,
true
);
clipper
.
Execute
(
ClipperLib
::
ctDifference
,
clippedTransecs2
,
ClipperLib
::
pftNonZero
,
ClipperLib
::
pftNonZero
);
transects
=
clippedTransecs2
;
}
}
}
// Extract transects from PolyTree and convert them to BoostLineString
for
(
auto
&
child
:
transects
.
Childs
){
// Extract transects from PolyTree and convert them to BoostLineString
auto
&
clipperTransect
=
child
->
Contour
;
for
(
auto
&
child
:
transects
.
Childs
)
{
BoostPoint
v1
{
static_cast
<
double
>
(
clipperTransect
[
0
].
X
)
/
CLIPPER_SCALE
,
auto
&
clipperTransect
=
child
->
Contour
;
static_cast
<
double
>
(
clipperTransect
[
0
].
Y
)
/
CLIPPER_SCALE
};
BoostPoint
v1
{
static_cast
<
double
>
(
clipperTransect
[
0
].
X
)
/
CLIPPER_SCALE
,
BoostPoint
v2
{
static_cast
<
double
>
(
clipperTransect
[
1
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
0
].
Y
)
/
CLIPPER_SCALE
};
static_cast
<
double
>
(
clipperTransect
[
1
].
Y
)
/
CLIPPER_SCALE
};
BoostPoint
v2
{
static_cast
<
double
>
(
clipperTransect
[
1
].
X
)
/
CLIPPER_SCALE
,
static_cast
<
double
>
(
clipperTransect
[
1
].
Y
)
/
CLIPPER_SCALE
};
BoostLineString
transect
{
v1
,
v2
};
if
(
bg
::
length
(
transect
)
>=
minLength
.
value
())
BoostLineString
transect
{
v1
,
v2
};
t
.
push_back
(
transect
);
if
(
bg
::
length
(
transect
)
>=
minLength
.
value
())
}
t
.
push_back
(
transect
);
}
if
(
t
.
size
()
==
0
){
std
::
stringstream
ss
;
if
(
t
.
size
()
==
0
)
{
ss
<<
"Not able to generate transects. Parameter: minLength = "
<<
minLength
<<
std
::
endl
;
std
::
stringstream
ss
;
errorString
=
ss
.
str
();
ss
<<
"Not able to generate transects. Parameter: minLength = "
<<
minLength
return
false
;
<<
std
::
endl
;
}
errorString
=
ss
.
str
();
return
true
;
return
false
;
}
return
true
;
}
}
struct
RoutingDataModel
{
Matrix
<
int64_t
>
distanceMatrix
;
struct
RoutingDataModel
{
long
numVehicles
;
Matrix
<
int64_t
>
distanceMatrix
;
RoutingIndexManager
::
NodeIndex
depot
;
long
numVehicles
;
RoutingIndexManager
::
NodeIndex
depot
;
};
};
void
generateRoutingModel
(
const
BoostLineString
&
vertices
,
void
generateRoutingModel
(
const
BoostLineString
&
vertices
,
const
BoostPolygon
&
polygonOffset
,
const
BoostPolygon
&
polygonOffset
,
size_t
n0
,
size_t
n0
,
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
)
{
RoutingDataModel
&
dataModel
,
Matrix
<
double
>
&
graph
){
#ifdef SHOW_TIME
#ifdef SHOW_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 SHOW_TIME
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
auto
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time graphFromPolygon(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
#endif
Matrix
<
double
>
distanceMatrix
(
graph
);
Matrix
<
double
>
distanceMatrix
(
graph
);
#ifdef SHOW_TIME
#ifdef SHOW_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 SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time toDistanceMatrix(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
#endif
dataModel
.
distanceMatrix
.
setDimension
(
n0
,
n0
);
dataModel
.
distanceMatrix
.
setDimension
(
n0
,
n0
);
for
(
size_t
i
=
0
;
i
<
n0
;
++
i
){
for
(
size_t
i
=
0
;
i
<
n0
;
++
i
)
{
dataModel
.
distanceMatrix
.
set
(
i
,
i
,
0
);
dataModel
.
distanceMatrix
.
set
(
i
,
i
,
0
);
for
(
size_t
j
=
i
+
1
;
j
<
n0
;
++
j
){
for
(
size_t
j
=
i
+
1
;
j
<
n0
;
++
j
)
{
dataModel
.
distanceMatrix
.
set
(
i
,
j
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
dataModel
.
distanceMatrix
.
set
(
dataModel
.
distanceMatrix
.
set
(
j
,
i
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
i
,
j
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
}
dataModel
.
distanceMatrix
.
set
(
j
,
i
,
int64_t
(
distanceMatrix
.
get
(
i
,
j
)
*
CLIPPER_SCALE
));
}
}
dataModel
.
numVehicles
=
1
;
}
dataModel
.
depot
=
0
;
dataModel
.
numVehicles
=
1
;
dataModel
.
depot
=
0
;
}
}
bool
flight_plan
::
route
(
const
BoostPolygon
&
area
,
bool
flight_plan
::
route
(
const
BoostPolygon
&
area
,
const
flight_plan
::
Transects
&
transects
,
const
flight_plan
::
Transects
&
transects
,
Transects
&
transectsRouted
,
Transects
&
transectsRouted
,
flight_plan
::
Route
&
route
,
flight_plan
::
Route
&
route
,
string
&
errorString
)
{
string
&
errorString
)
//=======================================
{
// Route Transects using Google or-tools.
//=======================================
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Create vertex list;
BoostLineString
vertices
;
// Create vertex list;
size_t
n0
=
0
;
BoostLineString
vertices
;
for
(
const
auto
&
t
:
transects
)
{
size_t
n0
=
0
;
if
(
t
.
size
()
>=
2
)
{
for
(
const
auto
&
t
:
transects
){
n0
+=
2
;
if
(
t
.
size
()
>=
2
){
}
else
{
n0
+=
2
;
n0
+=
1
;
}
else
{
n0
+=
1
;
}
}
vertices
.
reserve
(
n0
);
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
f
)
:
index
(
n
)
,
front
(
f
)
{}
size_t
index
;
bool
front
;
};
std
::
vector
<
TransectInfo
>
transectInfoList
;
size_t
idx
=
0
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
){
const
auto
&
t
=
transects
[
i
];
vertices
.
push_back
(
t
.
front
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
true
});
if
(
t
.
size
()
>=
2
){
vertices
.
push_back
(
t
.
back
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
false
});
}
}
for
(
long
i
=
0
;
i
<
long
(
area
.
outer
().
size
())
-
1
;
++
i
)
{
vertices
.
push_back
(
area
.
outer
()[
i
]);
}
}
for
(
auto
&
ring
:
area
.
inners
())
{
}
for
(
auto
&
vertex
:
ring
)
vertices
.
reserve
(
n0
);
vertices
.
push_back
(
vertex
);
struct
TransectInfo
{
TransectInfo
(
size_t
n
,
bool
f
)
:
index
(
n
),
front
(
f
)
{}
size_t
index
;
bool
front
;
};
std
::
vector
<
TransectInfo
>
transectInfoList
;
size_t
idx
=
0
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
const
auto
&
t
=
transects
[
i
];
vertices
.
push_back
(
t
.
front
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
true
});
if
(
t
.
size
()
>=
2
)
{
vertices
.
push_back
(
t
.
back
());
transectInfoList
.
push_back
(
TransectInfo
{
i
,
false
});
}
}
size_t
n1
=
vertices
.
size
();
}
// Generate routing model.
RoutingDataModel
dataModel
;
for
(
long
i
=
0
;
i
<
long
(
area
.
outer
().
size
())
-
1
;
++
i
)
{
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
vertices
.
push_back
(
area
.
outer
()[
i
]);
// Offset joined area.
}
BoostPolygon
areaOffset
;
for
(
auto
&
ring
:
area
.
inners
())
{
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
for
(
auto
&
vertex
:
ring
)
vertices
.
push_back
(
vertex
);
}
size_t
n1
=
vertices
.
size
();
// Generate routing model.
RoutingDataModel
dataModel
;
Matrix
<
double
>
connectionGraph
(
n1
,
n1
);
// Offset joined area.
BoostPolygon
areaOffset
;
offsetPolygon
(
area
,
areaOffset
,
detail
::
offsetConstant
);
#ifdef SHOW_TIME
#ifdef SHOW_TIME
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
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 SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time _generateRoutingModel(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
#endif
// Create Routing Index Manager.
// Create Routing Index Manager.
RoutingIndexManager
manager
(
dataModel
.
distanceMatrix
.
getN
(),
dataModel
.
numVehicles
,
RoutingIndexManager
manager
(
dataModel
.
distanceMatrix
.
getN
(),
dataModel
.
depot
);
dataModel
.
numVehicles
,
dataModel
.
depot
);
// Create Routing Model.
// Create Routing Model.
RoutingModel
routing
(
manager
);
RoutingModel
routing
(
manager
);
// Create and register a transit callback.
// Create and register a transit callback.
const
int
transitCallbackIndex
=
routing
.
RegisterTransitCallback
(
const
int
transitCallbackIndex
=
routing
.
RegisterTransitCallback
(
[
&
dataModel
,
&
manager
](
int64
from_index
,
int64
to_index
)
->
int64
{
[
&
dataModel
,
&
manager
](
int64
from_index
,
int64
to_index
)
->
int64
{
// Convert from routing variable Index to distance matrix NodeIndex.
// Convert from routing variable Index to distance matrix NodeIndex.
auto
from_node
=
manager
.
IndexToNode
(
from_index
).
value
();
auto
from_node
=
manager
.
IndexToNode
(
from_index
).
value
();
auto
to_node
=
manager
.
IndexToNode
(
to_index
).
value
();
auto
to_node
=
manager
.
IndexToNode
(
to_index
).
value
();
return
dataModel
.
distanceMatrix
.
get
(
from_node
,
to_node
);
return
dataModel
.
distanceMatrix
.
get
(
from_node
,
to_node
);
});
});
// Define cost of each arc.
// Define cost of each arc.
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transitCallbackIndex
);
routing
.
SetArcCostEvaluatorOfAllVehicles
(
transitCallbackIndex
);
// Define Constraints (this constraints have a huge impact on the
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
// solving time, improvments could be done, e.g. SearchFilter).
Solver
*
solver
=
routing
.
solver
();
Solver
*
solver
=
routing
.
solver
();
size_t
index
=
0
;
size_t
index
=
0
;
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
)
{
for
(
size_t
i
=
0
;
i
<
transects
.
size
();
++
i
){
const
auto
&
t
=
transects
[
i
];
const
auto
&
t
=
transects
[
i
];
if
(
t
.
size
()
>=
2
)
{
if
(
t
.
size
()
>=
2
){
auto
idx0
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
));
auto
idx0
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
));
auto
idx1
=
auto
idx1
=
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
+
1
));
manager
.
NodeToIndex
(
RoutingIndexManager
::
NodeIndex
(
index
+
1
));
auto
cond0
=
routing
.
NextVar
(
idx0
)
->
IsEqual
(
idx1
);
auto
cond0
=
routing
.
NextVar
(
idx0
)
->
IsEqual
(
idx1
);
auto
cond1
=
routing
.
NextVar
(
idx1
)
->
IsEqual
(
idx0
);
auto
cond1
=
routing
.
NextVar
(
idx1
)
->
IsEqual
(
idx0
);
vector
<
IntVar
*>
conds
{
cond0
,
cond1
};
vector
<
IntVar
*>
conds
{
cond0
,
cond1
};
auto
c
=
solver
->
MakeAllDifferent
(
conds
);
auto
c
=
solver
->
MakeAllDifferent
(
conds
);
solver
->
MakeRejectFilter
();
solver
->
MakeRejectFilter
();
solver
->
AddConstraint
(
c
);
solver
->
AddConstraint
(
c
);
index
+=
2
;
index
+=
2
;
}
else
{
}
else
{
index
+=
1
;
index
+=
1
;
}
}
}
}
// Setting first solution heuristic.
RoutingSearchParameters
searchParameters
=
DefaultRoutingSearchParameters
();
// Setting first solution heuristic.
searchParameters
.
set_first_solution_strategy
(
RoutingSearchParameters
searchParameters
=
DefaultRoutingSearchParameters
();
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
searchParameters
.
set_first_solution_strategy
(
google
::
protobuf
::
Duration
*
tMax
=
new
google
::
protobuf
::
Duration
();
// seconds
FirstSolutionStrategy
::
PATH_CHEAPEST_ARC
);
tMax
->
set_seconds
(
10
);
google
::
protobuf
::
Duration
*
tMax
=
searchParameters
.
set_allocated_time_limit
(
tMax
);
new
google
::
protobuf
::
Duration
();
// seconds
tMax
->
set_seconds
(
10
);
// Solve the problem.
searchParameters
.
set_allocated_time_limit
(
tMax
);
// Solve the problem.
#ifdef SHOW_TIME
#ifdef SHOW_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 SHOW_TIME
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
delta
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
cout
<<
"Execution time routing.SolveWithParameters(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
);
cout
<<
"Execution time routing.SolveWithParameters(): "
<<
delta
.
count
()
<<
" ms"
<<
endl
;
#endif
#endif
if
(
!
solution
||
solution
->
Size
()
<=
1
){
if
(
!
solution
||
solution
->
Size
()
<=
1
)
{
errorString
=
"Not able to solve the routing problem."
;
errorString
=
"Not able to solve the routing problem."
;
return
false
;
return
false
;
}
}
// Extract index list from solution.
// Extract index list from solution.
index
=
routing
.
Start
(
0
);
index
=
routing
.
Start
(
0
);
std
::
vector
<
size_t
>
route_idx
;
std
::
vector
<
size_t
>
route_idx
;
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
while
(
!
routing
.
IsEnd
(
index
))
{
index
=
solution
->
Value
(
routing
.
NextVar
(
index
));
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
while
(
!
routing
.
IsEnd
(
index
)){
}
index
=
solution
->
Value
(
routing
.
NextVar
(
index
));
route_idx
.
push_back
(
manager
.
IndexToNode
(
index
).
value
());
}
// Helper Lambda.
// Helper Lambda.
auto
idx2Vertex
=
[
&
vertices
](
const
std
::
vector
<
size_t
>
&
idxArray
,
auto
idx2Vertex
=
[
&
vertices
](
const
std
::
vector
<
size_t
>
&
idxArray
,
std
::
vector
<
BoostPoint
>
&
path
)
{
std
::
vector
<
BoostPoint
>
&
path
)
{
for
(
auto
idx
:
idxArray
)
for
(
auto
idx
:
idxArray
)
path
.
push_back
(
vertices
[
idx
]);
path
.
push_back
(
vertices
[
idx
]);
};
};
// Construct route.
// Construct route.
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
){
for
(
size_t
i
=
0
;
i
<
route_idx
.
size
()
-
1
;
++
i
)
{
size_t
idx0
=
route_idx
[
i
];
size_t
idx0
=
route_idx
[
i
];
size_t
idx1
=
route_idx
[
i
+
1
];
size_t
idx1
=
route_idx
[
i
+
1
];
const
auto
&
info1
=
transectInfoList
[
idx0
];
const
auto
&
info1
=
transectInfoList
[
idx0
];
const
auto
&
info2
=
transectInfoList
[
idx1
];
const
auto
&
info2
=
transectInfoList
[
idx1
];
if
(
info1
.
index
==
info2
.
index
)
{
// same transect?
if
(
info1
.
index
==
info2
.
index
)
{
// same transect?
if
(
!
info1
.
front
)
{
// transect reversal needed?
if
(
!
info1
.
front
)
{
// transect reversal needed?
BoostLineString
reversedTransect
;
BoostLineString
reversedTransect
;
const
auto
&
t
=
transects
[
info1
.
index
];
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
){
for
(
auto
it
=
t
.
end
()
-
1
;
it
>=
t
.
begin
();
--
it
)
{
reversedTransect
.
push_back
(
*
it
);
reversedTransect
.
push_back
(
*
it
);
}
}
transectsRouted
.
push_back
(
reversedTransect
);
transectsRouted
.
push_back
(
reversedTransect
);
for
(
auto
it
=
reversedTransect
.
begin
();
it
<
reversedTransect
.
end
()
-
1
;
++
it
){
for
(
auto
it
=
reversedTransect
.
begin
();
route
.
push_back
(
*
it
);
it
<
reversedTransect
.
end
()
-
1
;
++
it
)
{
}
route
.
push_back
(
*
it
);
}
else
{
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
){
route
.
push_back
(
*
it
);
}
transectsRouted
.
push_back
(
t
);
}
}
else
{
std
::
vector
<
size_t
>
idxList
;
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
idxList
);
if
(
i
!=
route_idx
.
size
()
-
2
){
idxList
.
pop_back
();
}
idx2Vertex
(
idxList
,
route
);
}
}
}
else
{
const
auto
&
t
=
transects
[
info1
.
index
];
for
(
auto
it
=
t
.
begin
();
it
<
t
.
end
()
-
1
;
++
it
)
{
route
.
push_back
(
*
it
);
}
transectsRouted
.
push_back
(
t
);
}
}
else
{
std
::
vector
<
size_t
>
idxList
;
shortestPathFromGraph
(
connectionGraph
,
idx0
,
idx1
,
idxList
);
if
(
i
!=
route_idx
.
size
()
-
2
)
{
idxList
.
pop_back
();
}
idx2Vertex
(
idxList
,
route
);
}
}
}
}
}
}
// namespace snake
}
// namespace snake
bool
boost
::
geometry
::
model
::
operator
==
(
snake
::
BoostPoint
p1
,
snake
::
BoostPoint
p2
)
{
return
(
p1
.
get
<
0
>
()
==
p2
.
get
<
0
>
())
&&
(
p1
.
get
<
1
>
()
==
p2
.
get
<
1
>
());
}
bool
boost
::
geometry
::
model
::
operator
!=
(
snake
::
BoostPoint
p1
,
snake
::
BoostPoint
p2
)
{
return
!
(
p1
==
p2
);
}
src/Snake/snake.h
View file @
c6e6a519
#pragma once
#pragma once
#include <vector>
#include <string>
#include <array>
#include <array>
#include <memory>
#include <memory>
#include <string>
#include <vector>
#include <boost/geometry.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include "snake_typedefs.h"
namespace
bg
=
boost
::
geometry
;
namespace
bu
=
boost
::
units
;
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <GeographicLib/LocalCartesian.hpp>
...
@@ -13,256 +21,239 @@
...
@@ -13,256 +21,239 @@
using
namespace
std
;
using
namespace
std
;
namespace
snake
{
namespace
snake
{
//=========================================================================
//=========================================================================
// Geometry stuff.
// Geometry stuff.
//=========================================================================
//=========================================================================
template
<
class
T
>
class
Matrix
{
typedef
bg
::
model
::
point
<
double
,
2
,
bg
::
cs
::
cartesian
>
BoostPoint
;
// typedef std::vector<BoostPoint> BoostPointList;
typedef
bg
::
model
::
polygon
<
BoostPoint
>
BoostPolygon
;
typedef
bg
::
model
::
linestring
<
BoostPoint
>
BoostLineString
;
typedef
std
::
vector
<
std
::
vector
<
int64_t
>>
Int64Matrix
;
typedef
bg
::
model
::
box
<
BoostPoint
>
BoostBox
;
template
<
class
T
>
class
Matrix
{
public:
public:
Matrix
()
:
Matrix
()
:
_elements
(
0
),
_m
(
0
),
_n
(
0
),
_isInitialized
(
false
)
{}
_elements
(
0
),
Matrix
(
size_t
m
,
size_t
n
)
:
Matrix
(
m
,
n
,
T
{
0
})
{}
_m
(
0
),
Matrix
(
size_t
m
,
size_t
n
,
T
value
)
_n
(
0
),
:
_elements
(
m
*
n
),
_m
(
m
),
_n
(
n
),
_isInitialized
(
true
)
{
_isInitialized
(
false
)
assert
((
m
>
0
)
||
(
n
>
0
));
{
_matrix
.
resize
(
_elements
,
value
);
}
}
Matrix
(
size_t
m
,
size_t
n
)
:
Matrix
(
m
,
n
,
T
{
0
}){}
Matrix
(
size_t
m
,
size_t
n
,
T
value
)
:
double
get
(
size_t
i
,
size_t
j
)
const
{
_elements
(
m
*
n
),
assert
(
_isInitialized
);
_m
(
m
),
assert
(
i
<
_m
);
_n
(
n
),
assert
(
j
<
_n
);
_isInitialized
(
true
)
return
_matrix
[
i
*
_m
+
j
];
{
}
assert
((
m
>
0
)
||
(
n
>
0
));
_matrix
.
resize
(
_elements
,
value
);
size_t
getM
()
const
{
return
_m
;
}
}
size_t
getN
()
const
{
return
_n
;
}
double
get
(
size_t
i
,
size_t
j
)
const
void
set
(
size_t
i
,
size_t
j
,
const
T
&
value
)
{
{
assert
(
_isInitialized
);
assert
(
_isInitialized
);
assert
(
i
<
_m
);
assert
(
i
<
_m
);
assert
(
j
<
_n
);
assert
(
j
<
_n
);
_matrix
[
i
*
_m
+
j
]
=
value
;
return
_matrix
[
i
*
_m
+
j
];
}
}
void
setDimension
(
size_t
m
,
size_t
n
,
const
T
&
value
)
{
size_t
getM
()
const
{
return
_m
;}
assert
((
m
>
0
)
||
(
n
>
0
));
size_t
getN
()
const
{
return
_n
;}
assert
(
!
_isInitialized
);
_m
=
m
;
void
set
(
size_t
i
,
size_t
j
,
const
T
&
value
)
_n
=
n
;
{
_elements
=
n
*
m
;
assert
(
_isInitialized
);
_matrix
.
resize
(
_elements
,
value
);
assert
(
i
<
_m
);
_isInitialized
=
true
;
assert
(
j
<
_n
);
}
_matrix
[
i
*
_m
+
j
]
=
value
;
}
void
setDimension
(
size_t
m
,
size_t
n
)
{
setDimension
(
m
,
n
,
T
{
0
});
}
void
setDimension
(
size_t
m
,
size_t
n
,
const
T
&
value
)
{
assert
((
m
>
0
)
||
(
n
>
0
));
assert
(
!
_isInitialized
);
_m
=
m
;
_n
=
n
;
_elements
=
n
*
m
;
_matrix
.
resize
(
_elements
,
value
);
_isInitialized
=
true
;
}
void
setDimension
(
size_t
m
,
size_t
n
)
{
setDimension
(
m
,
n
,
T
{
0
});
}
private:
private:
size_t
_elements
;
size_t
_elements
;
size_t
_m
;
size_t
_m
;
size_t
_n
;
size_t
_n
;
bool
_isInitialized
;
bool
_isInitialized
;
std
::
vector
<
T
>
_matrix
;
std
::
vector
<
T
>
_matrix
;
};
};
struct
BoundingBox
{
struct
BoundingBox
{
BoundingBox
();
BoundingBox
();
void
clear
();
void
clear
();
double
width
;
double
width
;
double
height
;
double
height
;
double
angle
;
double
angle
;
BoostPolygon
corners
;
BoostPolygon
corners
;
};
};
template
<
class
GeoPoint
>
template
<
class
GeoPoint
>
void
toENU
(
const
GeoPoint
&
origin
,
const
GeoPoint
&
in
,
BoostPoint
&
out
)
void
toENU
(
const
GeoPoint
&
origin
,
const
GeoPoint
&
in
,
BoostPoint
&
out
)
{
{
GeographicLib
::
Geocentric
earth
(
GeographicLib
::
Constants
::
WGS84_a
(),
GeographicLib
::
Geocentric
earth
(
GeographicLib
::
Constants
::
WGS84_a
(),
GeographicLib
::
Constants
::
WGS84_f
());
GeographicLib
::
Constants
::
WGS84_f
());
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
altitude
(),
earth
);
origin
.
longitude
(),
origin
.
altitude
(),
double
x
,
y
,
z
;
earth
);
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
in
.
altitude
(),
x
,
y
,
z
);
out
.
set
<
0
>
(
x
);
double
x
,
y
,
z
;
out
.
set
<
0
>
(
y
);
proj
.
Forward
(
in
.
latitude
(),
in
.
longitude
(),
in
.
altitude
(),
x
,
y
,
z
);
(
void
)
z
;
out
.
set
<
0
>
(
x
);
out
.
set
<
0
>
(
y
);
(
void
)
z
;
}
}
template
<
class
GeoPoint
>
template
<
class
GeoPoint
>
void
fromENU
(
const
GeoPoint
&
origin
,
const
BoostPoint
&
in
,
GeoPoint
&
out
)
void
fromENU
(
const
GeoPoint
&
origin
,
const
BoostPoint
&
in
,
GeoPoint
&
out
)
{
{
GeographicLib
::
Geocentric
earth
(
GeographicLib
::
Constants
::
WGS84_a
(),
GeographicLib
::
Geocentric
earth
(
GeographicLib
::
Constants
::
WGS84_a
(),
GeographicLib
::
Constants
::
WGS84_f
());
GeographicLib
::
Constants
::
WGS84_f
());
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
longitude
(),
GeographicLib
::
LocalCartesian
proj
(
origin
.
latitude
(),
origin
.
altitude
(),
earth
);
origin
.
longitude
(),
origin
.
altitude
(),
double
lat
,
lon
,
alt
;
earth
);
proj
.
Reverse
(
in
.
get
<
0
>
(),
in
.
get
<
1
>
(),
0
.
0
,
lat
,
lon
,
alt
);
out
.
setLatitude
(
lat
);
double
lat
,
lon
,
alt
;
out
.
setLongitude
(
lon
);
proj
.
Reverse
(
in
.
get
<
0
>
(),
in
.
get
<
1
>
(),
0
.
0
,
lat
,
lon
,
alt
);
out
.
setAltitude
(
alt
);
out
.
setLatitude
(
lat
);
out
.
setLongitude
(
lon
);
out
.
setAltitude
(
alt
);
}
}
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
void
polygonCenter
(
const
BoostPolygon
&
polygon
,
BoostPoint
&
center
);
BoostPoint
&
center
);
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
BoundingBox
&
minBBox
);
void
minimalBoundingBox
(
const
BoostPolygon
&
polygon
,
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
BoundingBox
&
minBBox
);
double
offset
);
void
offsetPolygon
(
const
BoostPolygon
&
polygon
,
BoostPolygon
&
polygonOffset
,
void
graphFromPolygon
(
const
BoostPolygon
&
polygon
,
double
offset
);
const
BoostLineString
&
vertices
,
Matrix
<
double
>
&
graph
);
void
graphFromPolygon
(
const
BoostPolygon
&
polygon
,
const
BoostLineString
&
vertices
,
Matrix
<
double
>
&
graph
);
void
toDistanceMatrix
(
Matrix
<
double
>
&
graph
);
void
toDistanceMatrix
(
Matrix
<
double
>
&
graph
);
bool
dijkstraAlgorithm
(
const
size_t
numElements
,
bool
dijkstraAlgorithm
(
size_t
startIndex
,
const
size_t
numElements
,
size_t
startIndex
,
size_t
endIndex
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
elementPath
,
std
::
vector
<
size_t
>
&
elementPath
,
std
::
function
<
double
(
const
size_t
,
const
size_t
)
>
distanceDij
);
std
::
function
<
double
(
const
size_t
,
const
size_t
)
>
distanceDij
);
void
shortestPathFromGraph
(
const
Matrix
<
double
>
&
graph
,
size_t
startIndex
,
void
shortestPathFromGraph
(
const
Matrix
<
double
>
&
graph
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
pathIdx
);
size_t
startIndex
,
size_t
endIndex
,
std
::
vector
<
size_t
>
&
pathIdx
);
//=========================================================================
//=========================================================================
// Scenario.
// Scenario.
//=========================================================================
//=========================================================================
class
Scenario
{
public:
Scenario
();
void
setMeasurementArea
(
const
BoostPolygon
&
area
)
;
typedef
bu
::
quantity
<
bu
::
si
::
length
>
Length
;
void
setServiceArea
(
const
BoostPolygon
&
area
)
;
typedef
bu
::
quantity
<
bu
::
si
::
area
>
Area
;
void
setCorridor
(
const
BoostPolygon
&
area
)
;
typedef
bu
::
quantity
<
bu
::
si
::
plane_angle
>
Angle
;
const
BoundingBox
&
mAreaBoundingBox
()
const
;
class
Scenario
{
public:
Scenario
();
void
setMeasurementArea
(
const
BoostPolygon
&
area
);
void
setServiceArea
(
const
BoostPolygon
&
area
);
void
setCorridor
(
const
BoostPolygon
&
area
);
const
BoostPolygon
&
measurementArea
()
const
;
const
BoundingBox
&
mAreaBoundingBox
()
const
;
const
BoostPolygon
&
serviceArea
()
const
;
const
BoostPolygon
&
corridor
()
const
;
BoostPolygon
&
measurementArea
()
;
const
BoostPolygon
&
measurementArea
()
const
;
BoostPolygon
&
serviceArea
()
;
const
BoostPolygon
&
serviceArea
()
const
;
BoostPolygon
&
corridor
()
;
const
BoostPolygon
&
corridor
()
const
;
Length
tileWidth
()
const
;
BoostPolygon
&
measurementArea
();
void
setTileWidth
(
Length
tileWidth
);
BoostPolygon
&
serviceArea
();
BoostPolygon
&
corridor
();
Length
tileHeight
()
const
;
Length
tileWidth
()
const
;
void
setTileHeight
(
Length
tileHeight
);
void
setTileWidth
(
Length
tileWidth
);
Area
minTileArea
()
const
;
Length
tileHeight
()
const
;
void
setMinTileArea
(
Area
minTileArea
);
void
setTileHeight
(
Length
tileHeight
);
const
BoostPolygon
&
joinedArea
()
const
;
Area
minTileArea
()
const
;
const
vector
<
BoostPolygon
>
&
tiles
()
const
;
void
setMinTileArea
(
Area
minTileArea
);
const
BoostLineString
&
tileCenterPoints
()
const
;
const
BoundingBox
&
measurementAreaBBox
()
const
;
const
BoostPoint
&
homePositon
()
const
;
bool
update
();
const
BoostPolygon
&
joinedArea
()
const
;
const
vector
<
BoostPolygon
>
&
tiles
()
const
;
const
BoostLineString
&
tileCenterPoints
()
const
;
const
BoundingBox
&
measurementAreaBBox
()
const
;
const
BoostPoint
&
homePositon
()
const
;
string
errorString
;
bool
update
();
string
errorString
;
private:
private:
bool
_calculateBoundingBox
();
bool
_calculateBoundingBox
();
bool
_calculateTiles
();
bool
_calculateTiles
();
bool
_calculateJoinedArea
();
bool
_calculateJoinedArea
();
Length
_tileWidth
;
Length
_tileWidth
;
Length
_tileHeight
;
Length
_tileHeight
;
Area
_minTileArea
;
Area
_minTileArea
;
mutable
bool
_needsUpdate
;
mutable
bool
_needsUpdate
;
BoostPolygon
_mArea
;
BoostPolygon
_mArea
;
BoostPolygon
_sArea
;
BoostPolygon
_sArea
;
BoostPolygon
_corridor
;
BoostPolygon
_corridor
;
BoostPolygon
_jArea
;
BoostPolygon
_jArea
;
BoundingBox
_mAreaBoundingBox
;
BoundingBox
_mAreaBoundingBox
;
vector
<
BoostPolygon
>
_tiles
;
vector
<
BoostPolygon
>
_tiles
;
BoostLineString
_tileCenterPoints
;
BoostLineString
_tileCenterPoints
;
BoostPoint
_homePosition
;
BoostPoint
_homePosition
;
};
};
template
<
class
GeoPoint
,
template
<
class
,
class
...
>
class
Container
>
template
<
class
GeoPoint
,
template
<
class
,
class
...
>
class
Container
>
void
areaToEnu
(
const
GeoPoint
&
origin
,
void
areaToEnu
(
const
GeoPoint
&
origin
,
const
Container
<
GeoPoint
>
&
in
,
const
Container
<
GeoPoint
>
&
in
,
BoostPolygon
&
out
)
{
BoostPolygon
&
out
)
for
(
auto
vertex
:
in
)
{
{
BoostPoint
p
;
for
(
auto
vertex
:
in
)
{
toENU
(
origin
,
vertex
,
p
);
BoostPoint
p
;
out
.
outer
().
push_back
(
p
);
toENU
(
origin
,
vertex
,
p
);
}
out
.
outer
().
push_back
(
p
);
bg
::
correct
(
out
);
}
bg
::
correct
(
out
);
}
}
template
<
class
GeoPoint
,
template
<
class
,
class
...
>
class
Container
>
template
<
class
GeoPoint
,
template
<
class
,
class
...
>
class
Container
>
void
areaFromEnu
(
const
GeoPoint
&
origin
,
void
areaFromEnu
(
const
GeoPoint
&
origin
,
BoostPolygon
&
in
,
BoostPolygon
&
in
,
const
Container
<
GeoPoint
>
&
out
)
{
const
Container
<
GeoPoint
>
&
out
)
for
(
auto
vertex
:
in
.
outer
())
{
{
GeoPoint
p
;
for
(
auto
vertex
:
in
.
outer
())
{
fromENU
(
origin
,
vertex
,
p
);
GeoPoint
p
;
out
.
push_back
(
p
);
fromENU
(
origin
,
vertex
,
p
);
}
out
.
push_back
(
p
);
}
}
}
bool
joinAreas
(
const
std
::
vector
<
BoostPolygon
>
&
areas
,
bool
joinAreas
(
const
std
::
vector
<
BoostPolygon
>
&
areas
,
BoostPolygon
&
joinedArea
);
BoostPolygon
&
joinedArea
);
//========================================================================================
//========================================================================================
// flight_plan
// flight_plan
//========================================================================================
//========================================================================================
namespace
flight_plan
{
namespace
flight_plan
{
using
Transects
=
vector
<
BoostLineString
>
;
using
Transects
=
vector
<
BoostLineString
>
;
using
Progress
=
vector
<
int
>
;
using
Progress
=
vector
<
int
>
;
using
Route
=
vector
<
BoostPoint
>
;
using
Route
=
vector
<
BoostPoint
>
;
bool
transectsFromScenario
(
Length
distance
,
bool
transectsFromScenario
(
Length
distance
,
Length
minLength
,
Angle
angle
,
Length
minLength
,
const
Scenario
&
scenario
,
const
Progress
&
p
,
Angle
angle
,
Transects
&
t
,
string
&
errorString
);
const
Scenario
&
scenario
,
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
const
Progress
&
p
,
Transects
&
transectsRouted
,
Route
&
route
,
string
&
errorString
);
Transects
&
t
,
string
&
errorString
);
bool
route
(
const
BoostPolygon
&
area
,
const
Transects
&
transects
,
Transects
&
transectsRouted
,
Route
&
route
,
string
&
errorString
);
}
// namespace flight_plan
}
// namespace flight_plan
namespace
detail
{
namespace
detail
{
const
double
offsetConstant
=
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
const
double
offsetConstant
=
0
.
1
;
// meter, polygon offset to compenstate for numerical inaccurracies.
}
// namespace detail
}
// namespace detail
}
}
// namespace snake
// operator== and operator!= for boost point
namespace
boost
{
namespace
geometry
{
namespace
model
{
bool
operator
==
(
snake
::
BoostPoint
p1
,
snake
::
BoostPoint
p2
);
bool
operator
!=
(
snake
::
BoostPoint
p1
,
snake
::
BoostPoint
p2
);
}
// namespace model
}
// namespace geometry
}
// namespace boost
src/Wima/Snake/SnakeDataManager.cc
View file @
c6e6a519
...
@@ -4,32 +4,30 @@
...
@@ -4,32 +4,30 @@
#include <QMutexLocker>
#include <QMutexLocker>
#include "QGCApplication.h"
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "QGCToolbox.h"
#include "QGCToolbox.h"
#include "WimaSettings.h"
#include "SettingsFact.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <memory>
#include <memory>
#include <shared_mutex>
#include <mutex>
#include <mutex>
#include <shared_mutex>
#include "snake.h"
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#define EVENT_TIMER_INTERVAL 500 // ms
#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000 // ms
#define TIMEOUT 3000 // ms
using
QVariantList
=
QList
<
QVariant
>
;
using
QVariantList
=
QList
<
QVariant
>
;
using
ROSBridgePtr
=
std
::
unique_ptr
<
ros_bridge
::
ROSBridge
>
;
using
ROSBridgePtr
=
std
::
unique_ptr
<
ros_bridge
::
ROSBridge
>
;
...
@@ -38,549 +36,552 @@ using UniqueLock = std::unique_lock<shared_timed_mutex>;
...
@@ -38,549 +36,552 @@ using UniqueLock = std::unique_lock<shared_timed_mutex>;
using
SharedLock
=
std
::
shared_lock
<
shared_timed_mutex
>
;
using
SharedLock
=
std
::
shared_lock
<
shared_timed_mutex
>
;
using
JsonDocUPtr
=
ros_bridge
::
com_private
::
JsonDocUPtr
;
using
JsonDocUPtr
=
ros_bridge
::
com_private
::
JsonDocUPtr
;
class
SnakeImpl
:
public
QObject
{
class
SnakeDataManager
::
SnakeImpl
{
Q_OBJECT
public:
public:
SnakeImpl
(
SnakeDataManager
*
p
);
SnakeImpl
(
SnakeDataManager
*
p
)
:
rosBridgeEnabeled
(
false
),
topicServiceSetupDone
(
false
),
bool
precondition
()
const
;
lineDistance
(
1
*
si
::
meter
),
minTransectLength
(
1
*
si
::
meter
),
void
resetWaypointData
();
calcInProgress
(
false
),
parent
(
p
)
{
bool
doTopicServiceSetup
();
// ROS Bridge.
WimaSettings
*
wimaSettings
=
// Private data.
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
ROSBridgePtr
pRosBridge
;
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
bool
rosBridgeEnabeled
;
auto
setConnectionString
=
[
connectionStringFact
,
this
]
{
bool
topicServiceSetupDone
;
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
QTimer
eventTimer
;
if
(
ros_bridge
::
isValidConnectionString
(
QTimer
timeout
;
connectionString
.
toLocal8Bit
().
data
()))
{
mutable
std
::
shared_timed_mutex
mutex
;
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
// Scenario
}
else
{
snake
::
Scenario
scenario
;
QString
defaultString
(
"localhost:9090"
);
Length
lineDistance
;
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
Length
minTransectLength
;
connectionString
);
QList
<
QGeoCoordinate
>
mArea
;
qgcApp
()
->
showMessage
(
"Resetting connection string to: "
+
QList
<
QGeoCoordinate
>
sArea
;
defaultString
);
QList
<
QGeoCoordinate
>
corridor
;
connectionStringFact
->
setRawValue
(
QGeoCoordinate
ENUOrigin
;
QVariant
(
defaultString
));
// calls this function recursively
SnakeTiles
tiles
;
}
QVariantList
tileCenterPoints
;
};
SnakeTilesLocal
tilesENU
;
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
QVector
<
QPointF
>
tileCenterPointsENU
;
setConnectionString
);
setConnectionString
();
// Waypoints
QVector
<
QGeoCoordinate
>
waypoints
;
QVector
<
QGeoCoordinate
>
arrivalPath
;
QVector
<
QGeoCoordinate
>
returnPath
;
QVector
<
QPointF
>
waypointsENU
;
QVector
<
QPointF
>
arrivalPathENU
;
QVector
<
QPointF
>
returnPathENU
;
QString
errorMessage
;
// Other
std
::
atomic_bool
calcInProgress
;
QNemoProgress
qProgress
;
std
::
vector
<
int
>
progress
;
QNemoHeartbeat
heartbeat
;
SnakeDataManager
*
parent
;
// Periodic.
connect
(
&
this
->
eventTimer
,
&
QTimer
::
timeout
,
[
this
]
{
if
(
this
->
rosBridgeEnabeled
)
{
if
(
!
this
->
pRosBridge
->
isRunning
())
{
this
->
pRosBridge
->
start
();
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
this
->
pRosBridge
->
connected
()
&&
!
this
->
topicServiceSetupDone
)
{
if
(
this
->
doTopicServiceSetup
())
this
->
topicServiceSetupDone
=
true
;
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
!
this
->
pRosBridge
->
connected
()
&&
this
->
topicServiceSetupDone
)
{
this
->
pRosBridge
->
reset
();
this
->
pRosBridge
->
start
();
this
->
topicServiceSetupDone
=
false
;
}
}
else
if
(
this
->
pRosBridge
->
isRunning
())
{
this
->
pRosBridge
->
reset
();
this
->
topicServiceSetupDone
=
false
;
}
});
this
->
eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
}
bool
precondition
()
const
;
void
resetOutput
();
bool
doTopicServiceSetup
();
// Private data.
ROSBridgePtr
pRosBridge
;
bool
rosBridgeEnabeled
;
bool
topicServiceSetupDone
;
QTimer
eventTimer
;
QTimer
timeout
;
mutable
std
::
shared_timed_mutex
mutex
;
// Scenario
snake
::
Scenario
scenario
;
Length
lineDistance
;
Length
minTransectLength
;
QList
<
QGeoCoordinate
>
mArea
;
QList
<
QGeoCoordinate
>
sArea
;
QList
<
QGeoCoordinate
>
corridor
;
QGeoCoordinate
ENUOrigin
;
SnakeTiles
tiles
;
QmlObjectListModel
tilesQml
;
QVariantList
tileCenterPoints
;
SnakeTilesLocal
tilesENU
;
QVector
<
QPointF
>
tileCenterPointsENU
;
// Waypoints
QVector
<
QGeoCoordinate
>
waypoints
;
QVector
<
QGeoCoordinate
>
arrivalPath
;
QVector
<
QGeoCoordinate
>
returnPath
;
QVector
<
QPointF
>
waypointsENU
;
QVector
<
QPointF
>
arrivalPathENU
;
QVector
<
QPointF
>
returnPathENU
;
QString
errorMessage
;
// Other
std
::
atomic_bool
calcInProgress
;
QNemoProgress
qProgress
;
std
::
vector
<
int
>
progress
;
QNemoHeartbeat
heartbeat
;
SnakeDataManager
*
parent
;
};
};
template
<
class
AtomicType
>
bool
SnakeDataManager
::
SnakeImpl
::
doTopicServiceSetup
()
{
class
ToggleRAII
{
using
namespace
ros_bridge
::
messages
;
public:
UniqueLock
lk
(
this
->
mutex
);
ToggleRAII
(
AtomicType
&
t
)
:
_t
(
t
)
if
(
this
->
tilesENU
.
polygons
().
size
()
==
0
)
{}
return
false
;
~
ToggleRAII
()
// Publish snake origin.
{
this
->
pRosBridge
->
advertiseTopic
(
if
(
_t
.
load
()
){
"/snake/origin"
,
geographic_msgs
::
geo_point
::
messageType
().
c_str
());
_t
.
store
(
false
);
JsonDocUPtr
jOrigin
(
}
else
{
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
_t
.
store
(
true
);
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
this
->
pRosBridge
->
advertiseTopic
(
"/snake/tiles"
,
jsk_recognition_msgs
::
polygon_array
::
messageType
().
c_str
());
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
// Subscribe nemo progress.
this
->
pRosBridge
->
subscribe
(
"/nemo/progress"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
)
{
UniqueLock
lk
(
this
->
mutex
);
int
requiredSize
=
this
->
tilesENU
.
polygons
().
size
();
auto
&
progressMsg
=
this
->
qProgress
;
if
(
!
nemo_msgs
::
progress
::
fromJson
(
*
pDoc
,
progressMsg
)
||
progressMsg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
// Set progress to default.
progressMsg
.
progress
().
fill
(
0
,
requiredSize
);
// Publish snake origin.
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
}
}
this
->
progress
.
clear
();
private:
for
(
const
auto
&
p
:
progressMsg
.
progress
())
{
AtomicType
&
_t
;
this
->
progress
.
push_back
(
p
);
};
}
lk
.
unlock
();
emit
this
->
parent
->
nemoProgressChanged
();
});
// Subscribe /nemo/heartbeat.
this
->
pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
)
{
UniqueLock
lk
(
this
->
mutex
);
if
(
this
->
timeout
.
isActive
())
{
this
->
timeout
.
stop
();
}
auto
&
heartbeatMsg
=
this
->
heartbeat
;
if
(
!
nemo_msgs
::
heartbeat
::
fromJson
(
*
pDoc
,
heartbeatMsg
))
{
heartbeatMsg
.
setStatus
(
integral
(
NemoStatus
::
InvalidHeartbeat
));
}
this
->
timeout
.
singleShot
(
TIMEOUT
,
[
this
]
{
UniqueLock
lk
(
this
->
mutex
);
this
->
heartbeat
.
setStatus
(
integral
(
NemoStatus
::
Timeout
));
emit
this
->
parent
->
nemoStatusChanged
(
integral
(
NemoStatus
::
Timeout
));
});
emit
this
->
parent
->
nemoStatusChanged
(
heartbeatMsg
.
status
());
});
// Advertise /snake/get_origin.
this
->
pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
using
namespace
ros_bridge
::
messages
;
SharedLock
lk
(
this
->
mutex
);
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
auto
&
origin
=
this
->
ENUOrigin
;
rapidjson
::
Value
jOrigin
(
rapidjson
::
kObjectType
);
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
());
lk
.
unlock
();
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
// Advertise /snake/get_tiles.
this
->
pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
SharedLock
lk
(
this
->
mutex
);
SnakeDataManager
::
SnakeDataManager
(
QObject
*
parent
)
JsonDocUPtr
pDoc
(
:
QThread
(
parent
)
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
,
pImpl
(
std
::
make_unique
<
SnakeImpl
>
(
this
))
rapidjson
::
Value
jSnakeTiles
(
rapidjson
::
kObjectType
);
bool
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
{
return
true
;
}
}
SnakeDataManager
::~
SnakeDataManager
()
bool
SnakeDataManager
::
SnakeImpl
::
precondition
()
const
{
return
true
;
}
{
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void
SnakeDataManager
::
SnakeImpl
::
resetOutput
()
{
this
->
waypoints
.
clear
();
this
->
arrivalPath
.
clear
();
this
->
returnPath
.
clear
();
this
->
ENUOrigin
=
QGeoCoordinate
(
0.0
,
0.0
,
0.0
);
this
->
waypointsENU
.
clear
();
this
->
arrivalPathENU
.
clear
();
this
->
returnPathENU
.
clear
();
this
->
tilesQml
.
clearAndDeleteContents
();
this
->
tiles
.
polygons
().
clear
();
this
->
tileCenterPoints
.
clear
();
this
->
tilesENU
.
polygons
().
clear
();
this
->
tileCenterPointsENU
.
clear
();
this
->
errorMessage
.
clear
();
}
}
void
SnakeDataManager
::
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
)
template
<
class
Callable
>
class
CommandRAII
{
{
public:
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
CommandRAII
(
Callable
&
fun
)
:
_fun
(
fun
)
{}
this
->
pImpl
->
mArea
=
measurementArea
;
}
~
CommandRAII
()
{
_fun
();
}
void
SnakeDataManager
::
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
)
private:
{
Callable
_fun
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
};
this
->
pImpl
->
sArea
=
serviceArea
;
SnakeDataManager
::
SnakeDataManager
(
QObject
*
parent
)
:
QThread
(
parent
),
pImpl
(
std
::
make_unique
<
SnakeImpl
>
(
this
))
{}
SnakeDataManager
::~
SnakeDataManager
()
{}
void
SnakeDataManager
::
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
)
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
mArea
=
measurementArea
;
}
}
void
SnakeDataManager
::
set
Corridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
)
void
SnakeDataManager
::
set
ServiceArea
(
{
const
QList
<
QGeoCoordinate
>
&
serviceArea
)
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
corridor
=
corridor
;
this
->
pImpl
->
sArea
=
serviceArea
;
}
}
QNemoProgress
SnakeDataManager
::
nemoProgress
()
void
SnakeDataManager
::
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
)
{
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
corridor
=
corridor
;
return
this
->
pImpl
->
qProgress
;
}
}
int
SnakeDataManager
::
nemoStatus
()
const
QmlObjectListModel
*
SnakeDataManager
::
tiles
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
&
this
->
pImpl
->
tilesQml
;
return
this
->
pImpl
->
heartbeat
.
status
();
}
}
bool
SnakeDataManager
::
calcInProgress
()
QVariantList
SnakeDataManager
::
tileCenterPoints
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
calcInProgress
.
load
()
;
return
this
->
pImpl
->
tileCenterPoints
;
}
}
Length
SnakeDataManager
::
lineDistance
()
const
QNemoProgress
SnakeDataManager
::
nemoProgress
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
qProgress
;
return
this
->
pImpl
->
lineDistance
;
}
}
void
SnakeDataManager
::
setLineDistance
(
Length
lineDistance
)
int
SnakeDataManager
::
nemoStatus
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
heartbeat
.
status
();
this
->
pImpl
->
lineDistance
=
lineDistance
;
}
}
Area
SnakeDataManager
::
minTileArea
()
const
bool
SnakeDataManager
::
calcInProgress
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
calcInProgress
.
load
();
return
this
->
pImpl
->
scenario
.
minTileArea
();
}
}
void
SnakeDataManager
::
setMinTileArea
(
Area
minTileArea
)
QString
SnakeDataManager
::
errorMessage
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
errorMessage
;
this
->
pImpl
->
scenario
.
setMinTileArea
(
minTileArea
);
}
}
Length
SnakeDataManager
::
tileHeight
()
const
bool
SnakeDataManager
::
success
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
errorMessage
.
isEmpty
()
?
true
:
false
;
return
this
->
pImpl
->
scenario
.
tileHeight
();
}
}
void
SnakeDataManager
::
setTileHeight
(
Length
tileHeight
)
QVector
<
QGeoCoordinate
>
SnakeDataManager
::
waypoints
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
waypoints
;
this
->
pImpl
->
scenario
.
setTileHeight
(
tileHeight
);
}
}
Length
SnakeDataManager
::
tileWidth
()
const
QVector
<
QGeoCoordinate
>
SnakeDataManager
::
arrivalPath
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
arrivalPath
;
return
this
->
pImpl
->
scenario
.
tileWidth
();
}
}
void
SnakeDataManager
::
setTileWidth
(
Length
tileWidth
)
QVector
<
QGeoCoordinate
>
SnakeDataManager
::
returnPath
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
returnPath
;
this
->
pImpl
->
scenario
.
setTileWidth
(
tileWidth
);
}
}
void
SnakeDataManager
::
enableRosBridge
()
Length
SnakeDataManager
::
lineDistance
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
rosBridgeEnabeled
=
tru
e
;
return
this
->
pImpl
->
lineDistanc
e
;
}
}
void
SnakeDataManager
::
disableRosBride
()
void
SnakeDataManager
::
setLineDistance
(
Length
lineDistance
)
{
{
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
rosBridgeEnabeled
=
fals
e
;
this
->
pImpl
->
lineDistance
=
lineDistanc
e
;
}
}
bool
SnakeImpl
::
precondition
()
const
Area
SnakeDataManager
::
minTileArea
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
true
;
return
this
->
pImpl
->
scenario
.
minTileArea
()
;
}
}
//!
void
SnakeDataManager
::
setMinTileArea
(
Area
minTileArea
)
{
//! \brief Resets waypoint data.
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
//! \pre this->_pImpl->mutex must be locked.
this
->
pImpl
->
scenario
.
setMinTileArea
(
minTileArea
);
void
SnakeImpl
::
resetWaypointData
()
{
this
->
waypoints
.
clear
();
this
->
arrivalPath
.
clear
();
this
->
returnPath
.
clear
();
this
->
ENUOrigin
=
QGeoCoordinate
(
0.0
,
0.0
,
0.0
);
this
->
waypointsENU
.
clear
();
this
->
arrivalPathENU
.
clear
();
this
->
returnPathENU
.
clear
();
this
->
tiles
.
polygons
().
clear
();
this
->
tileCenterPoints
.
clear
();
this
->
tilesENU
.
polygons
().
clear
();
this
->
tileCenterPointsENU
.
clear
();
this
->
errorMessage
.
clear
();
}
}
void
SnakeDataManager
::
run
()
Length
SnakeDataManager
::
tileHeight
()
const
{
{
SharedLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
calcInProgress
.
store
(
true
);
return
this
->
pImpl
->
scenario
.
tileHeight
(
);
ToggleRAII
<
std
::
atomic_bool
>
tr
(
this
->
pImpl
->
calcInProgress
);
}
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
void
SnakeDataManager
::
setTileHeight
(
Length
tileHeight
)
{
this
->
pImpl
->
resetWaypointData
();
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
scenario
.
setTileHeight
(
tileHeight
);
}
if
(
!
this
->
pImpl
->
precondition
()
)
Length
SnakeDataManager
::
tileWidth
()
const
{
return
;
SharedLock
lk
(
this
->
pImpl
->
mutex
);
return
this
->
pImpl
->
scenario
.
tileWidth
();
}
if
(
this
->
pImpl
->
mArea
.
size
()
<
3
)
{
void
SnakeDataManager
::
setTileWidth
(
Length
tileWidth
)
{
this
->
pImpl
->
errorMessage
=
"Measurement area invalid: size < 3."
;
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
return
;
this
->
pImpl
->
scenario
.
setTileWidth
(
tileWidth
);
}
}
if
(
this
->
pImpl
->
sArea
.
size
()
<
3
)
{
this
->
pImpl
->
errorMessage
=
"Service area invalid: size < 3."
;
return
;
}
void
SnakeDataManager
::
enableRosBridge
()
{
this
->
pImpl
->
rosBridgeEnabeled
=
true
;
}
this
->
pImpl
->
ENUOrigin
=
this
->
pImpl
->
mArea
.
front
();
void
SnakeDataManager
::
disableRosBride
()
{
auto
&
origin
=
this
->
pImpl
->
ENUOrigin
;
this
->
pImpl
->
rosBridgeEnabeled
=
false
;
// Update measurement area.
}
auto
&
mAreaEnu
=
this
->
pImpl
->
scenario
.
measurementArea
();
auto
&
mArea
=
this
->
pImpl
->
mArea
;
mAreaEnu
.
clear
();
for
(
auto
geoVertex
:
mArea
){
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
mAreaEnu
.
outer
().
push_back
(
p
);
}
// Update service area.
void
SnakeDataManager
::
run
()
{
auto
&
sAreaEnu
=
this
->
pImpl
->
scenario
.
serviceArea
();
this
->
pImpl
->
calcInProgress
.
store
(
true
);
auto
&
sArea
=
this
->
pImpl
->
sArea
;
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
sAreaEnu
.
clear
();
auto
onExit
=
[
this
]
{
for
(
auto
geoVertex
:
sArea
){
this
->
pImpl
->
calcInProgress
.
store
(
false
);
snake
::
BoostPoint
p
;
emit
calcInProgressChanged
(
this
->
pImpl
->
calcInProgress
.
load
());
snake
::
toENU
(
origin
,
geoVertex
,
p
);
};
sAreaEnu
.
outer
().
push_back
(
p
);
CommandRAII
<
decltype
(
onExit
)
>
onExitRAII
(
onExit
);
UniqueLock
lk
(
this
->
pImpl
->
mutex
);
this
->
pImpl
->
resetOutput
();
if
(
!
this
->
pImpl
->
precondition
())
return
;
if
(
this
->
pImpl
->
mArea
.
size
()
<
3
)
{
this
->
pImpl
->
errorMessage
=
"Measurement area invalid: size < 3."
;
return
;
}
if
(
this
->
pImpl
->
sArea
.
size
()
<
3
)
{
this
->
pImpl
->
errorMessage
=
"Service area invalid: size < 3."
;
return
;
}
this
->
pImpl
->
ENUOrigin
=
this
->
pImpl
->
mArea
.
front
();
auto
&
origin
=
this
->
pImpl
->
ENUOrigin
;
// Update measurement area.
auto
&
mAreaEnu
=
this
->
pImpl
->
scenario
.
measurementArea
();
auto
&
mArea
=
this
->
pImpl
->
mArea
;
mAreaEnu
.
clear
();
for
(
auto
geoVertex
:
mArea
)
{
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
mAreaEnu
.
outer
().
push_back
(
p
);
}
// Update service area.
auto
&
sAreaEnu
=
this
->
pImpl
->
scenario
.
serviceArea
();
auto
&
sArea
=
this
->
pImpl
->
sArea
;
sAreaEnu
.
clear
();
for
(
auto
geoVertex
:
sArea
)
{
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
sAreaEnu
.
outer
().
push_back
(
p
);
}
// Update corridor.
auto
&
corridorEnu
=
this
->
pImpl
->
scenario
.
corridor
();
auto
&
corridor
=
this
->
pImpl
->
corridor
;
corridor
.
clear
();
for
(
auto
geoVertex
:
corridor
)
{
snake
::
BoostPoint
p
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
corridorEnu
.
outer
().
push_back
(
p
);
}
if
(
!
this
->
pImpl
->
scenario
.
update
())
{
this
->
pImpl
->
errorMessage
=
this
->
pImpl
->
scenario
.
errorString
.
c_str
();
return
;
}
// Asynchronously update flightplan.
std
::
string
errorString
;
auto
future
=
std
::
async
([
this
,
&
errorString
,
&
origin
]
{
snake
::
Angle
alpha
(
-
this
->
pImpl
->
scenario
.
mAreaBoundingBox
().
angle
*
degree
::
degree
);
snake
::
flight_plan
::
Transects
transects
;
transects
.
push_back
(
bg
::
model
::
linestring
<
snake
::
BoostPoint
>
{
this
->
pImpl
->
scenario
.
homePositon
()});
bool
value
=
snake
::
flight_plan
::
transectsFromScenario
(
this
->
pImpl
->
lineDistance
,
this
->
pImpl
->
minTransectLength
,
alpha
,
this
->
pImpl
->
scenario
,
this
->
pImpl
->
progress
,
transects
,
errorString
);
if
(
!
value
)
{
this
->
pImpl
->
errorMessage
=
"Not able to generate transects."
;
return
value
;
}
}
snake
::
flight_plan
::
Transects
transectsRouted
;
// Update corridor.
snake
::
flight_plan
::
Route
route
;
auto
&
corridorEnu
=
this
->
pImpl
->
scenario
.
corridor
();
value
=
auto
&
corridor
=
this
->
pImpl
->
corridor
;
snake
::
flight_plan
::
route
(
this
->
pImpl
->
scenario
.
joinedArea
(),
transects
,
corridor
.
clear
();
transectsRouted
,
route
,
errorString
);
for
(
auto
geoVertex
:
corridor
){
if
(
!
value
)
{
snake
::
BoostPoint
p
;
this
->
pImpl
->
errorMessage
=
"Routing error."
;
snake
::
toENU
(
origin
,
geoVertex
,
p
);
return
value
;
corridorEnu
.
outer
().
push_back
(
p
);
}
}
if
(
!
this
->
pImpl
->
scenario
.
update
()
){
// Store arrival path.
this
->
pImpl
->
errorMessage
=
this
->
pImpl
->
scenario
.
errorString
.
c_str
();
const
auto
&
firstWaypoint
=
transectsRouted
.
front
().
front
();
return
;
long
startIdx
=
0
;
for
(
long
i
=
0
;
i
<
long
(
route
.
size
());
++
i
)
{
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
firstWaypoint
)
{
startIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
}
// Store return path.
// Asynchronously update flightplan.
long
endIdx
=
0
;
qWarning
()
<<
"route calculation missing"
;
const
auto
&
lastWaypoint
=
transectsRouted
.
back
().
back
();
std
::
string
errorString
;
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
auto
future
=
std
::
async
([
this
,
&
errorString
,
&
origin
]{
const
auto
&
boostVertex
=
route
[
i
];
snake
::
Angle
alpha
(
-
this
->
pImpl
->
scenario
.
mAreaBoundingBox
().
angle
*
degree
::
degree
);
if
(
boostVertex
==
lastWaypoint
)
{
snake
::
flight_plan
::
Transects
transects
;
endIdx
=
i
;
transects
.
push_back
(
bg
::
model
::
linestring
<
snake
::
BoostPoint
>
{
break
;
this
->
pImpl
->
scenario
.
homePositon
()
}
});
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
bool
value
=
snake
::
flight_plan
::
transectsFromScenario
(
this
->
pImpl
->
lineDistance
,
QGeoCoordinate
geoVertex
;
this
->
pImpl
->
minTransectLength
,
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
alpha
,
this
->
pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
scenario
,
this
->
pImpl
->
returnPath
.
push_back
(
geoVertex
);
this
->
pImpl
->
progress
,
transects
,
errorString
);
if
(
!
value
)
return
value
;
snake
::
flight_plan
::
Transects
transectsRouted
;
snake
::
flight_plan
::
Route
route
;
value
=
snake
::
flight_plan
::
route
(
this
->
pImpl
->
scenario
.
joinedArea
(),
transects
,
transectsRouted
,
route
,
errorString
);
if
(
!
value
)
return
value
;
// Store arrival path.
const
auto
&
firstWaypoint
=
transectsRouted
.
front
().
front
();
long
startIdx
=
0
;
for
(
long
i
=
0
;
i
<
route
.
size
();
++
i
){
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
firstWaypoint
){
startIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
arrivalPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
arrivalPath
.
push_back
(
geoVertex
);
}
// Store return path.
long
endIdx
=
0
;
const
auto
&
lastWaypoint
=
transectsRouted
.
back
().
back
();
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
){
const
auto
&
boostVertex
=
route
[
i
];
if
(
boostVertex
==
lastWaypoint
){
endIdx
=
i
;
break
;
}
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
returnPathENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
returnPath
.
push_back
(
geoVertex
);
}
// Store waypoints.
for
(
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
){
const
auto
&
boostVertex
=
route
[
i
];
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
this
->
pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
this
->
pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
return
true
;
});
// Continue with storing scenario data in the mean time.
{
// Get tiles.
const
auto
&
tiles
=
this
->
pImpl
->
scenario
.
tiles
();
const
auto
&
centerPoints
=
this
->
pImpl
->
scenario
.
tileCenterPoints
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
SnakeTile
geoTile
;
SnakeTileLocal
enuTile
;
for
(
size_t
i
=
tile
.
outer
().
size
();
i
<
tile
.
outer
().
size
()
-
1
;
++
i
)
{
auto
&
p
=
tile
.
outer
()[
i
];
QPointF
enuVertex
(
p
.
get
<
0
>
(),
p
.
get
<
1
>
());
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
p
,
geoVertex
);
enuTile
.
polygon
().
points
().
push_back
(
enuVertex
);
geoTile
.
push_back
(
geoVertex
);
}
const
auto
&
boostPoint
=
centerPoints
[
i
];
QPointF
enuVertex
(
boostPoint
.
get
<
0
>
(),
boostPoint
.
get
<
1
>
());
QGeoCoordinate
geoVertex
;
snake
::
fromENU
(
origin
,
boostPoint
,
geoVertex
);
geoTile
.
setCenter
(
geoVertex
);
this
->
pImpl
->
tiles
.
polygons
().
push_back
(
geoTile
);
this
->
pImpl
->
tileCenterPoints
.
push_back
(
QVariant
::
fromValue
(
geoVertex
));
this
->
pImpl
->
tilesENU
.
polygons
().
push_back
(
enuTile
);
this
->
pImpl
->
tileCenterPointsENU
.
push_back
(
enuVertex
);
}
}
}
// Store waypoints.
future
.
wait
();
for
(
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
)
{
// Trying to generate flight plan.
const
auto
&
boostVertex
=
route
[
i
];
if
(
!
future
.
get
()
){
QPointF
enuVertex
{
boostVertex
.
get
<
0
>
(),
boostVertex
.
get
<
1
>
()};
// error
QGeoCoordinate
geoVertex
;
this
->
pImpl
->
errorMessage
=
errorString
.
c_str
();
snake
::
fromENU
(
origin
,
boostVertex
,
geoVertex
);
}
else
{
this
->
pImpl
->
waypointsENU
.
push_back
(
enuVertex
);
// Success!!!
this
->
pImpl
->
waypoints
.
push_back
(
geoVertex
);
}
}
}
bool
SnakeImpl
::
doTopicServiceSetup
()
{
using
namespace
ros_bridge
::
messages
;
UniqueLock
lk
(
this
->
mutex
);
if
(
this
->
tilesENU
.
polygons
().
size
()
==
0
)
return
false
;
// Publish snake origin.
this
->
pRosBridge
->
advertiseTopic
(
"/snake/origin"
,
geographic_msgs
::
geo_point
::
messageType
().
c_str
());
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
this
->
pRosBridge
->
advertiseTopic
(
"/snake/tiles"
,
jsk_recognition_msgs
::
polygon_array
::
messageType
().
c_str
());
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
// Subscribe nemo progress.
this
->
pRosBridge
->
subscribe
(
"/nemo/progress"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
UniqueLock
lk
(
this
->
mutex
);
int
requiredSize
=
this
->
tilesENU
.
polygons
().
size
();
auto
&
progressMsg
=
this
->
qProgress
;
if
(
!
nemo_msgs
::
progress
::
fromJson
(
*
pDoc
,
progressMsg
)
||
progressMsg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
// Set progress to default.
progressMsg
.
progress
().
fill
(
0
,
requiredSize
);
// Publish snake origin.
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
this
->
progress
.
clear
();
for
(
const
auto
&
p
:
progressMsg
.
progress
()){
this
->
progress
.
push_back
(
p
);
}
lk
.
unlock
();
emit
this
->
parent
->
nemoProgressChanged
();
});
// Subscribe /nemo/heartbeat.
this
->
pRosBridge
->
subscribe
(
"/nemo/heartbeat"
,
/* callback */
[
this
](
JsonDocUPtr
pDoc
){
UniqueLock
lk
(
this
->
mutex
);
if
(
this
->
timeout
.
isActive
()
)
{
this
->
timeout
.
stop
();
}
auto
&
heartbeatMsg
=
this
->
heartbeat
;
if
(
!
nemo_msgs
::
heartbeat
::
fromJson
(
*
pDoc
,
heartbeatMsg
)
)
{
heartbeatMsg
.
setStatus
(
integral
(
NemoStatus
::
InvalidHeartbeat
));
}
this
->
timeout
.
singleShot
(
TIMEOUT
,
[
this
]{
UniqueLock
lk
(
this
->
mutex
);
this
->
heartbeat
.
setStatus
(
integral
(
NemoStatus
::
Timeout
));
emit
this
->
parent
->
nemoStatusChanged
(
integral
(
NemoStatus
::
Timeout
));
});
emit
this
->
parent
->
nemoStatusChanged
(
heartbeatMsg
.
status
());
});
// Advertise /snake/get_origin.
this
->
pRosBridge
->
advertiseService
(
"/snake/get_origin"
,
"snake_msgs/GetOrigin"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
using
namespace
ros_bridge
::
messages
;
SharedLock
lk
(
this
->
mutex
);
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
auto
&
origin
=
this
->
ENUOrigin
;
rapidjson
::
Value
jOrigin
(
rapidjson
::
kObjectType
);
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
());
lk
.
unlock
();
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
// Advertise /snake/get_tiles.
this
->
pRosBridge
->
advertiseService
(
"/snake/get_tiles"
,
"snake_msgs/GetTiles"
,
[
this
](
JsonDocUPtr
)
->
JsonDocUPtr
{
SharedLock
lk
(
this
->
mutex
);
JsonDocUPtr
pDoc
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
rapidjson
::
Value
jSnakeTiles
(
rapidjson
::
kObjectType
);
bool
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
Q_ASSERT
(
ret
);
(
void
)
ret
;
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
return
pDoc
;
});
return
true
;
return
true
;
}
});
// Continue with storing scenario data in the mean time.
{
SnakeImpl
::
SnakeImpl
(
SnakeDataManager
*
p
)
:
// Get tiles.
rosBridgeEnabeled
(
false
)
const
auto
&
tiles
=
this
->
pImpl
->
scenario
.
tiles
();
,
topicServiceSetupDone
(
false
)
const
auto
&
centerPoints
=
this
->
pImpl
->
scenario
.
tileCenterPoints
();
,
lineDistance
(
1
*
si
::
meter
)
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
,
minTransectLength
(
1
*
si
::
meter
)
const
auto
&
tile
=
tiles
[
i
];
,
calcInProgress
(
false
)
SnakeTile
geoTile
;
,
parent
(
p
)
SnakeTileLocal
enuTile
;
{
for
(
size_t
i
=
tile
.
outer
().
size
();
i
<
tile
.
outer
().
size
()
-
1
;
++
i
)
{
// ROS Bridge.
auto
&
p
=
tile
.
outer
()[
i
];
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
QPointF
enuVertex
(
p
.
get
<
0
>
(),
p
.
get
<
1
>
());
auto
connectionStringFact
=
wimaSettings
->
rosbridgeConnectionString
();
QGeoCoordinate
geoVertex
;
auto
setConnectionString
=
[
connectionStringFact
,
this
]{
snake
::
fromENU
(
origin
,
p
,
geoVertex
);
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
enuTile
.
polygon
().
points
().
push_back
(
enuVertex
);
if
(
ros_bridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
())
){
geoTile
.
push_back
(
geoVertex
);
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
}
else
{
const
auto
&
boostPoint
=
centerPoints
[
i
];
QString
defaultString
(
"localhost:9090"
);
QPointF
enuVertex
(
boostPoint
.
get
<
0
>
(),
boostPoint
.
get
<
1
>
());
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
QGeoCoordinate
geoVertex
;
qgcApp
()
->
showMessage
(
"Resetting connection string to: "
+
defaultString
);
snake
::
fromENU
(
origin
,
boostPoint
,
geoVertex
);
connectionStringFact
->
setRawValue
(
QVariant
(
defaultString
));
// calls this function recursively
geoTile
.
setCenter
(
geoVertex
);
}
this
->
pImpl
->
tilesQml
.
append
(
new
SnakeTile
(
geoTile
));
};
this
->
pImpl
->
tiles
.
polygons
().
push_back
(
geoTile
);
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
this
->
pImpl
->
tileCenterPoints
.
push_back
(
QVariant
::
fromValue
(
geoVertex
));
setConnectionString
();
this
->
pImpl
->
tilesENU
.
polygons
().
push_back
(
enuTile
);
this
->
pImpl
->
tileCenterPointsENU
.
push_back
(
enuVertex
);
// Periodic.
}
connect
(
&
this
->
eventTimer
,
&
QTimer
::
timeout
,
[
this
]{
}
if
(
this
->
rosBridgeEnabeled
)
{
if
(
!
this
->
pRosBridge
->
isRunning
())
{
future
.
wait
();
this
->
pRosBridge
->
start
();
// Trying to generate flight plan.
}
else
if
(
this
->
pRosBridge
->
isRunning
()
if
(
!
future
.
get
())
{
&&
this
->
pRosBridge
->
connected
()
// error
&&
!
this
->
topicServiceSetupDone
)
{
this
->
pImpl
->
errorMessage
=
errorString
.
c_str
();
if
(
this
->
doTopicServiceSetup
()
)
}
else
{
this
->
topicServiceSetupDone
=
true
;
// Success!!!
}
else
if
(
this
->
pRosBridge
->
isRunning
()
}
&&
!
this
->
pRosBridge
->
connected
()
&&
this
->
topicServiceSetupDone
){
this
->
pRosBridge
->
reset
();
this
->
pRosBridge
->
start
();
this
->
topicServiceSetupDone
=
false
;
}
}
else
if
(
this
->
pRosBridge
->
isRunning
()
)
{
this
->
pRosBridge
->
reset
();
this
->
topicServiceSetupDone
=
false
;
}
});
this
->
eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
}
}
src/Wima/Snake/SnakeDataManager.h
View file @
c6e6a519
#pragma once
#pragma once
#include <QGeoCoordinate>
#include <QList>
#include <QObject>
#include <QObject>
#include <QThread>
#include <QThread>
#include <QList>
#include <QGeoCoordinate>
#include "QNemoProgress.h"
#include "QNemoHeartbeat.h"
#include "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "SnakeTiles.h"
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si.hpp>
using
namespace
boost
::
units
;
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
SnakeImpl
;
enum
class
NemoStatus
{
NotConnected
=
0
,
enum
class
NemoStatus
{
Connected
=
1
,
NotConnected
=
0
,
Timeout
=
-
1
,
Connected
=
1
,
InvalidHeartbeat
=
-
2
Timeout
=
-
1
,
InvalidHeartbeat
=
-
2
};
};
class
SnakeDataManager
:
public
QThread
{
class
SnakeDataManager
:
public
QThread
{
Q_OBJECT
Q_OBJECT
public:
class
SnakeImpl
;
using
SnakeImplPtr
=
std
::
unique_ptr
<
SnakeImpl
>
;
using
SnakeImplPtr
=
std
::
unique_ptr
<
SnakeImpl
>
;
SnakeDataManager
(
QObject
*
parent
=
nullptr
);
public:
~
SnakeDataManager
()
override
;
SnakeDataManager
(
QObject
*
parent
=
nullptr
);
~
SnakeDataManager
()
override
;
void
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
);
void
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
);
void
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
);
void
setMeasurementArea
(
const
QList
<
QGeoCoordinate
>
&
measurementArea
);
const
QmlObjectListModel
*
tiles
()
const
;
void
setServiceArea
(
const
QList
<
QGeoCoordinate
>
&
serviceArea
);
QVariantList
tileCenterPoints
()
const
;
void
setCorridor
(
const
QList
<
QGeoCoordinate
>
&
corridor
);
QNemoProgress
nemoProgress
()
const
;
int
nemoStatus
()
const
;
bool
calcInProgress
()
const
;
QString
errorMessage
()
const
;
bool
success
()
const
;
QNemoProgress
nemoProgress
()
;
QVector
<
QGeoCoordinate
>
waypoints
()
const
;
int
nemoStatus
()
;
QVector
<
QGeoCoordinate
>
arrivalPath
()
const
;
bool
calcInProgress
()
;
QVector
<
QGeoCoordinate
>
returnPath
()
const
;
Length
lineDistance
()
const
;
Length
lineDistance
()
const
;
void
setLineDistance
(
Length
lineDistance
);
void
setLineDistance
(
Length
lineDistance
);
Length
minTransectLength
()
const
;
Length
minTransectLength
()
const
;
void
setMinTransectLength
(
Length
minTransectLength
);
void
setMinTransectLength
(
Length
minTransectLength
);
Area
minTileArea
()
const
;
Area
minTileArea
()
const
;
void
setMinTileArea
(
Area
minTileArea
);
void
setMinTileArea
(
Area
minTileArea
);
Length
tileHeight
()
const
;
Length
tileHeight
()
const
;
void
setTileHeight
(
Length
tileHeight
);
void
setTileHeight
(
Length
tileHeight
);
Length
tileWidth
()
const
;
Length
tileWidth
()
const
;
void
setTileWidth
(
Length
tileWidth
);
void
setTileWidth
(
Length
tileWidth
);
void
enableRosBridge
();
void
enableRosBridge
();
void
disableRosBride
();
void
disableRosBride
();
signals:
signals:
void
nemoProgressChanged
();
void
nemoProgressChanged
();
void
nemoStatusChanged
(
int
status
);
void
nemoStatusChanged
(
int
status
);
void
calcInProgressChanged
(
bool
inProgress
);
void
calcInProgressChanged
(
bool
inProgress
);
protected:
protected:
void
run
()
override
;
void
run
()
override
;
private:
private:
SnakeImplPtr
pImpl
;
SnakeImplPtr
pImpl
;
};
};
src/Wima/WimaController.cc
View file @
c6e6a519
...
@@ -2,37 +2,38 @@
...
@@ -2,37 +2,38 @@
#include "utilities.h"
#include "utilities.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoProgress.h"
#include "QVector3D"
#include "QVector3D"
#include <QScopedPointer>
#include <QScopedPointer>
#include <QtConcurrentRun>
#include <memory>
#include <memory>
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_MAX_ATTEMPTS 3
// times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define EVENT_TIMER_INTERVAL 50
// ms
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems"
;
const
char
*
WimaController
::
areaItemsName
=
"AreaItems
"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController
"
;
const
char
*
WimaController
::
missionItemsName
=
"MissionItems
"
;
const
char
*
WimaController
::
enableWimaControllerName
=
"EnableWimaController
"
;
const
char
*
WimaController
::
settingsGroup
=
"WimaController
"
;
const
char
*
WimaController
::
overlapWaypointsName
=
"OverlapWaypoints
"
;
const
char
*
WimaController
::
enableWimaControllerName
=
"EnableWimaController
"
;
const
char
*
WimaController
::
maxWaypointsPerPhaseName
=
"MaxWaypointsPerPhase
"
;
const
char
*
WimaController
::
overlapWaypointsName
=
"OverlapWaypoints
"
;
const
char
*
WimaController
::
startWaypointIndexName
=
"StartWaypointIndex
"
;
const
char
*
WimaController
::
maxWaypointsPerPhaseName
=
"MaxWaypointsPerPhase
"
;
const
char
*
WimaController
::
showAllMissionItemsName
=
"ShowAllMissionItems
"
;
const
char
*
WimaController
::
startWaypointIndexName
=
"StartWaypointIndex"
;
const
char
*
WimaController
::
showCurrentMissionItemsName
=
const
char
*
WimaController
::
showAllMissionItemsName
=
"ShowAll
MissionItems"
;
"ShowCurrent
MissionItems"
;
const
char
*
WimaController
::
showCurrentMissionItemsName
=
"ShowCurrentMissionItems
"
;
const
char
*
WimaController
::
flightSpeedName
=
"FlightSpeed
"
;
const
char
*
WimaController
::
flightSpeedName
=
"Flight
Speed"
;
const
char
*
WimaController
::
arrivalReturnSpeedName
=
"ArrivalReturn
Speed"
;
const
char
*
WimaController
::
arrivalReturnSpeedName
=
"ArrivalReturnSpeed
"
;
const
char
*
WimaController
::
altitudeName
=
"Altitude
"
;
const
char
*
WimaController
::
altitudeName
=
"Altitude
"
;
const
char
*
WimaController
::
snakeTileWidthName
=
"SnakeTileWidth
"
;
const
char
*
WimaController
::
snakeTileWidthName
=
"SnakeTileWidth
"
;
const
char
*
WimaController
::
snakeTileHeightName
=
"SnakeTileHeight
"
;
const
char
*
WimaController
::
snakeTileHeightName
=
"SnakeTileHeight
"
;
const
char
*
WimaController
::
snakeMinTileAreaName
=
"SnakeMinTileArea
"
;
const
char
*
WimaController
::
snakeMinTileAreaName
=
"SnakeMinTileArea
"
;
const
char
*
WimaController
::
snakeLineDistanceName
=
"SnakeLineDistance
"
;
const
char
*
WimaController
::
snakeLineDistanceName
=
"SnakeLineDistance"
;
const
char
*
WimaController
::
snakeMinTransectLengthName
=
const
char
*
WimaController
::
snakeMinTransectLengthName
=
"SnakeMinTransectLength"
;
"SnakeMinTransectLength"
;
WimaController
::
StatusMap
WimaController
::
_nemoStatusMap
{
WimaController
::
StatusMap
WimaController
::
_nemoStatusMap
{
std
::
make_pair
<
int
,
QString
>
(
0
,
"No Heartbeat"
),
std
::
make_pair
<
int
,
QString
>
(
0
,
"No Heartbeat"
),
...
@@ -40,909 +41,754 @@ WimaController::StatusMap WimaController::_nemoStatusMap{
...
@@ -40,909 +41,754 @@ WimaController::StatusMap WimaController::_nemoStatusMap{
std
::
make_pair
<
int
,
QString
>
(
-
1
,
"Timeout"
),
std
::
make_pair
<
int
,
QString
>
(
-
1
,
"Timeout"
),
std
::
make_pair
<
int
,
QString
>
(
-
2
,
"Error"
)};
std
::
make_pair
<
int
,
QString
>
(
-
2
,
"Error"
)};
using
namespace
snake
;
WimaController
::
WimaController
(
QObject
*
parent
)
WimaController
::
WimaController
(
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
),
_joinedArea
(),
_measurementArea
(),
_serviceArea
(),
,
_joinedArea
()
_corridor
(),
_localPlanDataValid
(
false
),
,
_measurementArea
()
_areaInterface
(
&
_measurementArea
,
&
_serviceArea
,
&
_corridor
,
,
_serviceArea
()
&
_joinedArea
),
,
_corridor
()
_WMSettings
(),
_defaultWM
(
_WMSettings
,
_areaInterface
),
,
_localPlanDataValid
(
false
)
_snakeWM
(
_WMSettings
,
_areaInterface
),
,
_areaInterface
(
&
_measurementArea
,
&
_serviceArea
,
&
_corridor
,
&
_joinedArea
)
_rtlWM
(
_WMSettings
,
_areaInterface
),
,
_managerSettings
()
_currentWM
(
&
_defaultWM
),
_WMList
{
&
_defaultWM
,
&
_snakeWM
,
&
_rtlWM
},
,
_defaultManager
(
_managerSettings
,
_areaInterface
)
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
,
_snakeManager
(
_managerSettings
,
_areaInterface
)
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
)),
,
_rtlManager
(
_managerSettings
,
_areaInterface
)
_enableWimaController
(
settingsGroup
,
,
_currentManager
(
&
_defaultManager
)
_metaDataMap
[
enableWimaControllerName
]),
,
_managerList
{
&
_defaultManager
,
&
_snakeManager
,
&
_rtlManager
}
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
]),
,
_metaDataMap
(
_maxWaypointsPerPhase
(
settingsGroup
,
FactMetaData
::
createMapFromJsonFile
(
_metaDataMap
[
maxWaypointsPerPhaseName
]),
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
))
_nextPhaseStartWaypointIndex
(
settingsGroup
,
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
])
_metaDataMap
[
startWaypointIndexName
]),
,
_overlapWaypoints
(
settingsGroup
,
_metaDataMap
[
overlapWaypointsName
])
_showAllMissionItems
(
settingsGroup
,
,
_maxWaypointsPerPhase
(
settingsGroup
,
_metaDataMap
[
maxWaypointsPerPhaseName
])
_metaDataMap
[
showAllMissionItemsName
]),
,
_nextPhaseStartWaypointIndex
(
settingsGroup
,
_metaDataMap
[
startWaypointIndexName
])
_showCurrentMissionItems
(
settingsGroup
,
,
_showAllMissionItems
(
settingsGroup
,
_metaDataMap
[
showAllMissionItemsName
])
_metaDataMap
[
showCurrentMissionItemsName
]),
,
_showCurrentMissionItems
(
settingsGroup
,
_metaDataMap
[
showCurrentMissionItemsName
])
_flightSpeed
(
settingsGroup
,
_metaDataMap
[
flightSpeedName
]),
,
_flightSpeed
(
settingsGroup
,
_metaDataMap
[
flightSpeedName
])
_arrivalReturnSpeed
(
settingsGroup
,
_metaDataMap
[
arrivalReturnSpeedName
]),
,
_arrivalReturnSpeed
(
settingsGroup
,
_metaDataMap
[
arrivalReturnSpeedName
])
_altitude
(
settingsGroup
,
_metaDataMap
[
altitudeName
]),
,
_altitude
(
settingsGroup
,
_metaDataMap
[
altitudeName
])
_snakeTileWidth
(
settingsGroup
,
_metaDataMap
[
snakeTileWidthName
]),
,
_snakeTileWidth
(
settingsGroup
,
_metaDataMap
[
snakeTileWidthName
])
_snakeTileHeight
(
settingsGroup
,
_metaDataMap
[
snakeTileHeightName
]),
,
_snakeTileHeight
(
settingsGroup
,
_metaDataMap
[
snakeTileHeightName
])
_snakeMinTileArea
(
settingsGroup
,
_metaDataMap
[
snakeMinTileAreaName
]),
,
_snakeMinTileArea
(
settingsGroup
,
_metaDataMap
[
snakeMinTileAreaName
])
_snakeLineDistance
(
settingsGroup
,
_metaDataMap
[
snakeLineDistanceName
]),
,
_snakeLineDistance
(
settingsGroup
,
_metaDataMap
[
snakeLineDistanceName
])
_snakeMinTransectLength
(
settingsGroup
,
,
_snakeMinTransectLength
(
settingsGroup
,
_metaDataMap
[
snakeMinTransectLengthName
])
_metaDataMap
[
snakeMinTransectLengthName
]),
,
_lowBatteryHandlingTriggered
(
false
)
_lowBatteryHandlingTriggered
(
false
),
_measurementPathLength
(
-
1
),
,
_measurementPathLength
(
-
1
)
_currentDM
(
&
_emptyDM
),
,
_snakeCalcInProgress
(
false
)
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
{
,
_nemoHeartbeat
(
0
/*status: not connected*/
)
,
_fallbackStatus
(
0
/*status: not connected*/
)
// Set up facts.
,
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
_showAllMissionItems
.
setRawValue
(
true
);
,
_snakeTicker
(
EVENT_TIMER_INTERVAL
,
200
/*ms*/
)
_showCurrentMissionItems
.
setRawValue
(
true
);
,
_nemoTimeoutTicker
(
EVENT_TIMER_INTERVAL
,
5000
/*ms*/
)
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
,
_topicServiceSetupDone
(
false
)
&
WimaController
::
_updateOverlap
);
{
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
// Set up facts.
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
_showAllMissionItems
.
setRawValue
(
true
);
&
WimaController
::
_setStartIndex
);
_showCurrentMissionItems
.
setRawValue
(
true
);
connect
(
&
_flightSpeed
,
&
Fact
::
rawValueChanged
,
this
,
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
&
WimaController
::
_updateflightSpeed
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
connect
(
&
_arrivalReturnSpeed
,
&
Fact
::
rawValueChanged
,
this
,
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
&
WimaController
::
_updateArrivalReturnSpeed
);
connect
(
&
_flightSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateflightSpeed
);
connect
(
&
_altitude
,
&
Fact
::
rawValueChanged
,
this
,
connect
(
&
_arrivalReturnSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateArrivalReturnSpeed
);
&
WimaController
::
_updateAltitude
);
connect
(
&
_altitude
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateAltitude
);
// Init waypoint managers.
// Init waypoint managers.
bool
value
;
bool
value
;
size_t
overlap
=
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
);
size_t
overlap
=
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
Q_ASSERT
(
value
);
size_t
N
=
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
);
size_t
N
=
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
Q_ASSERT
(
value
);
size_t
startIdx
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
);
size_t
startIdx
=
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
);
Q_ASSERT
(
value
);
Q_ASSERT
(
value
);
(
void
)
value
;
(
void
)
value
;
for
(
auto
manager
:
_WMList
)
{
for
(
auto
manager
:
_managerList
){
manager
->
setOverlap
(
overlap
);
manager
->
setOverlap
(
overlap
);
manager
->
setN
(
N
);
manager
->
setN
(
N
);
manager
->
setStartIndex
(
startIdx
);
manager
->
setStartIndex
(
startIdx
);
}
}
// Periodic.
// Periodic.
connect
(
&
_eventTimer
,
&
QTimer
::
timeout
,
this
,
connect
(
&
_eventTimer
,
&
QTimer
::
timeout
,
this
,
&
WimaController
::
_eventTimerHandler
);
&
WimaController
::
_eventTimerHandler
);
//_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
_eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
_eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
// Snake Data Manager.
// Snake Worker Thread.
connect
(
_currentDM
,
&
SnakeDataManager
::
finished
,
this
,
connect
(
&
_snakeDataManager
,
&
SnakeDataManager
::
finished
,
this
,
&
WimaController
::
_snakeStoreWorkerResults
);
&
WimaController
::
_DMFinishedHandler
);
connect
(
this
,
&
WimaController
::
nemoProgressChanged
,
this
,
&
WimaController
::
_initStartSnakeWorker
);
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoProgressChanged
,
this
,
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeDataManager
,
&
SnakeDataManager
::
quit
);
&
WimaController
::
_progressChangedHandler
);
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
// Snake.
&
WimaController
::
nemoStatusChanged
);
auto
configRosBridge
=
[
this
]{
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
if
(
this
->
_enableSnake
.
rawValue
().
toBool
()
){
&
WimaController
::
nemoStatusStringChanged
);
this
->
_snakeDataManager
.
enableRosBridge
();
}
else
{
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_snakeDM
,
&
SnakeDataManager
::
quit
);
this
->
_snakeDataManager
.
disableRosBride
();
connect
(
this
,
&
QObject
::
destroyed
,
&
this
->
_emptyDM
,
&
SnakeDataManager
::
quit
);
}
};
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
configRosBridge
);
&
WimaController
::
_enableSnakeChangedHandler
);
configRosBridge
();
_enableSnakeChangedHandler
();
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_switchSnakeManager
);
// Snake Waypoint Manager.
_switchSnakeManager
(
_enableSnake
.
rawValue
());
connect
(
&
_enableSnake
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_switchToSnakeWaypointManager
);
_switchToSnakeWaypointManager
(
_enableSnake
.
rawValue
());
}
}
PlanMasterController
*
WimaController
::
masterController
()
{
PlanMasterController
*
WimaController
::
masterController
()
{
return
_masterController
;
return
_masterController
;
}
}
MissionController
*
WimaController
::
missionController
()
{
MissionController
*
WimaController
::
missionController
()
{
return
_missionController
;
return
_missionController
;
}
}
QmlObjectListModel
*
WimaController
::
visualItems
()
{
QmlObjectListModel
*
WimaController
::
visualItems
()
{
return
&
_areas
;
}
return
&
_areas
;
}
QmlObjectListModel
*
WimaController
::
missionItems
()
{
QmlObjectListModel
*
WimaController
::
missionItems
()
{
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentManager
->
missionItems
());
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentWM
->
missionItems
());
}
}
QmlObjectListModel
*
WimaController
::
currentMissionItems
()
{
QmlObjectListModel
*
WimaController
::
currentMissionItems
()
{
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentManager
->
currentMissionItems
());
return
const_cast
<
QmlObjectListModel
*>
(
&
_currentWM
->
currentMissionItems
());
}
}
QVariantList
WimaController
::
waypointPath
()
QVariantList
WimaController
::
waypointPath
()
{
{
return
const_cast
<
QVariantList
&>
(
_currentWM
->
waypointsVariant
());
return
const_cast
<
QVariantList
&>
(
_currentManager
->
waypointsVariant
());
}
}
QVariantList
WimaController
::
currentWaypointPath
()
QVariantList
WimaController
::
currentWaypointPath
()
{
{
return
const_cast
<
QVariantList
&>
(
_currentWM
->
currentWaypointsVariant
());
return
const_cast
<
QVariantList
&>
(
_currentManager
->
currentWaypointsVariant
());
}
}
Fact
*
WimaController
::
enableWimaController
()
{
Fact
*
WimaController
::
enableWimaController
()
{
return
&
_enableWimaController
;
}
return
&
_enableWimaController
;
}
Fact
*
WimaController
::
overlapWaypoints
()
{
Fact
*
WimaController
::
overlapWaypoints
()
{
return
&
_overlapWaypoints
;
}
return
&
_overlapWaypoints
;
}
Fact
*
WimaController
::
maxWaypointsPerPhase
()
{
Fact
*
WimaController
::
maxWaypointsPerPhase
()
{
return
&
_maxWaypointsPerPhase
;
}
return
&
_maxWaypointsPerPhase
;
}
Fact
*
WimaController
::
startWaypointIndex
()
{
Fact
*
WimaController
::
startWaypointIndex
()
{
return
&
_nextPhaseStartWaypointIndex
;
return
&
_nextPhaseStartWaypointIndex
;
}
}
Fact
*
WimaController
::
showAllMissionItems
()
{
Fact
*
WimaController
::
showAllMissionItems
()
{
return
&
_showAllMissionItems
;
}
return
&
_showAllMissionItems
;
}
Fact
*
WimaController
::
showCurrentMissionItems
()
{
Fact
*
WimaController
::
showCurrentMissionItems
()
{
return
&
_showCurrentMissionItems
;
return
&
_showCurrentMissionItems
;
}
}
Fact
*
WimaController
::
flightSpeed
()
{
Fact
*
WimaController
::
flightSpeed
()
{
return
&
_flightSpeed
;
}
return
&
_flightSpeed
;
}
Fact
*
WimaController
::
arrivalReturnSpeed
()
{
Fact
*
WimaController
::
arrivalReturnSpeed
()
{
return
&
_arrivalReturnSpeed
;
}
return
&
_arrivalReturnSpeed
;
}
Fact
*
WimaController
::
altitude
()
{
Fact
*
WimaController
::
altitude
()
{
return
&
_altitude
;
}
return
&
_altitude
;
}
QmlObjectListModel
*
WimaController
::
snakeTiles
()
QmlObjectListModel
*
WimaController
::
snakeTiles
()
{
{
return
const_cast
<
QmlObjectListModel
*>
(
this
->
_snakeDM
.
tiles
());
static
QmlObjectListModel
list
;
qWarning
()
<<
"using snake tile dummy"
;
return
&
list
;
}
}
QVariantList
WimaController
::
snakeTileCenterPoints
()
QVariantList
WimaController
::
snakeTileCenterPoints
()
{
{
return
_currentDM
->
tileCenterPoints
();
qWarning
()
<<
"using snakeTileCenterPoints dummy"
;
return
QVariantList
();
}
}
QVector
<
int
>
WimaController
::
nemoProgress
()
QVector
<
int
>
WimaController
::
nemoProgress
()
{
{
return
_currentDM
->
nemoProgress
().
progress
();
qWarning
()
<<
"using nemoProgress dummy"
;
return
QVector
<
int
>
();
}
}
double
WimaController
::
phaseDistance
()
const
double
WimaController
::
phaseDistance
()
const
{
{
qWarning
()
<<
"using phaseDistance dummy"
;
return
0.0
;
return
0.0
;
}
}
double
WimaController
::
phaseDuration
()
const
double
WimaController
::
phaseDuration
()
const
{
{
qWarning
()
<<
"using phaseDuration dummy"
;
return
0.0
;
return
0.0
;
}
}
int
WimaController
::
nemoStatus
()
const
int
WimaController
::
nemoStatus
()
const
{
return
_currentDM
->
nemoStatus
();
}
{
qWarning
()
<<
"using nemoStatus dummy"
;
return
0
;
}
QString
WimaController
::
nemoStatusString
()
const
QString
WimaController
::
nemoStatusString
()
const
{
{
return
_nemoStatusMap
.
at
(
nemoStatus
());
return
_nemoStatusMap
.
at
(
nemoStatus
());
}
}
bool
WimaController
::
snakeCalcInProgress
()
const
bool
WimaController
::
snakeCalcInProgress
()
const
{
{
return
_currentDM
->
calcInProgress
();
qWarning
()
<<
"using snakeCalcInProgress dummy"
;
return
false
;
}
}
void
WimaController
::
setMasterController
(
PlanMasterController
*
masterC
)
void
WimaController
::
setMasterController
(
PlanMasterController
*
masterC
)
{
{
_masterController
=
masterC
;
_masterController
=
masterC
;
_WMSettings
.
setMasterController
(
masterC
);
_managerSettings
.
setMasterController
(
masterC
);
emit
masterControllerChanged
();
emit
masterControllerChanged
();
}
}
void
WimaController
::
setMissionController
(
MissionController
*
missionC
)
void
WimaController
::
setMissionController
(
MissionController
*
missionC
)
{
{
_missionController
=
missionC
;
_missionController
=
missionC
;
_WMSettings
.
setMissionController
(
missionC
);
_managerSettings
.
setMissionController
(
missionC
);
emit
missionControllerChanged
();
emit
missionControllerChanged
();
}
}
void
WimaController
::
nextPhase
()
void
WimaController
::
nextPhase
()
{
_calcNextPhase
();
}
{
_calcNextPhase
();
}
void
WimaController
::
previousPhase
()
void
WimaController
::
previousPhase
()
{
{
if
(
!
_currentWM
->
previous
())
{
if
(
!
_currentManager
->
previous
()
)
{
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
}
}
emit
missionItemsChanged
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
emit
waypointPathChanged
();
}
}
void
WimaController
::
resetPhase
()
void
WimaController
::
resetPhase
()
{
{
if
(
!
_currentWM
->
reset
())
{
if
(
!
_currentManager
->
reset
()
)
{
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
}
}
emit
missionItemsChanged
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
emit
waypointPathChanged
();
}
}
void
WimaController
::
requestSmartRTL
()
void
WimaController
::
requestSmartRTL
()
{
{
QString
errorString
(
"Smart RTL requested. "
);
QString
errorString
(
"Smart RTL requested. "
);
if
(
!
_checkSmartRTLPreCondition
(
errorString
))
{
if
(
!
_checkSmartRTLPreCondition
(
errorString
)
){
qgcApp
()
->
showMessage
(
errorString
);
qgcApp
()
->
showMessage
(
errorString
);
return
;
return
;
}
}
emit
smartRTLRequestConfirm
();
emit
smartRTLRequestConfirm
();
}
}
bool
WimaController
::
upload
()
bool
WimaController
::
upload
()
{
{
auto
&
currentMissionItems
=
_defaultWM
.
currentMissionItems
();
auto
&
currentMissionItems
=
_defaultManager
.
currentMissionItems
();
if
(
!
_serviceArea
.
containsCoordinate
(
if
(
!
_serviceArea
.
containsCoordinate
(
_masterController
->
managerVehicle
()
->
coordinate
())
_masterController
->
managerVehicle
()
->
coordinate
())
&&
&&
currentMissionItems
.
count
()
>
0
)
{
currentMissionItems
.
count
()
>
0
)
{
emit
forceUploadConfirm
();
emit
forceUploadConfirm
();
return
false
;
return
false
;
}
}
return
forceUpload
();
return
forceUpload
();
}
}
bool
WimaController
::
forceUpload
()
bool
WimaController
::
forceUpload
()
{
{
auto
&
currentMissionItems
=
_defaultWM
.
currentMissionItems
();
auto
&
currentMissionItems
=
_defaultManager
.
currentMissionItems
();
if
(
currentMissionItems
.
count
()
<
1
)
if
(
currentMissionItems
.
count
()
<
1
)
return
false
;
return
false
;
_missionController
->
removeAll
();
_missionController
->
removeAll
();
// Set homeposition of settingsItem.
// Set homeposition of settingsItem.
QmlObjectListModel
*
visuals
=
_missionController
->
visualItems
();
QmlObjectListModel
*
visuals
=
_missionController
->
visualItems
();
MissionSettingsItem
*
settingsItem
=
visuals
->
value
<
MissionSettingsItem
*>
(
0
);
MissionSettingsItem
*
settingsItem
=
visuals
->
value
<
MissionSettingsItem
*>
(
0
);
if
(
settingsItem
==
nullptr
)
{
if
(
settingsItem
==
nullptr
)
{
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
qWarning
(
"WimaController::updateCurrentMissionItems(): nullptr"
);
qWarning
(
"WimaController::updateCurrentMissionItems(): nullptr"
);
return
false
;
return
false
;
}
}
settingsItem
->
setCoordinate
(
_manager
Settings
.
homePosition
());
settingsItem
->
setCoordinate
(
_WM
Settings
.
homePosition
());
// Copy mission items to _missionController.
// Copy mission items to _missionController.
for
(
int
i
=
1
;
i
<
currentMissionItems
.
count
();
i
++
)
{
for
(
int
i
=
1
;
i
<
currentMissionItems
.
count
();
i
++
)
{
auto
*
item
=
currentMissionItems
.
value
<
const
SimpleMissionItem
*>
(
i
);
auto
*
item
=
currentMissionItems
.
value
<
const
SimpleMissionItem
*>
(
i
);
_missionController
->
insertSimpleMissionItem
(
*
item
,
visuals
->
count
());
_missionController
->
insertSimpleMissionItem
(
*
item
,
visuals
->
count
());
}
}
_masterController
->
sendToVehicle
();
_masterController
->
sendToVehicle
();
return
true
;
return
true
;
}
}
void
WimaController
::
removeFromVehicle
()
void
WimaController
::
removeFromVehicle
()
{
{
_masterController
->
removeAllFromVehicle
();
_masterController
->
removeAllFromVehicle
();
_missionController
->
removeAll
();
_missionController
->
removeAll
();
}
}
void
WimaController
::
executeSmartRTL
()
void
WimaController
::
executeSmartRTL
()
{
{
forceUpload
();
forceUpload
();
masterController
()
->
managerVehicle
()
->
startMission
();
masterController
()
->
managerVehicle
()
->
startMission
();
}
}
void
WimaController
::
initSmartRTL
()
void
WimaController
::
initSmartRTL
()
{
_initSmartRTL
();
}
{
_initSmartRTL
();
}
void
WimaController
::
removeVehicleTrajectoryHistory
()
void
WimaController
::
removeVehicleTrajectoryHistory
()
{
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
managerVehicle
->
trajectoryPoints
()
->
clear
();
managerVehicle
->
trajectoryPoints
()
->
clear
();
}
}
bool
WimaController
::
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
bool
WimaController
::
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
QVector
<
QGeoCoordinate
>
&
path
)
{
{
using
namespace
GeoUtilities
;
using
namespace
GeoUtilities
;
using
namespace
PolygonCalculus
;
using
namespace
PolygonCalculus
;
QPolygonF
polygon2D
;
QPolygonF
polygon2D
;
toCartesianList
(
_joinedArea
.
coordinateList
(),
/*origin*/
start
,
polygon2D
);
toCartesianList
(
_joinedArea
.
coordinateList
(),
/*origin*/
start
,
polygon2D
);
QPointF
start2D
(
0
,
0
);
QPointF
start2D
(
0
,
0
);
QPointF
end2D
;
QPointF
end2D
;
toCartesian
(
destination
,
start
,
end2D
);
toCartesian
(
destination
,
start
,
end2D
);
QVector
<
QPointF
>
path2D
;
QVector
<
QPointF
>
path2D
;
bool
retVal
=
PolygonCalculus
::
shortestPath
(
polygon2D
,
start2D
,
end2D
,
path2D
);
toGeoList
(
path2D
,
/*origin*/
start
,
path
);
return
retVal
;
}
bool
WimaController
::
setWimaPlanData
(
const
WimaPlanData
&
planData
)
{
// reset visual items
_areas
.
clear
();
_defaultManager
.
clear
();
_snakeManager
.
clear
();
_snakeTiles
.
polygons
().
clear
();
_snakeTilesLocal
.
polygons
().
clear
();
_snakeTileCenterPoints
.
clear
();
emit
visualItemsChanged
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
_localPlanDataValid
=
false
;
// extract list with WimaAreas
QList
<
const
WimaAreaData
*>
areaList
=
planData
.
areaList
();
int
areaCounter
=
0
;
bool
retVal
=
const
int
numAreas
=
4
;
// extract only numAreas Areas, if there are more they are invalid and ignored
PolygonCalculus
::
shortestPath
(
polygon2D
,
start2D
,
end2D
,
path2D
);
for
(
int
i
=
0
;
i
<
areaList
.
size
();
i
++
)
{
toGeoList
(
path2D
,
/*origin*/
start
,
path
);
const
WimaAreaData
*
areaData
=
areaList
[
i
];
if
(
areaData
->
type
()
==
WimaServiceAreaData
::
typeString
)
{
// is it a service area?
return
retVal
;
_serviceArea
=
*
qobject_cast
<
const
WimaServiceAreaData
*>
(
areaData
);
}
areaCounter
++
;
_areas
.
append
(
&
_serviceArea
);
continue
;
}
if
(
areaData
->
type
()
==
WimaMeasurementAreaData
::
typeString
)
{
// is it a measurement area?
_measurementArea
=
*
qobject_cast
<
const
WimaMeasurementAreaData
*>
(
areaData
);
areaCounter
++
;
_areas
.
append
(
&
_measurementArea
);
continue
;
bool
WimaController
::
setWimaPlanData
(
const
WimaPlanData
&
planData
)
{
}
// reset visual items
_areas
.
clear
();
_defaultWM
.
clear
();
_snakeWM
.
clear
();
if
(
areaData
->
type
()
==
WimaCorridorData
::
typeString
)
{
// is it a corridor?
emit
visualItemsChanged
();
_corridor
=
*
qobject_cast
<
const
WimaCorridorData
*>
(
areaData
);
emit
missionItemsChanged
();
areaCounter
++
;
emit
currentMissionItemsChanged
();
//_visualItems.append(&_corridor); // not needed
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
continue
;
_localPlanDataValid
=
false
;
}
if
(
areaData
->
type
()
==
WimaJoinedAreaData
::
typeString
)
{
// is it a corridor?
// extract list with WimaAreas
_joinedArea
=
*
qobject_cast
<
const
WimaJoinedAreaData
*>
(
areaData
);
QList
<
const
WimaAreaData
*>
areaList
=
planData
.
areaList
();
areaCounter
++
;
_areas
.
append
(
&
_joinedArea
);
continue
;
int
areaCounter
=
0
;
}
const
int
numAreas
=
4
;
// extract only numAreas Areas, if there are more they
// are invalid and ignored
for
(
int
i
=
0
;
i
<
areaList
.
size
();
i
++
)
{
const
WimaAreaData
*
areaData
=
areaList
[
i
];
if
(
areaCounter
>=
numAreas
)
if
(
areaData
->
type
()
==
break
;
WimaServiceAreaData
::
typeString
)
{
// is it a service area?
}
_serviceArea
=
*
qobject_cast
<
const
WimaServiceAreaData
*>
(
areaData
);
areaCounter
++
;
_areas
.
append
(
&
_serviceArea
);
if
(
areaCounter
!=
numAreas
)
{
continue
;
Q_ASSERT
(
false
);
return
false
;
}
}
emit
visualItemsChanged
();
if
(
areaData
->
type
()
==
WimaMeasurementAreaData
::
typeString
)
{
// is it a measurement area?
// extract mission items
_measurementArea
=
QList
<
MissionItem
>
tempMissionItems
=
planData
.
missionItems
();
*
qobject_cast
<
const
WimaMeasurementAreaData
*>
(
areaData
);
if
(
tempMissionItems
.
size
()
<
1
)
{
areaCounter
++
;
qWarning
(
"WimaController: Mission items from WimaPlaner empty!"
);
_areas
.
append
(
&
_measurementArea
);
return
false
;
}
for
(
auto
item
:
tempMissionItems
)
{
continue
;
_defaultManager
.
push_back
(
item
.
coordinate
());
}
}
_managerSettings
.
setHomePosition
(
QGeoCoordinate
(
_serviceArea
.
center
().
latitude
(),
if
(
areaData
->
type
()
==
WimaCorridorData
::
typeString
)
{
// is it a corridor?
_serviceArea
.
center
().
longitude
(),
_corridor
=
*
qobject_cast
<
const
WimaCorridorData
*>
(
areaData
);
0
)
);
areaCounter
++
;
//_visualItems.append(&_corridor); // not needed
if
(
!
_defaultManager
.
reset
()
){
continue
;
Q_ASSERT
(
false
);
return
false
;
}
}
emit
missionItemsChanged
();
if
(
areaData
->
type
()
==
emit
currentMissionItemsChanged
();
WimaJoinedAreaData
::
typeString
)
{
// is it a corridor?
emit
waypointPathChanged
();
_joinedArea
=
*
qobject_cast
<
const
WimaJoinedAreaData
*>
(
areaData
);
emit
currentWaypointPathChanged
();
areaCounter
++
;
_areas
.
append
(
&
_joinedArea
);
// Initialize _scenario.
Area
mArea
;
for
(
auto
variant
:
_measurementArea
.
path
()){
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
mArea
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
mArea
.
type
=
AreaType
::
MeasurementArea
;
Area
sArea
;
continue
;
for
(
auto
variant
:
_serviceArea
.
path
()){
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
sArea
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
}
sArea
.
type
=
AreaType
::
ServiceArea
;
Area
corridor
;
if
(
areaCounter
>=
numAreas
)
for
(
auto
variant
:
_corridor
.
path
()){
break
;
QGeoCoordinate
c
{
variant
.
value
<
QGeoCoordinate
>
()};
}
corridor
.
geoPolygon
.
push_back
(
GeoPoint2D
{
c
.
latitude
(),
c
.
longitude
()});
}
corridor
.
type
=
AreaType
::
Corridor
;
_scenario
.
addArea
(
mArea
);
_scenario
.
addArea
(
sArea
);
_scenario
.
addArea
(
corridor
);
// Check if scenario is defined.
if
(
!
_scenario
.
defined
(
_snakeTileWidth
.
rawValue
().
toUInt
(),
_snakeTileHeight
.
rawValue
().
toUInt
(),
_snakeMinTileArea
.
rawValue
().
toUInt
())
)
{
Q_ASSERT
(
false
);
return
false
;
}
{
// Get tiles and origin.
auto
origin
=
_scenario
.
getOrigin
();
_snakeOrigin
.
setLatitude
(
origin
[
0
]);
_snakeOrigin
.
setLongitude
(
origin
[
1
]);
_snakeOrigin
.
setAltitude
(
origin
[
2
]);
const
auto
&
tiles
=
_scenario
.
getTiles
();
const
auto
&
cps
=
_scenario
.
getTileCenterPoints
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
SnakeTile
Tile
;
for
(
const
auto
&
vertex
:
tile
)
{
QGeoCoordinate
QVertex
(
vertex
[
0
],
vertex
[
1
],
vertex
[
2
]);
Tile
.
append
(
QVertex
);
}
const
auto
&
centerPoint
=
cps
[
i
];
QGeoCoordinate
QCenterPoint
(
centerPoint
[
0
],
centerPoint
[
1
],
centerPoint
[
2
]);
Tile
.
setCenter
(
QCenterPoint
);
_snakeTiles
.
polygons
().
append
(
Tile
);
_snakeTileCenterPoints
.
append
(
QVariant
::
fromValue
(
QCenterPoint
));
}
}
{
// Get local tiles.
const
auto
&
tiles
=
_scenario
.
getTilesENU
();
for
(
unsigned
int
i
=
0
;
i
<
tiles
.
size
();
++
i
)
{
const
auto
&
tile
=
tiles
[
i
];
Polygon2D
Tile
;
for
(
const
auto
&
vertex
:
tile
.
outer
())
{
QPointF
QVertex
(
vertex
.
get
<
0
>
(),
vertex
.
get
<
1
>
());
Tile
.
path
().
append
(
QVertex
);
}
_snakeTilesLocal
.
polygons
().
append
(
Tile
);
}
}
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
if
(
_enableSnake
.
rawValue
().
toBool
()
&&
_pRosBridge
->
isRunning
()
&&
_pRosBridge
->
connected
()
&&
_topicServiceSetupDone
&&
_snakeTilesLocal
.
polygons
().
size
()
>
0
)
{
using
namespace
ros_bridge
::
messages
;
// Publish snake origin.
JsonDocUPtr
jOrigin
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
bool
ret
=
geographic_msgs
::
geo_point
::
toJson
(
_snakeOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
());
Q_ASSERT
(
ret
);
_pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
// Publish snake tiles.
JsonDocUPtr
jSnakeTiles
(
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
ret
=
jsk_recognition_msgs
::
polygon_array
::
toJson
(
_snakeTilesLocal
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
());
Q_ASSERT
(
ret
);
_pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
_localPlanDataValid
=
true
;
return
true
;
}
WimaController
*
WimaController
::
thisPointer
()
{
return
this
;
}
bool
WimaController
::
_calcNextPhase
()
{
if
(
!
_currentManager
->
next
()
)
{
Q_ASSERT
(
false
);
return
false
;
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
return
true
;
}
bool
WimaController
::
_setStartIndex
()
if
(
areaCounter
!=
numAreas
)
{
{
Q_ASSERT
(
false
);
bool
value
;
return
false
;
_currentManager
->
setStartIndex
(
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
)
-
1
);
}
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
emit
visualItemsChanged
();
Q_ASSERT
(
false
);
return
false
;
}
emit
missionItemsChanged
();
// extract mission items
emit
currentMissionItemsChanged
();
QList
<
MissionItem
>
tempMissionItems
=
planData
.
missionItems
();
emit
currentWaypointPathChanged
();
if
(
tempMissionItems
.
size
()
<
1
)
{
emit
waypointPathChanged
();
qWarning
(
"WimaController: Mission items from WimaPlaner empty!"
);
return
false
;
}
return
true
;
for
(
auto
item
:
tempMissionItems
)
{
}
_defaultWM
.
push_back
(
item
.
coordinate
());
}
void
WimaController
::
_recalcCurrentPhase
()
_WMSettings
.
setHomePosition
(
QGeoCoordinate
(
{
_serviceArea
.
center
().
latitude
(),
_serviceArea
.
center
().
longitude
(),
0
));
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
if
(
!
_defaultWM
.
reset
())
{
emit
currentMissionItemsChanged
();
Q_ASSERT
(
false
);
emit
currentWaypointPathChanged
();
return
false
;
emit
waypointPathChanged
();
}
}
void
WimaController
::
_updateOverlap
()
emit
missionItemsChanged
();
{
emit
currentMissionItemsChanged
();
bool
value
;
emit
waypointPathChanged
();
_currentManager
->
setOverlap
(
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
));
emit
currentWaypointPathChanged
();
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
// Update Snake Data Manager
assert
(
false
);
_snakeDM
.
setMeasurementArea
(
_measurementArea
.
coordinateList
());
}
_snakeDM
.
setServiceArea
(
_serviceArea
.
coordinateList
());
_snakeDM
.
setCorridor
(
_corridor
.
coordinateList
());
_currentDM
->
start
();
emit
missionItemsChanged
();
_localPlanDataValid
=
true
;
emit
currentMissionItemsChanged
();
return
true
;
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
}
void
WimaController
::
_updateMaxWaypoints
()
WimaController
*
WimaController
::
thisPointer
()
{
return
this
;
}
{
bool
value
;
_currentManager
->
setN
(
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateflightSpeed
()
bool
WimaController
::
_calcNextPhase
()
{
{
if
(
!
_currentWM
->
next
())
{
bool
value
;
Q_ASSERT
(
false
);
_managerSettings
.
setFlightSpeed
(
_flightSpeed
.
rawValue
().
toDouble
(
&
value
));
return
false
;
Q_ASSERT
(
value
);
}
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
emit
missionItemsChanged
();
Q_ASSERT
(
false
);
emit
currentMissionItemsChanged
();
}
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
emit
missionItemsChanged
();
return
true
;
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
}
void
WimaController
::
_updateArrivalReturnSpeed
()
bool
WimaController
::
_setStartIndex
()
{
{
bool
value
;
bool
value
;
_currentWM
->
setStartIndex
(
_managerSettings
.
setArrivalReturnSpeed
(
_arrivalReturnSpeed
.
rawValue
().
toDouble
(
&
value
)
);
_nextPhaseStartWaypointIndex
.
rawValue
().
toUInt
(
&
value
)
-
1
);
Q_ASSERT
(
value
);
Q_ASSERT
(
value
);
(
void
)
value
;
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
if
(
!
_currentWM
->
update
())
{
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
}
return
false
;
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateAltitude
()
{
bool
value
;
_managerSettings
.
setAltitude
(
_altitude
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentManager
->
update
()
)
{
emit
missionItemsChanged
();
Q_ASSERT
(
false
);
emit
currentMissionItemsChanged
();
}
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
emit
missionItemsChanged
();
return
true
;
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
}
void
WimaController
::
_checkBatteryLevel
()
void
WimaController
::
_recalcCurrentPhase
()
{
{
if
(
!
_currentWM
->
update
())
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
Q_ASSERT
(
false
);
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
}
int
batteryThreshold
=
wimaSettings
->
lowBatteryThreshold
()
->
rawValue
().
toInt
();
bool
enabled
=
_enableWimaController
.
rawValue
().
toBool
();
unsigned
int
minTime
=
wimaSettings
->
minimalRemainingMissionTime
()
->
rawValue
().
toUInt
();
if
(
managerVehicle
!=
nullptr
&&
enabled
==
true
)
{
Fact
*
battery1percentRemaining
=
managerVehicle
->
battery1FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
Fact
*
battery2percentRemaining
=
managerVehicle
->
battery2FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
emit
missionItemsChanged
();
if
(
battery1percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
emit
currentMissionItemsChanged
();
&&
battery2percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
)
{
emit
currentWaypointPathChanged
();
if
(
!
_lowBatteryHandlingTriggered
)
{
emit
waypointPathChanged
();
_lowBatteryHandlingTriggered
=
true
;
}
if
(
!
(
_missionController
->
remainingTime
()
<=
minTime
)
)
{
requestSmartRTL
();
void
WimaController
::
_updateOverlap
()
{
}
bool
value
;
}
_currentWM
->
setOverlap
(
_overlapWaypoints
.
rawValue
().
toUInt
(
&
value
));
}
Q_ASSERT
(
value
);
else
{
(
void
)
value
;
_lowBatteryHandlingTriggered
=
false
;
if
(
!
_currentWM
->
update
())
{
assert
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateMaxWaypoints
()
{
bool
value
;
_currentWM
->
setN
(
_maxWaypointsPerPhase
.
rawValue
().
toUInt
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentWM
->
update
())
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateflightSpeed
()
{
bool
value
;
_WMSettings
.
setFlightSpeed
(
_flightSpeed
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentWM
->
update
())
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateArrivalReturnSpeed
()
{
bool
value
;
_WMSettings
.
setArrivalReturnSpeed
(
_arrivalReturnSpeed
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentWM
->
update
())
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_updateAltitude
()
{
bool
value
;
_WMSettings
.
setAltitude
(
_altitude
.
rawValue
().
toDouble
(
&
value
));
Q_ASSERT
(
value
);
(
void
)
value
;
if
(
!
_currentWM
->
update
())
{
Q_ASSERT
(
false
);
}
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_checkBatteryLevel
()
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
int
batteryThreshold
=
wimaSettings
->
lowBatteryThreshold
()
->
rawValue
().
toInt
();
bool
enabled
=
_enableWimaController
.
rawValue
().
toBool
();
unsigned
int
minTime
=
wimaSettings
->
minimalRemainingMissionTime
()
->
rawValue
().
toUInt
();
if
(
managerVehicle
!=
nullptr
&&
enabled
==
true
)
{
Fact
*
battery1percentRemaining
=
managerVehicle
->
battery1FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
Fact
*
battery2percentRemaining
=
managerVehicle
->
battery2FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
if
(
battery1percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
&&
battery2percentRemaining
->
rawValue
().
toDouble
()
<
batteryThreshold
)
{
if
(
!
_lowBatteryHandlingTriggered
)
{
_lowBatteryHandlingTriggered
=
true
;
if
(
!
(
_missionController
->
remainingTime
()
<=
minTime
))
{
requestSmartRTL
();
}
}
}
}
else
{
_lowBatteryHandlingTriggered
=
false
;
}
}
}
}
}
void
WimaController
::
_eventTimerHandler
()
void
WimaController
::
_eventTimerHandler
()
{
{
// Battery level check necessary?
// Battery level check necessary?
Fact
*
enableLowBatteryHandling
=
qgcApp
()
Fact
*
enableLowBatteryHandling
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
()
->
enableLowBatteryHandling
();
->
toolbox
()
if
(
enableLowBatteryHandling
->
rawValue
().
toBool
()
&&
_batteryLevelTicker
.
ready
()
)
->
settingsManager
()
_checkBatteryLevel
();
->
wimaSettings
()
->
enableLowBatteryHandling
();
// Snake flight plan update necessary?
if
(
enableLowBatteryHandling
->
rawValue
().
toBool
()
&&
// if ( snakeEventLoopTicker.ready() ) {
_batteryLevelTicker
.
ready
())
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
_checkBatteryLevel
();
// }
// }
if
(
_nemoTimeoutTicker
.
ready
()
&&
_enableSnake
.
rawValue
().
toBool
()
)
{
this
->
_nemoHeartbeat
.
setStatus
(
_fallbackStatus
);
emit
WimaController
::
nemoStatusChanged
();
emit
WimaController
::
nemoStatusStringChanged
();
}
}
}
void
WimaController
::
_smartRTLCleanUp
(
bool
flying
)
void
WimaController
::
_smartRTLCleanUp
(
bool
flying
)
{
{
if
(
!
flying
)
{
// vehicle has landed
if
(
!
flying
)
{
// vehicle has landed
_switchWaypointManager
(
_defaultWM
);
_switchWaypointManager
(
_defaultManager
);
_missionController
->
removeAllFromVehicle
();
_missionController
->
removeAllFromVehicle
();
_missionController
->
removeAll
();
_missionController
->
removeAll
();
disconnect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
disconnect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
this
,
&
WimaController
::
_smartRTLCleanUp
);
}
}
}
void
WimaController
::
_setPhaseDistance
(
double
distance
)
{
(
void
)
distance
;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// _phaseDistance = distance;
// emit phaseDistanceChanged();
// }
}
}
void
WimaController
::
_setPhaseDuration
(
double
duration
)
void
WimaController
::
_setPhaseDistance
(
double
distance
)
{
{
(
void
)
distance
;
(
void
)
duration
;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// if (!qFuzzyCompare(duration, _phaseDuration)) {
// _phaseDistance = distance;
// _phaseDuration = duration;
// emit phaseDuration
Changed();
// emit phaseDistance
Changed();
// }
// }
}
}
bool
WimaController
::
_checkSmartRTLPreCondition
(
QString
&
errorString
)
void
WimaController
::
_setPhaseDuration
(
double
duration
)
{
{
(
void
)
duration
;
if
(
!
_localPlanDataValid
)
{
// if (!qFuzzyCompare(duration, _phaseDuration)) {
errorString
.
append
(
tr
(
"No WiMA data available. Please define at least a measurement and a service area."
));
// _phaseDuration = duration;
return
false
;
}
return
_rtlManager
.
checkPrecondition
(
errorString
);
// emit phaseDurationChanged();
// }
}
}
void
WimaController
::
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
)
bool
WimaController
::
_checkSmartRTLPreCondition
(
QString
&
errorString
)
{
{
if
(
!
_localPlanDataValid
)
{
if
(
_currentManager
!=
&
manager
)
{
errorString
.
append
(
tr
(
"No WiMA data available. Please define at least a "
_currentManager
=
&
manager
;
"measurement and a service area."
));
return
false
;
disconnect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
}
this
,
&
WimaController
::
_updateOverlap
);
disconnect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
_maxWaypointsPerPhase
.
setRawValue
(
_currentManager
->
N
());
_overlapWaypoints
.
setRawValue
(
_currentManager
->
overlap
());
_nextPhaseStartWaypointIndex
.
setRawValue
(
_currentManager
->
startIndex
());
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
qWarning
()
<<
"WimaController::_switchWaypointManager: statistics update missing."
;
}
}
void
WimaController
::
_initSmartRTL
()
return
_rtlWM
.
checkPrecondition
(
errorString
);
{
QString
errorString
;
static
int
attemptCounter
=
0
;
attemptCounter
++
;
if
(
_checkSmartRTLPreCondition
(
errorString
)
)
{
_masterController
->
managerVehicle
()
->
pauseVehicle
();
connect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
if
(
_rtlManager
.
update
()
)
{
// Calculate return path.
_switchWaypointManager
(
_rtlManager
);
attemptCounter
=
0
;
emit
smartRTLPathConfirm
();
return
;
}
}
else
if
(
attemptCounter
>
SMART_RTL_MAX_ATTEMPTS
)
{
errorString
.
append
(
tr
(
"Smart RTL: No success after maximum number of attempts."
));
qgcApp
()
->
showMessage
(
errorString
);
attemptCounter
=
0
;
}
else
{
_smartRTLTimer
.
singleShot
(
SMART_RTL_ATTEMPT_INTERVAL
,
this
,
&
WimaController
::
_initSmartRTL
);
}
}
}
void
WimaController
::
_setSnakeCalcInProgress
(
bool
inProgress
)
void
WimaController
::
_switchWaypointManager
(
{
WaypointManager
::
ManagerBase
&
manager
)
{
if
(
_snakeCalcInProgress
!=
inProgress
){
if
(
_currentWM
!=
&
manager
)
{
_snakeCalcInProgress
=
inProgress
;
_currentWM
=
&
manager
;
emit
snakeCalcInProgressChanged
();
}
}
void
WimaController
::
_snakeStoreWorkerResults
()
disconnect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
{
&
WimaController
::
_updateOverlap
);
_setSnakeCalcInProgress
(
false
);
disconnect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
&
WimaController
::
_updateMaxWaypoints
);
_snakeManager
.
clear
();
disconnect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
const
auto
&
r
=
_snakeDataManager
.
getResult
();
_maxWaypointsPerPhase
.
setRawValue
(
_currentWM
->
N
());
if
(
!
r
.
success
)
{
_overlapWaypoints
.
setRawValue
(
_currentWM
->
overlap
());
//qgcApp()->showMessage(r.errorMessage);
_nextPhaseStartWaypointIndex
.
setRawValue
(
_currentWM
->
startIndex
());
return
;
}
// create Mission items from r.waypoints
long
n
=
r
.
waypoints
.
size
()
-
r
.
returnPathIdx
.
size
()
-
r
.
arrivalPathIdx
.
size
()
+
2
;
if
(
n
<
1
)
{
return
;
}
// Create QVector<QGeoCoordinate> containing all waypoints;
unsigned
long
startIdx
=
r
.
arrivalPathIdx
.
last
();
unsigned
long
endIdx
=
r
.
returnPathIdx
.
first
();
for
(
unsigned
long
i
=
startIdx
;
i
<=
endIdx
;
++
i
)
{
_snakeManager
.
push_back
(
r
.
waypoints
[
int
(
i
)].
value
<
QGeoCoordinate
>
());
}
_snakeManager
.
update
();
connect
(
&
_overlapWaypoints
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateOverlap
);
connect
(
&
_maxWaypointsPerPhase
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateMaxWaypoints
);
connect
(
&
_nextPhaseStartWaypointIndex
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_setStartIndex
);
emit
missionItemsChanged
();
emit
missionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentMissionItemsChanged
();
emit
currentWaypointPathChanged
();
emit
waypointPathChanged
();
emit
waypointPathChanged
();
emit
currentWaypointPathChanged
();
auto
end
=
std
::
chrono
::
high_resolution_clock
::
now
();
qWarning
()
double
duration
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
end
-
start
).
count
();
<<
"WimaController::_switchWaypointManager: statistics update missing."
;
qWarning
()
<<
"WimaController::_snakeStoreWorkerResults execution time: "
<<
duration
<<
" ms."
;
}
}
void
WimaController
::
_initSmartRTL
()
{
QString
errorString
;
static
int
attemptCounter
=
0
;
attemptCounter
++
;
if
(
_checkSmartRTLPreCondition
(
errorString
))
{
_masterController
->
managerVehicle
()
->
pauseVehicle
();
connect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
if
(
_rtlWM
.
update
())
{
// Calculate return path.
_switchWaypointManager
(
_rtlWM
);
attemptCounter
=
0
;
emit
smartRTLPathConfirm
();
return
;
}
}
else
if
(
attemptCounter
>
SMART_RTL_MAX_ATTEMPTS
)
{
errorString
.
append
(
tr
(
"Smart RTL: No success after maximum number of attempts."
));
qgcApp
()
->
showMessage
(
errorString
);
attemptCounter
=
0
;
}
else
{
_smartRTLTimer
.
singleShot
(
SMART_RTL_ATTEMPT_INTERVAL
,
this
,
&
WimaController
::
_initSmartRTL
);
}
}
void
WimaController
::
_DMFinishedHandler
()
{
if
(
!
_snakeDM
.
success
())
{
// qgcApp()->showMessage(r.errorMessage);
return
;
}
// Copy waypoints to waypoint manager.
_snakeWM
.
clear
();
auto
waypoints
=
_snakeDM
.
waypoints
();
if
(
waypoints
.
size
()
<
1
)
{
return
;
}
for
(
auto
&
vertex
:
waypoints
)
{
_snakeWM
.
push_back
(
vertex
);
}
// Do update.
auto
fut
=
QtConcurrent
::
run
([
this
]
{
this
->
_snakeWM
.
update
();
// this can take a while (ca. 200ms)
emit
this
->
missionItemsChanged
();
emit
this
->
currentMissionItemsChanged
();
emit
this
->
currentWaypointPathChanged
();
emit
this
->
waypointPathChanged
();
});
(
void
)
fut
;
}
void
WimaController
::
_switchToSnakeWaypointManager
(
QVariant
variant
)
{
if
(
variant
.
value
<
bool
>
())
{
_switchWaypointManager
(
_snakeWM
);
}
else
{
_switchWaypointManager
(
_defaultWM
);
}
}
void
WimaController
::
_switchDataManager
(
SnakeDataManager
&
dataManager
)
{
if
(
_currentDM
!=
&
dataManager
)
{
disconnect
(
_currentDM
,
&
SnakeDataManager
::
finished
,
this
,
&
WimaController
::
_DMFinishedHandler
);
disconnect
(
_currentDM
,
&
SnakeDataManager
::
nemoProgressChanged
,
this
,
&
WimaController
::
_progressChangedHandler
);
disconnect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
&
WimaController
::
nemoStatusChanged
);
disconnect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
&
WimaController
::
nemoStatusStringChanged
);
_currentDM
=
&
dataManager
;
connect
(
_currentDM
,
&
SnakeDataManager
::
finished
,
this
,
&
WimaController
::
_DMFinishedHandler
);
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoProgressChanged
,
this
,
&
WimaController
::
_progressChangedHandler
);
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
&
WimaController
::
nemoStatusChanged
);
connect
(
_currentDM
,
&
SnakeDataManager
::
nemoStatusChanged
,
this
,
&
WimaController
::
nemoStatusStringChanged
);
emit
snakeConnectionStatusChanged
();
emit
snakeCalcInProgressChanged
();
emit
snakeTilesChanged
();
emit
snakeTileCenterPointsChanged
();
emit
nemoProgressChanged
();
emit
nemoStatusChanged
();
emit
nemoStatusStringChanged
();
}
}
}
void
WimaController
::
_initStartSnakeWorker
()
void
WimaController
::
_progressChangedHandler
()
{
{
emit
this
->
nemoProgressChanged
();
if
(
!
_enableSnake
.
rawValue
().
toBool
()
)
this
->
_currentDM
->
start
();
return
;
// Stop worker thread if running.
if
(
_snakeDataManager
.
isRunning
()
)
{
_snakeDataManager
.
quit
();
}
// Initialize _snakeWorker.
_snakeDataManager
.
setProgress
(
_nemoProgress
.
progress
());
_snakeDataManager
.
setLineDistance
(
_snakeLineDistance
.
rawValue
().
toDouble
());
_snakeDataManager
.
setMinTransectLength
(
_snakeMinTransectLength
.
rawValue
().
toDouble
());
_snakeDataManager
.
setTileHeight
(
_snakeTileHeight
.
rawValue
().
toDouble
());
_snakeDataManager
.
setTileWidth
(
_snakeTileWidth
.
rawValue
().
toDouble
());
_snakeDataManager
.
setMinTileArea
(
_snakeMinTileArea
.
rawValue
().
toDouble
());
_setSnakeCalcInProgress
(
true
);
// Start worker thread.
_snakeDataManager
.
start
();
}
void
WimaController
::
_switchSnakeManager
(
QVariant
variant
)
{
if
(
variant
.
value
<
bool
>
()){
_switchWaypointManager
(
_snakeManager
);
}
else
{
_switchWaypointManager
(
_defaultManager
);
}
}
}
void
WimaController
::
_enableSnakeChangedHandler
()
{
if
(
this
->
_enableSnake
.
rawValue
().
toBool
())
{
this
->
_snakeDM
.
enableRosBridge
();
_switchDataManager
(
_snakeDM
);
_currentDM
->
start
();
}
else
{
this
->
_snakeDM
.
disableRosBride
();
_switchDataManager
(
_emptyDM
);
}
}
src/Wima/WimaController.h
View file @
c6e6a519
...
@@ -7,32 +7,29 @@
...
@@ -7,32 +7,29 @@
#include "QmlObjectListModel.h"
#include "QmlObjectListModel.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "WimaPlanData.h"
#include "
PlanMasterControll
er.h"
#include "
JsonHelp
er.h"
#include "MissionController.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "MissionSettingsItem.h"
#include "
JsonHelp
er.h"
#include "
PlanMasterControll
er.h"
#include "QGCApplication.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "SettingsManager.h"
#include "SimpleMissionItem.h"
#include "SurveyComplexItem.h"
#include "
snake
.h"
#include "
Geometry/GeoPoint3D
.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/SnakeDataManager.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
#include "Snake/SnakeTilesLocal.h"
#include "Geometry/GeoPoint3D.h"
#include "ros_bridge/include/ros_bridge.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
#include "WaypointManager/RTLManager.h"
...
@@ -41,363 +38,285 @@
...
@@ -41,363 +38,285 @@
#include <map>
#include <map>
using
namespace
snake
;
typedef
std
::
unique_ptr
<
rapidjson
::
Document
>
JsonDocUPtr
;
typedef
std
::
unique_ptr
<
rapidjson
::
Document
>
JsonDocUPtr
;
class
WimaController
:
public
QObject
class
WimaController
:
public
QObject
{
{
Q_OBJECT
Q_OBJECT
enum
FileType
{
WimaFile
,
PlanFile
};
enum
FileType
{
WimaFile
,
PlanFile
};
public:
WimaController
(
QObject
*
parent
=
nullptr
);
// Controllers.
public:
Q_PROPERTY
(
PlanMasterController
*
masterController
WimaController
(
QObject
*
parent
=
nullptr
);
READ
masterController
WRITE
setMasterController
// Controllers.
NOTIFY
masterControllerChanged
Q_PROPERTY
(
PlanMasterController
*
masterController
READ
masterController
WRITE
)
setMasterController
NOTIFY
masterControllerChanged
)
Q_PROPERTY
(
MissionController
*
missionController
Q_PROPERTY
(
MissionController
*
missionController
READ
missionController
WRITE
READ
missionController
setMissionController
NOTIFY
missionControllerChanged
)
WRITE
setMissionController
// Wima Data.
NOTIFY
missionControllerChanged
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
READ
visualItems
NOTIFY
)
visualItemsChanged
)
// Wima Data.
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
READ
missionItems
NOTIFY
Q_PROPERTY
(
QmlObjectListModel
*
visualItems
missionItemsChanged
)
READ
visualItems
Q_PROPERTY
(
QmlObjectListModel
*
currentMissionItems
READ
currentMissionItems
NOTIFY
visualItemsChanged
NOTIFY
currentMissionItemsChanged
)
)
Q_PROPERTY
(
Q_PROPERTY
(
QmlObjectListModel
*
missionItems
QVariantList
waypointPath
READ
waypointPath
NOTIFY
waypointPathChanged
)
READ
missionItems
Q_PROPERTY
(
QVariantList
currentWaypointPath
READ
currentWaypointPath
NOTIFY
NOTIFY
missionItemsChanged
currentWaypointPathChanged
)
)
Q_PROPERTY
(
Fact
*
enableWimaController
READ
enableWimaController
CONSTANT
)
Q_PROPERTY
(
QmlObjectListModel
*
currentMissionItems
// Waypoint navigaton.
READ
currentMissionItems
Q_PROPERTY
(
Fact
*
overlapWaypoints
READ
overlapWaypoints
CONSTANT
)
NOTIFY
currentMissionItemsChanged
Q_PROPERTY
(
Fact
*
maxWaypointsPerPhase
READ
maxWaypointsPerPhase
CONSTANT
)
)
Q_PROPERTY
(
Fact
*
startWaypointIndex
READ
startWaypointIndex
CONSTANT
)
Q_PROPERTY
(
QVariantList
waypointPath
Q_PROPERTY
(
Fact
*
showAllMissionItems
READ
showAllMissionItems
CONSTANT
)
READ
waypointPath
Q_PROPERTY
(
NOTIFY
waypointPathChanged
Fact
*
showCurrentMissionItems
READ
showCurrentMissionItems
CONSTANT
)
)
// Waypoint settings.
Q_PROPERTY
(
QVariantList
currentWaypointPath
Q_PROPERTY
(
Fact
*
flightSpeed
READ
flightSpeed
CONSTANT
)
READ
currentWaypointPath
Q_PROPERTY
(
Fact
*
altitude
READ
altitude
CONSTANT
)
NOTIFY
currentWaypointPathChanged
Q_PROPERTY
(
Fact
*
arrivalReturnSpeed
READ
arrivalReturnSpeed
CONSTANT
)
)
// Waypoint statistics.
Q_PROPERTY
(
Fact
*
enableWimaController
Q_PROPERTY
(
READ
enableWimaController
double
phaseDistance
READ
phaseDistance
NOTIFY
phaseDistanceChanged
)
CONSTANT
)
Q_PROPERTY
(
// Waypoint navigaton.
double
phaseDuration
READ
phaseDuration
NOTIFY
phaseDurationChanged
)
Q_PROPERTY
(
Fact
*
overlapWaypoints
READ
overlapWaypoints
// Snake
CONSTANT
Q_PROPERTY
(
Fact
*
enableSnake
READ
enableSnake
CONSTANT
)
)
Q_PROPERTY
(
int
nemoStatus
READ
nemoStatus
NOTIFY
nemoStatusChanged
)
Q_PROPERTY
(
Fact
*
maxWaypointsPerPhase
Q_PROPERTY
(
QString
nemoStatusString
READ
nemoStatusString
NOTIFY
READ
maxWaypointsPerPhase
nemoStatusStringChanged
)
CONSTANT
Q_PROPERTY
(
bool
snakeCalcInProgress
READ
snakeCalcInProgress
NOTIFY
)
snakeCalcInProgressChanged
)
Q_PROPERTY
(
Fact
*
startWaypointIndex
Q_PROPERTY
(
Fact
*
snakeTileWidth
READ
snakeTileWidth
CONSTANT
)
READ
startWaypointIndex
Q_PROPERTY
(
Fact
*
snakeTileHeight
READ
snakeTileHeight
CONSTANT
)
CONSTANT
Q_PROPERTY
(
Fact
*
snakeMinTileArea
READ
snakeMinTileArea
CONSTANT
)
)
Q_PROPERTY
(
Fact
*
snakeLineDistance
READ
snakeLineDistance
CONSTANT
)
Q_PROPERTY
(
Fact
*
showAllMissionItems
Q_PROPERTY
(
Fact
*
snakeMinTransectLength
READ
snakeMinTransectLength
CONSTANT
)
READ
showAllMissionItems
Q_PROPERTY
(
CONSTANT
QmlObjectListModel
*
snakeTiles
READ
snakeTiles
NOTIFY
snakeTilesChanged
)
)
Q_PROPERTY
(
QVariantList
snakeTileCenterPoints
READ
snakeTileCenterPoints
Q_PROPERTY
(
Fact
*
showCurrentMissionItems
NOTIFY
snakeTileCenterPointsChanged
)
READ
showCurrentMissionItems
Q_PROPERTY
(
CONSTANT
QVector
<
int
>
nemoProgress
READ
nemoProgress
NOTIFY
nemoProgressChanged
)
)
// Waypoint settings.
// Property accessors
Q_PROPERTY
(
Fact
*
flightSpeed
// Controllers.
READ
flightSpeed
PlanMasterController
*
masterController
(
void
);
CONSTANT
MissionController
*
missionController
(
void
);
)
// Wima Data
Q_PROPERTY
(
Fact
*
altitude
QmlObjectListModel
*
visualItems
(
void
);
READ
altitude
QGCMapPolygon
joinedArea
(
void
)
const
;
CONSTANT
// Waypoints.
)
QmlObjectListModel
*
missionItems
(
void
);
Q_PROPERTY
(
Fact
*
arrivalReturnSpeed
QmlObjectListModel
*
currentMissionItems
(
void
);
READ
arrivalReturnSpeed
QVariantList
waypointPath
(
void
);
CONSTANT
QVariantList
currentWaypointPath
(
void
);
)
// Settings facts.
// Waypoint statistics.
Fact
*
enableWimaController
(
void
);
Q_PROPERTY
(
double
phaseDistance
Fact
*
overlapWaypoints
(
void
);
READ
phaseDistance
Fact
*
maxWaypointsPerPhase
(
void
);
NOTIFY
phaseDistanceChanged
Fact
*
startWaypointIndex
(
void
);
)
Fact
*
showAllMissionItems
(
void
);
Q_PROPERTY
(
double
phaseDuration
Fact
*
showCurrentMissionItems
(
void
);
READ
phaseDuration
Fact
*
flightSpeed
(
void
);
NOTIFY
phaseDurationChanged
Fact
*
arrivalReturnSpeed
(
void
);
)
Fact
*
altitude
(
void
);
// Snake settings facts.
// Snake
Fact
*
enableSnake
(
void
)
{
return
&
_enableSnake
;
}
Q_PROPERTY
(
Fact
*
enableSnake
Fact
*
snakeTileWidth
(
void
)
{
return
&
_snakeTileWidth
;
}
READ
enableSnake
Fact
*
snakeTileHeight
(
void
)
{
return
&
_snakeTileHeight
;
}
CONSTANT
Fact
*
snakeMinTileArea
(
void
)
{
return
&
_snakeMinTileArea
;
}
)
Fact
*
snakeLineDistance
(
void
)
{
return
&
_snakeLineDistance
;
}
Q_PROPERTY
(
int
nemoStatus
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;
}
READ
nemoStatus
// Snake data.
NOTIFY
nemoStatusChanged
QmlObjectListModel
*
snakeTiles
(
void
);
)
QVariantList
snakeTileCenterPoints
(
void
);
Q_PROPERTY
(
QString
nemoStatusString
QVector
<
int
>
nemoProgress
(
void
);
READ
nemoStatusString
int
nemoStatus
(
void
)
const
;
NOTIFY
nemoStatusStringChanged
QString
nemoStatusString
(
void
)
const
;
)
bool
snakeCalcInProgress
(
void
)
const
;
Q_PROPERTY
(
bool
snakeCalcInProgress
READ
snakeCalcInProgress
// Smart RTL.
NOTIFY
snakeCalcInProgressChanged
bool
uploadOverrideRequired
(
void
)
const
;
)
bool
vehicleHasLowBattery
(
void
)
const
;
Q_PROPERTY
(
Fact
*
snakeTileWidth
// Waypoint statistics.
READ
snakeTileWidth
double
phaseDistance
(
void
)
const
;
CONSTANT
double
phaseDuration
(
void
)
const
;
)
Q_PROPERTY
(
Fact
*
snakeTileHeight
// Property setters
READ
snakeTileHeight
void
setMasterController
(
PlanMasterController
*
masterController
);
CONSTANT
void
setMissionController
(
MissionController
*
missionController
);
)
bool
setWimaPlanData
(
const
WimaPlanData
&
planData
);
Q_PROPERTY
(
Fact
*
snakeMinTileArea
READ
snakeMinTileArea
// Member Methodes
CONSTANT
Q_INVOKABLE
WimaController
*
thisPointer
();
)
// Waypoint navigation.
Q_PROPERTY
(
Fact
*
snakeLineDistance
Q_INVOKABLE
void
nextPhase
();
READ
snakeLineDistance
Q_INVOKABLE
void
previousPhase
();
CONSTANT
Q_INVOKABLE
void
resetPhase
();
)
// Smart RTL.
Q_PROPERTY
(
Fact
*
snakeMinTransectLength
Q_INVOKABLE
void
requestSmartRTL
();
READ
snakeMinTransectLength
Q_INVOKABLE
void
initSmartRTL
();
CONSTANT
Q_INVOKABLE
void
executeSmartRTL
();
)
// Other.
Q_PROPERTY
(
QmlObjectListModel
*
snakeTiles
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
READ
snakeTiles
Q_INVOKABLE
bool
upload
();
NOTIFY
snakeTilesChanged
Q_INVOKABLE
bool
forceUpload
();
)
Q_INVOKABLE
void
removeFromVehicle
();
Q_PROPERTY
(
QVariantList
snakeTileCenterPoints
READ
snakeTileCenterPoints
// static Members
NOTIFY
snakeTileCenterPointsChanged
static
const
char
*
areaItemsName
;
)
static
const
char
*
missionItemsName
;
Q_PROPERTY
(
QVector
<
int
>
nemoProgress
static
const
char
*
settingsGroup
;
READ
nemoProgress
static
const
char
*
endWaypointIndexName
;
NOTIFY
nemoProgressChanged
static
const
char
*
enableWimaControllerName
;
)
static
const
char
*
overlapWaypointsName
;
static
const
char
*
maxWaypointsPerPhaseName
;
static
const
char
*
startWaypointIndexName
;
static
const
char
*
showAllMissionItemsName
;
// Property accessors
static
const
char
*
showCurrentMissionItemsName
;
// Controllers.
static
const
char
*
flightSpeedName
;
PlanMasterController
*
masterController
(
void
);
static
const
char
*
arrivalReturnSpeedName
;
MissionController
*
missionController
(
void
);
static
const
char
*
altitudeName
;
// Wima Data
static
const
char
*
snakeTileWidthName
;
QmlObjectListModel
*
visualItems
(
void
);
static
const
char
*
snakeTileHeightName
;
QGCMapPolygon
joinedArea
(
void
)
const
;
static
const
char
*
snakeMinTileAreaName
;
// Waypoints.
static
const
char
*
snakeLineDistanceName
;
QmlObjectListModel
*
missionItems
(
void
);
static
const
char
*
snakeMinTransectLengthName
;
QmlObjectListModel
*
currentMissionItems
(
void
);
QVariantList
waypointPath
(
void
);
QVariantList
currentWaypointPath
(
void
);
// Settings facts.
Fact
*
enableWimaController
(
void
);
Fact
*
overlapWaypoints
(
void
);
Fact
*
maxWaypointsPerPhase
(
void
);
Fact
*
startWaypointIndex
(
void
);
Fact
*
showAllMissionItems
(
void
);
Fact
*
showCurrentMissionItems
(
void
);
Fact
*
flightSpeed
(
void
);
Fact
*
arrivalReturnSpeed
(
void
);
Fact
*
altitude
(
void
);
// Snake settings facts.
Fact
*
enableSnake
(
void
)
{
return
&
_enableSnake
;
}
Fact
*
snakeTileWidth
(
void
)
{
return
&
_snakeTileWidth
;}
Fact
*
snakeTileHeight
(
void
)
{
return
&
_snakeTileHeight
;}
Fact
*
snakeMinTileArea
(
void
)
{
return
&
_snakeMinTileArea
;}
Fact
*
snakeLineDistance
(
void
)
{
return
&
_snakeLineDistance
;}
Fact
*
snakeMinTransectLength
(
void
)
{
return
&
_snakeMinTransectLength
;}
// Snake data.
QmlObjectListModel
*
snakeTiles
(
void
);
QVariantList
snakeTileCenterPoints
(
void
);
QVector
<
int
>
nemoProgress
(
void
);
int
nemoStatus
(
void
)
const
;
QString
nemoStatusString
(
void
)
const
;
bool
snakeCalcInProgress
(
void
)
const
;
// Smart RTL.
bool
uploadOverrideRequired
(
void
)
const
;
bool
vehicleHasLowBattery
(
void
)
const
;
// Waypoint statistics.
double
phaseDistance
(
void
)
const
;
double
phaseDuration
(
void
)
const
;
// Property setters
void
setMasterController
(
PlanMasterController
*
masterController
);
void
setMissionController
(
MissionController
*
missionController
);
bool
setWimaPlanData
(
const
WimaPlanData
&
planData
);
// Member Methodes
Q_INVOKABLE
WimaController
*
thisPointer
();
// Waypoint navigation.
Q_INVOKABLE
void
nextPhase
();
Q_INVOKABLE
void
previousPhase
();
Q_INVOKABLE
void
resetPhase
();
// Smart RTL.
Q_INVOKABLE
void
requestSmartRTL
();
Q_INVOKABLE
void
initSmartRTL
();
Q_INVOKABLE
void
executeSmartRTL
();
// Other.
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
bool
upload
();
Q_INVOKABLE
bool
forceUpload
();
Q_INVOKABLE
void
removeFromVehicle
();
// static Members
static
const
char
*
areaItemsName
;
static
const
char
*
missionItemsName
;
static
const
char
*
settingsGroup
;
static
const
char
*
endWaypointIndexName
;
static
const
char
*
enableWimaControllerName
;
static
const
char
*
overlapWaypointsName
;
static
const
char
*
maxWaypointsPerPhaseName
;
static
const
char
*
startWaypointIndexName
;
static
const
char
*
showAllMissionItemsName
;
static
const
char
*
showCurrentMissionItemsName
;
static
const
char
*
flightSpeedName
;
static
const
char
*
arrivalReturnSpeedName
;
static
const
char
*
altitudeName
;
static
const
char
*
snakeTileWidthName
;
static
const
char
*
snakeTileHeightName
;
static
const
char
*
snakeMinTileAreaName
;
static
const
char
*
snakeLineDistanceName
;
static
const
char
*
snakeMinTransectLengthName
;
signals:
signals:
// Controllers.
// Controllers.
void
masterControllerChanged
(
void
);
void
masterControllerChanged
(
void
);
void
missionControllerChanged
(
void
);
void
missionControllerChanged
(
void
);
// Wima data.
// Wima data.
void
visualItemsChanged
(
void
);
void
visualItemsChanged
(
void
);
// Waypoints.
// Waypoints.
void
missionItemsChanged
(
void
);
void
missionItemsChanged
(
void
);
void
currentMissionItemsChanged
(
void
);
void
currentMissionItemsChanged
(
void
);
void
waypointPathChanged
(
void
);
void
waypointPathChanged
(
void
);
void
currentWaypointPathChanged
(
void
);
void
currentWaypointPathChanged
(
void
);
// Smart RTL.
// Smart RTL.
void
smartRTLRequestConfirm
(
void
);
void
smartRTLRequestConfirm
(
void
);
void
smartRTLPathConfirm
(
void
);
void
smartRTLPathConfirm
(
void
);
// Upload.
// Upload.
void
forceUploadConfirm
(
void
);
void
forceUploadConfirm
(
void
);
// Waypoint statistics.
// Waypoint statistics.
void
phaseDistanceChanged
(
void
);
void
phaseDistanceChanged
(
void
);
void
phaseDurationChanged
(
void
);
void
phaseDurationChanged
(
void
);
// Snake.
// Snake.
void
snakeConnectionStatusChanged
(
void
);
void
snakeConnectionStatusChanged
(
void
);
void
snakeCalcInProgressChanged
(
void
);
void
snakeCalcInProgressChanged
(
void
);
void
snakeTilesChanged
(
void
);
void
snakeTilesChanged
(
void
);
void
snakeTileCenterPointsChanged
(
void
);
void
snakeTileCenterPointsChanged
(
void
);
void
nemoProgressChanged
(
void
);
void
nemoProgressChanged
(
void
);
void
nemoStatusChanged
(
void
);
void
nemoStatusChanged
(
void
);
void
nemoStatusStringChanged
(
void
);
void
nemoStatusStringChanged
(
void
);
private
slots
:
private
slots
:
// Waypoint navigation / helper.
// Waypoint navigation / helper.
bool
_calcNextPhase
(
void
);
bool
_calcNextPhase
(
void
);
void
_recalcCurrentPhase
(
void
);
void
_recalcCurrentPhase
(
void
);
bool
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
bool
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
);
QVector
<
QGeoCoordinate
>
&
path
);
// Slicing parameters
// Slicing parameters
bool
_setStartIndex
(
void
);
bool
_setStartIndex
(
void
);
void
_updateOverlap
(
void
);
void
_updateOverlap
(
void
);
void
_updateMaxWaypoints
(
void
);
void
_updateMaxWaypoints
(
void
);
// Waypoint settings.
// Waypoint settings.
void
_updateflightSpeed
(
void
);
void
_updateflightSpeed
(
void
);
void
_updateArrivalReturnSpeed
(
void
);
void
_updateArrivalReturnSpeed
(
void
);
void
_updateAltitude
(
void
);
void
_updateAltitude
(
void
);
// Waypoint Statistics.
// Waypoint Statistics.
void
_setPhaseDistance
(
double
distance
);
void
_setPhaseDistance
(
double
distance
);
void
_setPhaseDuration
(
double
duration
);
void
_setPhaseDuration
(
double
duration
);
// SMART RTL
// SMART RTL
void
_checkBatteryLevel
(
void
);
void
_checkBatteryLevel
(
void
);
bool
_checkSmartRTLPreCondition
(
QString
&
errorString
);
bool
_checkSmartRTLPreCondition
(
QString
&
errorString
);
void
_initSmartRTL
();
void
_initSmartRTL
();
void
_smartRTLCleanUp
(
bool
flying
);
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
// Snake.
void
_setSnakeCalcInProgress
(
bool
inProgress
);
void
_DMFinishedHandler
(
);
void
_snakeStoreWorkerResults
(
);
void
_switchToSnakeWaypointManager
(
QVariant
variant
);
void
_initStartSnakeWorker
(
);
void
_switchDataManager
(
SnakeDataManager
&
dataManager
);
void
_switchSnakeManager
(
QVariant
variant
);
void
_progressChangedHandler
(
);
bool
_doTopicServiceSetup
();
void
_enableSnakeChangedHandler
();
// Periodic tasks.
// Periodic tasks.
void
_eventTimerHandler
(
void
);
void
_eventTimerHandler
(
void
);
// Waypoint manager handling.
// Waypoint manager handling.
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
private:
private:
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
using
StatusMap
=
std
::
map
<
int
,
QString
>
;
// Controllers.
// Controllers.
PlanMasterController
*
_masterController
;
PlanMasterController
*
_masterController
;
MissionController
*
_missionController
;
MissionController
*
_missionController
;
// Wima Data.
// Wima Data.
QmlObjectListModel
_areas
;
// contains all visible areas
QmlObjectListModel
_areas
;
// contains all visible areas
WimaJoinedAreaData
_joinedArea
;
// joined area fromed by opArea, serArea, _corridor
WimaJoinedAreaData
WimaMeasurementAreaData
_measurementArea
;
// measurement area
_joinedArea
;
// joined area fromed by opArea, serArea, _corridor
WimaServiceAreaData
_serviceArea
;
// area for supplying
WimaMeasurementAreaData
_measurementArea
;
// measurement area
WimaCorridorData
_corridor
;
// corridor connecting opArea and serArea
WimaServiceAreaData
_serviceArea
;
// area for supplying
bool
_localPlanDataValid
;
WimaCorridorData
_corridor
;
// corridor connecting opArea and serArea
bool
_localPlanDataValid
;
// Waypoint Managers.
WaypointManager
::
AreaInterface
_areaInterface
;
// Waypoint Managers.
WaypointManager
::
Settings
_managerSettings
;
WaypointManager
::
AreaInterface
_areaInterface
;
WaypointManager
::
DefaultManager
_defaultManager
;
WaypointManager
::
Settings
_WMSettings
;
// Waypoint Manager Settings
WaypointManager
::
DefaultManager
_snakeManager
;
WaypointManager
::
DefaultManager
_defaultWM
;
WaypointManager
::
RTLManager
_rtlManager
;
WaypointManager
::
DefaultManager
_snakeWM
;
WaypointManager
::
ManagerBase
*
_currentManager
;
WaypointManager
::
RTLManager
_rtlWM
;
using
ManagerList
=
QList
<
WaypointManager
::
ManagerBase
*>
;
WaypointManager
::
ManagerBase
*
_currentWM
;
ManagerList
_managerList
;
using
ManagerList
=
QList
<
WaypointManager
::
ManagerBase
*>
;
ManagerList
_WMList
;
// Settings Facts.
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
// Settings Facts.
SettingsFact
_enableWimaController
;
// enables or disables the wimaControler
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
SettingsFact
_overlapWaypoints
;
// determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact
_enableWimaController
;
// enables or disables the wimaControler
SettingsFact
_maxWaypointsPerPhase
;
// determines the maximum number waypoints per phase
SettingsFact
SettingsFact
_nextPhaseStartWaypointIndex
;
// index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
_overlapWaypoints
;
// determines the number of overlapping waypoints
// defining the first element of the next phase
// between two consecutive mission phases
SettingsFact
_showAllMissionItems
;
// bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact
_maxWaypointsPerPhase
;
// determines the maximum number waypoints
SettingsFact
_showCurrentMissionItems
;
// bool value, Determines whether the mission items of the current mission phase are displayed or not.
// per phase
SettingsFact
_flightSpeed
;
// mission flight speed
SettingsFact
SettingsFact
_arrivalReturnSpeed
;
// arrival and return path speed
_nextPhaseStartWaypointIndex
;
// index (displayed on the map, -1 to get
SettingsFact
_altitude
;
// mission altitude
// index of item in _missionItems) of the
SettingsFact
_enableSnake
;
// Enable Snake (see snake.h)
// mission item defining the first element
SettingsFact
_snakeTileWidth
;
// of the next phase
SettingsFact
_snakeTileHeight
;
SettingsFact
SettingsFact
_snakeMinTileArea
;
_showAllMissionItems
;
// bool value, Determines whether the mission items
SettingsFact
_snakeLineDistance
;
// of the overall mission are displayed or not.
SettingsFact
_snakeMinTransectLength
;
SettingsFact
_showCurrentMissionItems
;
// bool value, Determines whether the
// mission items of the current mission
// Smart RTL.
// phase are displayed or not.
QTimer
_smartRTLTimer
;
SettingsFact
_flightSpeed
;
// mission flight speed
bool
_lowBatteryHandlingTriggered
;
SettingsFact
_arrivalReturnSpeed
;
// arrival and return path speed
SettingsFact
_altitude
;
// mission altitude
// Waypoint statistics.
SettingsFact
_enableSnake
;
// Enable Snake (see snake.h)
double
_measurementPathLength
;
// the lenght of the phase in meters
SettingsFact
_snakeTileWidth
;
SettingsFact
_snakeTileHeight
;
// Snake
SettingsFact
_snakeMinTileArea
;
SnakeDataManager
_snakeDataManager
;
SettingsFact
_snakeLineDistance
;
int
_fallbackStatus
;
SettingsFact
_snakeMinTransectLength
;
static
StatusMap
_nemoStatusMap
;
// Smart RTL.
// Periodic tasks.
QTimer
_smartRTLTimer
;
QTimer
_eventTimer
;
bool
_lowBatteryHandlingTriggered
;
EventTicker
_batteryLevelTicker
;
EventTicker
_snakeTicker
;
// Waypoint statistics.
EventTicker
_nemoTimeoutTicker
;
double
_measurementPathLength
;
// the lenght of the phase in meters
// Snake
SnakeDataManager
_snakeDM
;
// Snake Data Manager
SnakeDataManager
_emptyDM
;
SnakeDataManager
*
_currentDM
;
static
StatusMap
_nemoStatusMap
;
// Periodic tasks.
QTimer
_eventTimer
;
EventTicker
_batteryLevelTicker
;
};
};
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @
c6e6a519
...
@@ -17,7 +17,7 @@ std::string messageType();
...
@@ -17,7 +17,7 @@ std::string messageType();
//! @brief C++ representation of nemo_msgs/Heartbeat message
//! @brief C++ representation of nemo_msgs/Heartbeat message
class
Heartbeat
{
class
Heartbeat
{
public:
public:
Heartbeat
(){}
Heartbeat
()
:
_status
(
0
)
{}
Heartbeat
(
int
status
)
:
_status
(
status
){}
Heartbeat
(
int
status
)
:
_status
(
status
){}
Heartbeat
(
const
Heartbeat
&
heartbeat
)
:
_status
(
heartbeat
.
status
()){}
Heartbeat
(
const
Heartbeat
&
heartbeat
)
:
_status
(
heartbeat
.
status
()){}
...
...
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