ShortestPathFasterAlgorithm.h
Go to the documentation of this file.
1
/**
2
* This file is part of ArmarX.
3
*
4
* ArmarX is free software; you can redistribute it and/or modify
5
* it under the terms of the GNU General Public License version 2 as
6
* published by the Free Software Foundation.
7
*
8
* ArmarX is distributed in the hope that it will be useful, but
9
* WITHOUT ANY WARRANTY; without even the implied warranty of
10
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11
* GNU General Public License for more details.
12
*
13
* You should have received a copy of the GNU General Public License
14
* along with this program. If not, see <http://www.gnu.org/licenses/>.
15
*
16
* @author Fabian Reister ( fabian dot reister at kit dot edu )
17
* @date 2022
18
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19
* GNU General Public License
20
*/
21
22
#pragma once
23
24
#include <vector>
25
26
#include <Eigen/Core>
27
28
#include <
armarx/navigation/algorithms/Costmap.h
>
29
30
namespace
armarx::navigation::algorithms::spfa
31
{
32
33
class
ShortestPathFasterAlgorithm
34
{
35
public
:
36
struct
Parameters
37
{
38
bool
obstacleDistanceCosts
=
true
;
39
float
obstacleMaxDistance
= 1000.F;
40
41
float
obstacleDistanceWeight
= 1.3F;
42
float
obstacleCostExponent
= 4.F;
43
44
/// For ARMAR-7 the following was tested:
45
// float obstacleMaxDistance = 1500.F;
46
// float obstacleDistanceWeight = 6.F;
47
};
48
49
ShortestPathFasterAlgorithm
(
const
Costmap
& costmap,
const
Parameters
& params);
50
51
struct
Result
52
{
53
Eigen::MatrixXf
distances
;
54
std::vector<std::vector<Eigen::Vector2i>>
parents
;
55
56
// if 0, this cell is not reachable
57
using
Mask
=
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>
;
58
59
Mask
reachable
;
60
};
61
62
struct
PlanningResult
63
{
64
std::vector<Eigen::Vector2f>
path
;
65
66
bool
success
;
67
68
operator
bool() const noexcept
69
{
70
return
success
;
71
}
72
};
73
74
PlanningResult
plan
(
const
Eigen::Vector2f& start,
75
const
Eigen::Vector2f& goal,
76
bool
checkStartForCollision =
true
)
const
;
77
78
Result
spfa
(
const
Eigen::Vector2f& start,
bool
checkStartForCollision =
true
)
const
;
79
80
// protected:
81
/**
82
* @brief Implementation highly inspired by github.com:jimmyyhwu/spatial-action-maps
83
*
84
* @param input_map
85
* @param source
86
* @return Result
87
*/
88
Result
spfa
(
const
Eigen::MatrixXf& inputMap,
89
const
Eigen::Vector2i&
source
,
90
bool
checkStartForCollision =
true
)
const
;
91
92
93
private
:
94
const
Costmap
costmap;
95
96
const
Parameters params;
97
};
98
}
// namespace armarx::navigation::algorithms::spfa
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters
Definition:
ShortestPathFasterAlgorithm.h:36
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::spfa
Result spfa(const Eigen::Vector2f &start, bool checkStartForCollision=true) const
Definition:
ShortestPathFasterAlgorithm.cpp:139
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters::obstacleDistanceWeight
float obstacleDistanceWeight
Definition:
ShortestPathFasterAlgorithm.h:41
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult
Definition:
ShortestPathFasterAlgorithm.h:62
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result::parents
std::vector< std::vector< Eigen::Vector2i > > parents
Definition:
ShortestPathFasterAlgorithm.h:54
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult::success
bool success
Definition:
ShortestPathFasterAlgorithm.h:66
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters::obstacleMaxDistance
float obstacleMaxDistance
Definition:
ShortestPathFasterAlgorithm.h:39
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result::reachable
Mask reachable
Definition:
ShortestPathFasterAlgorithm.h:59
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result
Definition:
ShortestPathFasterAlgorithm.h:51
Costmap.h
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult::path
std::vector< Eigen::Vector2f > path
Definition:
ShortestPathFasterAlgorithm.h:64
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::plan
PlanningResult plan(const Eigen::Vector2f &start, const Eigen::Vector2f &goal, bool checkStartForCollision=true) const
Definition:
ShortestPathFasterAlgorithm.cpp:67
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters::obstacleCostExponent
float obstacleCostExponent
Definition:
ShortestPathFasterAlgorithm.h:42
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters::obstacleDistanceCosts
bool obstacleDistanceCosts
Definition:
ShortestPathFasterAlgorithm.h:38
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm
ShortestPathFasterAlgorithm(const Costmap &costmap, const Parameters ¶ms)
Definition:
ShortestPathFasterAlgorithm.cpp:58
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition:
point_cloud_graph.h:681
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result::distances
Eigen::MatrixXf distances
Definition:
ShortestPathFasterAlgorithm.h:53
armarx::navigation::algorithms::spfa
This file is part of ArmarX.
Definition:
ShortestPathFasterAlgorithm.cpp:36
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic >
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm
Definition:
ShortestPathFasterAlgorithm.h:33
armarx::navigation::algorithms::Costmap
Definition:
Costmap.h:13
armarx
navigation
algorithms
spfa
ShortestPathFasterAlgorithm.h
Generated on Sat Oct 12 2024 09:14:15 for armarx_documentation by
1.8.17