8 #include <boost/geometry.hpp>
9 #include <boost/geometry/algorithms/detail/intersects/interface.hpp>
11 #include <Eigen/Geometry>
13 #include <IceUtil/Time.h>
15 #include <VirtualRobot/BoundingBox.h>
16 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
17 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
18 #include <VirtualRobot/XML/ObjectIO.h>
23 #include "range/v3/algorithm/reverse.hpp"
27 namespace bg = boost::geometry;
33 intersects(
const VirtualRobot::BoundingBox& bb1,
const VirtualRobot::BoundingBox& bb2)
35 using point_t = bg::model::point<double, 3, bg::cs::cartesian>;
36 using box_t = bg::model::box<point_t>;
38 const auto toPoint = [](
const Eigen::Vector3f& pt)
39 {
return point_t(pt.x(), pt.y(), pt.z()); };
54 return (1.
F - obstacleDistanceWeightFactor) * (n1->position - n2->position).
norm();
58 AStarPlanner::createUniformGrid()
62 const size_t rows = costmap.
getGrid().rows();
63 const size_t cols = costmap.
getGrid().cols();
67 for (
size_t r = 0; r < rows; r++)
69 grid.push_back(std::vector<NodePtr>(cols));
73 for (
size_t r = 0; r < rows; r++)
75 for (
size_t c = 0;
c < cols;
c++)
78 const float obstacleDistance = costmap.
getGrid()(r,
c);
80 grid[r][
c] = std::make_shared<Node>(pos, obstacleDistance);
87 for (
size_t r = 0; r < rows; r++)
89 for (
size_t c = 0;
c < cols;
c++)
92 if (!fulfillsConstraints(candidate))
98 for (
int nR = -1; nR <= 1; nR++)
100 for (
int nC = -1; nC <= 1; nC++)
102 const int neighborIndexC =
c + nC;
103 const int neighborIndexR = r + nR;
104 if (neighborIndexC < 0 || neighborIndexR < 0 || (nR == 0 && nC == 0))
109 if (neighborIndexR >=
static_cast<int>(rows) ||
110 neighborIndexC >=
static_cast<int>(cols))
115 grid[neighborIndexR][neighborIndexC]->successors.push_back(candidate);
125 AStarPlanner::fulfillsConstraints(
const NodePtr& n)
134 AStarPlanner::closestNode(
const Eigen::Vector2f&
v)
138 const int r = vertex.
index.x();
139 const int c = vertex.index.y();
144 const int rows =
static_cast<int>(costmap.
getGrid().rows());
145 const int cols =
static_cast<int>(costmap.
getGrid().cols());
150 return grid[
static_cast<int>(r)][
static_cast<int>(
c)];
153 std::vector<Eigen::Vector2f>
158 std::vector<Eigen::Vector2f> result;
161 ARMARX_DEBUG <<
"Setting start node for position " << start;
162 NodePtr nodeStart = closestNode(start);
165 ARMARX_DEBUG <<
"Setting goal node for position " << goal;
166 NodePtr nodeGoal = closestNode(goal);
167 ARMARX_CHECK(fulfillsConstraints(nodeGoal)) <<
"Goal node in collision!";
169 std::vector<NodePtr> closedSet;
171 std::vector<NodePtr> openSet;
172 openSet.push_back(nodeStart);
174 std::map<NodePtr, float> gScore;
175 gScore[nodeStart] = 0;
177 std::map<NodePtr, float> fScore;
178 fScore[nodeStart] = gScore[nodeStart] + heuristic(nodeStart, nodeGoal);
180 while (!openSet.empty())
182 float lowestScore = fScore[openSet[0]];
183 auto currentIT = openSet.begin();
184 for (
auto it = openSet.begin() + 1; it != openSet.end(); it++)
186 if (fScore[*it] < lowestScore)
188 lowestScore = fScore[*it];
192 NodePtr currentBest = *currentIT;
193 if (currentBest == nodeGoal)
198 openSet.erase(currentIT);
199 closedSet.push_back(currentBest);
200 for (
size_t i = 0; i < currentBest->successors.size(); i++)
202 NodePtr neighbor = currentBest->successors[i];
203 if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end())
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])
212 neighbor->predecessor = currentBest;
213 gScore[neighbor] = tentativeGScore;
214 fScore[neighbor] = tentativeGScore + heuristic(neighbor, nodeGoal);
217 openSet.push_back(neighbor);
226 result = nodeGoal->traversePredecessors();
241 const float obstacleDistanceN1 =
std::min(n1->obstacleDistance, maxObstacleDistance);
242 const float obstacleDistanceN2 =
std::min(n2->obstacleDistance, maxObstacleDistance);
245 const float obstacleProximityChangeCosts = obstacleDistanceN1 - obstacleDistanceN2;