41 #ifndef OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H 42 #define OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H 49 #include <unordered_map> 56 #include <boost/version.hpp> 57 #if BOOST_VERSION == 106000 58 #include <boost/type_traits/ice.hpp> 61 #include <boost/graph/adjacency_matrix.hpp> 62 #include <boost/graph/adjacency_list.hpp> 67 template <
typename Graph,
72 LPAstarOnGraph(std::size_t source, std::size_t target, Graph &graph, Heuristic &h)
73 : costEstimator_(h), graph_(graph)
76 double c = std::numeric_limits<double>::infinity();
77 source_ =
new Node(c, costEstimator_(source), 0, source);
79 target_ =
new Node(c, 0, c, target);
88 void insertEdge(std::size_t u, std::size_t v,
double c)
90 Node *n_u = getNode(u);
91 Node *n_v = getNode(v);
93 if (n_v->rhs() > n_u->costToCome() + c)
96 n_v->setRhs(n_u->costToCome() + c);
100 void removeEdge(std::size_t u, std::size_t v)
102 assert(v != source_->getId());
104 Node *n_u = getNode(u);
105 Node *n_v = getNode(v);
107 if (n_v->getParent() == n_u)
109 WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
110 chooseBestIncomingNode(n_v, weights);
115 double computeShortestPath(std::list<std::size_t> &path)
117 WeightMap weights = boost::get(boost::edge_weight_t(), graph_);
120 return std::numeric_limits<double>::infinity();
122 while (topHead()->key() < target_->calculateKey() || target_->rhs() != target_->costToCome())
127 if (u->costToCome() > u->rhs())
129 u->setCostToCome(u->rhs());
133 typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
134 for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
136 std::size_t v = boost::target(*ei, graph_);
137 Node *n_v = getNode(v);
138 double c = boost::get(weights, *ei);
140 if (n_v->rhs() > u->costToCome() + c)
143 n_v->setRhs(u->costToCome() + c);
150 u->setCostToCome(std::numeric_limits<double>::infinity());
154 typename boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
155 for (boost::tie(ei, ei_end) = boost::out_edges(u->getId(), graph_); ei != ei_end; ++ei)
157 std::size_t v = boost::target(*ei, graph_);
158 Node *n_v = getNode(v);
160 if ((n_v == source_) || (n_v->getParent() != u))
163 chooseBestIncomingNode(n_v, weights);
173 Node *res = (target_->costToCome() == std::numeric_limits<double>::infinity() ? nullptr : target_);
174 while (res !=
nullptr)
176 path.push_front(res->getId());
177 res = res->getParent();
180 return target_->costToCome();
186 auto iter = idNodeMap_.find(u);
187 if (iter != idNodeMap_.end())
188 return iter->second->costToCome();
189 return std::numeric_limits<double>::infinity();
195 Key(
double first_ = -1,
double second_ = -1) : first(first_), second(second_)
198 bool operator<(
const Key &other)
200 return (first != other.first) ? (first < other.first) : (second < other.second);
202 double first, second;
208 Node(
double costToCome,
double costToGo,
double rhs, std::size_t &dataId, Node *parentNode =
nullptr)
209 : g(costToCome), h(costToGo), r(rhs), isInQ(false), parent(parentNode), id(dataId)
214 double costToCome()
const 218 double costToGo()
const 232 k = Key(std::min(g, r + h), std::min(g, r));
236 double setCostToCome(
double val)
240 double setRhs(
double val)
245 bool isInQueue()
const 249 void inQueue(
bool in)
254 Node *getParent()
const 258 void setParent(Node *p)
263 std::size_t getId()
const 267 bool isConsistent()
const 284 bool operator()(
const Node *n1,
const Node *n2)
const 286 return n1->key() < n2->key();
292 std::size_t
operator()(
const std::size_t
id)
const 296 std::hash<std::size_t> h;
299 using Queue = std::multiset<Node *, LessThanNodeK>;
300 using IdNodeMap = std::unordered_map<std::size_t, Node *, Hash>;
301 using IdNodeMapIter =
typename IdNodeMap::iterator;
302 using WeightMap =
typename boost::property_map<Graph, boost::edge_weight_t>::type;
305 void updateVertex(Node *n)
307 if (!n->isConsistent())
314 else if (n->isInQueue())
322 queue_.erase(queue_.begin());
328 return *queue_.begin();
331 void insertQueue(Node *node)
333 assert(node->isInQueue() ==
false);
335 node->calculateKey();
339 void removeQueue(Node *node)
341 if (node->isInQueue())
343 node->inQueue(
false);
347 void updateQueue(Node *node)
353 void chooseBestIncomingNode(Node *n_v, WeightMap &weights)
356 double min = std::numeric_limits<double>::infinity();
357 Node *best =
nullptr;
359 typename boost::graph_traits<Graph>::in_edge_iterator ei, ei_end;
360 for (boost::tie(ei, ei_end) = boost::in_edges(n_v->getId(), graph_); ei != ei_end; ++ei)
362 std::size_t u = boost::source(*ei, graph_);
363 Node *n_u = getNode(u);
364 double c = boost::get(weights, *ei);
366 double curr = n_u->costToCome() + c;
375 n_v->setParent(best);
378 void addNewNode(Node *n)
380 idNodeMap_[n->getId()] = n;
383 Node *getNode(std::size_t
id)
385 auto iter = idNodeMap_.find(
id);
386 if (iter != idNodeMap_.end())
389 double c = std::numeric_limits<double>::infinity();
390 auto *n =
new Node(c, costEstimator_(
id), c,
id);
398 for (
auto iter = idNodeMap_.begin(); iter != idNodeMap_.end(); ++iter)
400 Node *n = iter->second;
405 Heuristic &costEstimator_;
410 IdNodeMap idNodeMap_;
415 #endif // OMPL_DATASTRUCTURES_LPA_STAR_ON_G_H
Main namespace. Contains everything in this library.
double operator()(std::size_t u)
using LPA* to approximate costToCome