AStarPlanner.cpp
Go to the documentation of this file.
1 #include "AStarPlanner.h"
2 
3 #include <algorithm>
4 #include <cmath>
5 #include <omp.h>
6 #include <string>
7 
8 #include <boost/geometry.hpp>
9 #include <boost/geometry/algorithms/detail/intersects/interface.hpp>
10 
11 #include <Eigen/Geometry>
12 
13 #include <IceUtil/Time.h>
14 
15 #include <VirtualRobot/BoundingBox.h>
16 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
17 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
18 #include <VirtualRobot/XML/ObjectIO.h>
19 
22 
23 #include "range/v3/algorithm/reverse.hpp"
26 
27 namespace bg = boost::geometry;
28 
30 {
31 
32  bool
33  intersects(const VirtualRobot::BoundingBox& bb1, const VirtualRobot::BoundingBox& bb2)
34  {
35  using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
36  using box_t = bg::model::box<point_t>;
37 
38  const auto toPoint = [](const Eigen::Vector3f& pt)
39  { return point_t(pt.x(), pt.y(), pt.z()); };
40 
41  const box_t box1(toPoint(bb1.getMin()), toPoint(bb1.getMax()));
42  const box_t box2(toPoint(bb2.getMin()), toPoint(bb2.getMax()));
43 
44  return bg::intersects(box1, box2);
45  }
46 
47  AStarPlanner::AStarPlanner(const algorithms::Costmap& costmap) : costmap(costmap)
48  {
49  }
50 
51  float
52  AStarPlanner::heuristic(const NodePtr& n1, const NodePtr& n2)
53  {
54  return (1.F - obstacleDistanceWeightFactor) * (n1->position - n2->position).norm();
55  }
56 
57  void
58  AStarPlanner::createUniformGrid()
59  {
61 
62  const size_t rows = costmap.getGrid().rows();
63  const size_t cols = costmap.getGrid().cols();
64 
65  // create the grid with the correct size
66  ARMARX_VERBOSE << "Creating grid";
67  for (size_t r = 0; r < rows; r++)
68  {
69  grid.push_back(std::vector<NodePtr>(cols));
70  }
71 
72  // intitializing grid
73  for (size_t r = 0; r < rows; r++)
74  {
75  for (size_t c = 0; c < cols; c++)
76  {
77  const auto pos = costmap.toPositionGlobal({r, c});
78  const float obstacleDistance = costmap.getGrid()(r, c);
79 
80  grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
81  }
82  }
83 
84  ARMARX_VERBOSE << "Creating graph";
85 
86  // Init successors
87  for (size_t r = 0; r < rows; r++)
88  {
89  for (size_t c = 0; c < cols; c++)
90  {
91  NodePtr candidate = grid[r][c];
92  if (!fulfillsConstraints(candidate))
93  {
94  continue;
95  }
96 
97  // Add the valid node as successor to all its neighbors
98  for (int nR = -1; nR <= 1; nR++)
99  {
100  for (int nC = -1; nC <= 1; nC++)
101  {
102  const int neighborIndexC = c + nC;
103  const int neighborIndexR = r + nR;
104  if (neighborIndexC < 0 || neighborIndexR < 0 || (nR == 0 && nC == 0))
105  {
106  continue;
107  }
108 
109  if (neighborIndexR >= static_cast<int>(rows) ||
110  neighborIndexC >= static_cast<int>(cols))
111  {
112  continue;
113  }
114 
115  grid[neighborIndexR][neighborIndexC]->successors.push_back(candidate);
116  }
117  }
118  }
119  }
120 
121  ARMARX_VERBOSE << "Done.";
122  }
123 
124  bool
125  AStarPlanner::fulfillsConstraints(const NodePtr& n)
126  {
127  ARMARX_TRACE;
129 
130  return not costmap.isInCollision(n->position);
131  }
132 
133  NodePtr
134  AStarPlanner::closestNode(const Eigen::Vector2f& v)
135  {
136  const auto vertex = costmap.toVertex(v);
137 
138  const int r = vertex.index.x();
139  const int c = vertex.index.y();
140 
143 
144  const int rows = static_cast<int>(costmap.getGrid().rows());
145  const int cols = static_cast<int>(costmap.getGrid().cols());
146 
147  ARMARX_CHECK_LESS_EQUAL(r, rows - 1);
148  ARMARX_CHECK_LESS_EQUAL(c, cols - 1);
149 
150  return grid[static_cast<int>(r)][static_cast<int>(c)];
151  }
152 
153  std::vector<Eigen::Vector2f>
154  AStarPlanner::plan(const Eigen::Vector2f& start, const Eigen::Vector2f& goal)
155  {
156  ARMARX_TRACE;
157  grid.clear();
158  std::vector<Eigen::Vector2f> result;
159 
160  createUniformGrid();
161  ARMARX_DEBUG << "Setting start node for position " << start;
162  NodePtr nodeStart = closestNode(start);
163  // FIXME activate ARMARX_CHECK(fulfillsConstraints(nodeStart)) << "Start node in collision!";
164 
165  ARMARX_DEBUG << "Setting goal node for position " << goal;
166  NodePtr nodeGoal = closestNode(goal);
167  ARMARX_CHECK(fulfillsConstraints(nodeGoal)) << "Goal node in collision!";
168 
169  std::vector<NodePtr> closedSet;
170 
171  std::vector<NodePtr> openSet;
172  openSet.push_back(nodeStart);
173 
174  std::map<NodePtr, float> gScore;
175  gScore[nodeStart] = 0;
176 
177  std::map<NodePtr, float> fScore;
178  fScore[nodeStart] = gScore[nodeStart] + heuristic(nodeStart, nodeGoal);
179 
180  while (!openSet.empty())
181  {
182  float lowestScore = fScore[openSet[0]];
183  auto currentIT = openSet.begin();
184  for (auto it = openSet.begin() + 1; it != openSet.end(); it++)
185  {
186  if (fScore[*it] < lowestScore)
187  {
188  lowestScore = fScore[*it];
189  currentIT = it;
190  }
191  }
192  NodePtr currentBest = *currentIT;
193  if (currentBest == nodeGoal)
194  {
195  break;
196  }
197 
198  openSet.erase(currentIT);
199  closedSet.push_back(currentBest);
200  for (size_t i = 0; i < currentBest->successors.size(); i++)
201  {
202  NodePtr neighbor = currentBest->successors[i];
203  if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end())
204  {
205  continue;
206  }
207 
208  float tentativeGScore = gScore[currentBest] + costs(currentBest, neighbor);
209  bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
210  if (notInOS || tentativeGScore < gScore[neighbor])
211  {
212  neighbor->predecessor = currentBest;
213  gScore[neighbor] = tentativeGScore;
214  fScore[neighbor] = tentativeGScore + heuristic(neighbor, nodeGoal);
215  if (notInOS)
216  {
217  openSet.push_back(neighbor);
218  }
219  }
220  }
221  }
222 
223  // Found solution, now retrieve path from goal to start
224  if (nodeGoal)
225  {
226  result = nodeGoal->traversePredecessors();
227  }
228 
229  // Since the graph was traversed from goal to start, we have to reverse the order
230  ranges::reverse(result);
231 
232  return result;
233  }
234 
235  float
236  AStarPlanner::costs(const NodePtr& n1, const NodePtr& n2) const
237  {
238  const float euclideanDistance = (n1->position - n2->position).norm();
239 
240  // additional costs if
241  const float obstacleDistanceN1 = std::min(n1->obstacleDistance, maxObstacleDistance);
242  const float obstacleDistanceN2 = std::min(n2->obstacleDistance, maxObstacleDistance);
243 
244  // const float obstacleProximityChangeCosts = std::max(obstacleDistanceN1 - obstacleDistanceN2, 0.F);
245  const float obstacleProximityChangeCosts = obstacleDistanceN1 - obstacleDistanceN2;
246 
247  const float costs =
248  euclideanDistance + obstacleProximityChangeCosts * obstacleDistanceWeightFactor;
249  return std::max(costs, 0.F);
250  }
251 
252 
253 } // namespace armarx::navigation::algorithm::astar
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::navigation::algorithm::astar::intersects
bool intersects(const VirtualRobot::BoundingBox &bb1, const VirtualRobot::BoundingBox &bb2)
Definition: AStarPlanner.cpp:33
basic_types.h
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::navigation::algorithms::Costmap::Vertex::index
Index index
Definition: Costmap.h:61
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::navigation::algorithms::Costmap::toPositionGlobal
Position toPositionGlobal(const Index &index) const
Definition: Costmap.cpp:71
armarx::navigation::algorithm::astar::AStarPlanner::plan
std::vector< Eigen::Vector2f > plan(const Eigen::Vector2f &start, const Eigen::Vector2f &goal)
Definition: AStarPlanner.cpp:154
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::reverse
T reverse(const T &o)
Definition: container.h:32
armarx::navigation::algorithm::astar::NodePtr
std::shared_ptr< Node > NodePtr
Definition: Node.h:13
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::navigation::algorithm::astar::AStarPlanner::AStarPlanner
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AStarPlanner(const algorithms::Costmap &costmap)
Definition: AStarPlanner.cpp:47
AStarPlanner.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::navigation::algorithm::astar
Definition: AStarPlanner.cpp:29
armarx::navigation::algorithms::Costmap::toVertex
Vertex toVertex(const Position &globalPosition) const
Definition: Costmap.cpp:99
armarx::navigation::algorithms::Costmap::getGrid
const Grid & getGrid() const
Definition: Costmap.cpp:236
max
T max(T t1, T t2)
Definition: gdiam.h:48
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
ARMARX_CHECK_LESS_EQUAL
#define ARMARX_CHECK_LESS_EQUAL(lhs, rhs)
This macro evaluates whether lhs is less or equal (<=) rhs and if it turns out to be false it will th...
Definition: ExpressionException.h:109
ExpressionException.h
armarx::euclideanDistance
float euclideanDistance(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2)
Returns the euclidean distance.
Definition: Metrics.h:94
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
F
Definition: ExportDialogControllerTest.cpp:16
eigen.h
Logging.h
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::navigation::util::geometry::toPoint
point_type toPoint(const Eigen::Vector2f &pt)
Definition: geometry.cpp:42
armarx::navigation::algorithms::Costmap::isInCollision
bool isInCollision(const Position &p) const
Definition: Costmap.cpp:242
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13
norm
double norm(const Point &a)
Definition: point.hpp:94