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 
29 
31 {
32 
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
58 
60  };
61 
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 &params)
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