GCC Code Coverage Report


Directory: ./
File: src/astar.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 0 62 0.0%
Branches: 0 94 0.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #ifndef HPP_MANIPULATION_ASTAR_HH
31 #define HPP_MANIPULATION_ASTAR_HH
32
33 #include <hpp/core/distance.hh>
34 #include <hpp/core/edge.hh>
35 #include <hpp/core/node.hh>
36 #include <hpp/manipulation/fwd.hh>
37 #include <hpp/manipulation/graph/state-selector.hh>
38 #include <hpp/manipulation/roadmap-node.hh>
39 #include <limits>
40 // # include <hpp/core/path-vector.hh>
41
42 namespace hpp {
43 namespace manipulation {
44 class Astar {
45 public:
46 typedef std::map<RoadmapNodePtr_t, value_type> CostMap_t;
47 struct CostMapCompFunctor {
48 CostMap_t& cost_;
49 CostMapCompFunctor(CostMap_t& cost) : cost_(cost) {}
50 bool operator()(const RoadmapNodePtr_t& n1, const RoadmapNodePtr_t& n2) {
51 return cost_[n1] < cost_[n2];
52 }
53 bool operator()(const RoadmapNodePtr_t& n1, const value_type& val) {
54 return cost_[n1] < val;
55 }
56 }; // struc CostMapCompFunctor
57
58 typedef std::list<graph::StatePtr_t> States_t;
59 typedef std::list<RoadmapNodePtr_t> RoadmapNodes_t;
60 typedef std::list<core::EdgePtr_t> RoadmapEdges_t;
61 typedef std::map<RoadmapNodePtr_t, core::EdgePtr_t> Parent_t;
62
63 Astar(const core::DistancePtr_t distance,
64 const graph::StateSelectorPtr_t& stateSelector, RoadmapNodePtr_t from)
65 : distance_(distance), selector_(stateSelector), from_(from) {
66 open_.push_back(from);
67 costFromStart_[from] = 0;
68 }
69
70 States_t solution(RoadmapNodePtr_t to) {
71 if (parent_.find(to) != parent_.end() || findPath(to)) {
72 RoadmapNodePtr_t node = to;
73 States_t states;
74
75 states.push_front(selector_->getState(to));
76 while (node) {
77 Parent_t::const_iterator itNode = parent_.find(node);
78 if (itNode != parent_.end()) {
79 node = static_cast<RoadmapNodePtr_t>(itNode->second->from());
80 states.push_front(selector_->getState(node));
81 } else
82 node = RoadmapNodePtr_t(0);
83 }
84 // We may want to clean it a little
85 // std::unique (states.begin(), states.end ());
86
87 states.push_front(selector_->getState(from_));
88 return states;
89 }
90 return States_t();
91 }
92
93 private:
94 bool findPath(RoadmapNodePtr_t to) {
95 // Recompute the estimated cost to goal
96 for (CostMap_t::iterator it = estimatedCostToGoal_.begin();
97 it != estimatedCostToGoal_.end(); ++it) {
98 it->second = getCostFromStart(it->first) + heuristic(it->first, to);
99 }
100 open_.sort(CostMapCompFunctor(estimatedCostToGoal_));
101
102 while (!open_.empty()) {
103 RoadmapNodes_t::iterator itv = open_.begin();
104 RoadmapNodePtr_t current(*itv);
105 if (current == to) {
106 return true;
107 }
108 open_.erase(itv);
109 closed_.push_back(current);
110 for (RoadmapEdges_t::const_iterator itEdge = current->outEdges().begin();
111 itEdge != current->outEdges().end(); ++itEdge) {
112 RoadmapNodePtr_t child = static_cast<RoadmapNodePtr_t>((*itEdge)->to());
113 if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
114 // node is not in closed set
115 value_type transitionCost = edgeCost(*itEdge);
116 value_type tmpCost = getCostFromStart(current) + transitionCost;
117 bool childNotInOpenSet =
118 (std::find(open_.begin(), open_.end(), child) == open_.end());
119 if ((childNotInOpenSet) || (tmpCost < getCostFromStart(child))) {
120 parent_[child] = *itEdge;
121 costFromStart_[child] = tmpCost;
122 value_type estimatedCost = tmpCost + heuristic(child, to);
123 estimatedCostToGoal_[child] = estimatedCost;
124 if (childNotInOpenSet) {
125 // Find the first element not strictly smaller than child
126 RoadmapNodes_t::iterator pos =
127 std::lower_bound(open_.begin(), open_.end(), estimatedCost,
128 CostMapCompFunctor(estimatedCostToGoal_));
129 open_.insert(pos, child);
130 }
131 }
132 }
133 }
134 }
135 return false;
136 }
137
138 inline value_type heuristic(RoadmapNodePtr_t node,
139 RoadmapNodePtr_t to) const {
140 const Configuration_t& config = node->configuration();
141 return (*distance_)(config, to->configuration());
142 }
143
144 inline value_type edgeCost(const core::EdgePtr_t& edge) const {
145 return edge->path()->length();
146 }
147
148 value_type getCostFromStart(RoadmapNodePtr_t to) const {
149 CostMap_t::const_iterator it = costFromStart_.find(to);
150 if (it == costFromStart_.end())
151 return std::numeric_limits<value_type>::max();
152 return it->second;
153 }
154
155 RoadmapNodes_t closed_;
156 RoadmapNodes_t open_;
157 std::map<RoadmapNodePtr_t, value_type> costFromStart_;
158 std::map<RoadmapNodePtr_t, value_type> estimatedCostToGoal_;
159 Parent_t parent_;
160 core::DistancePtr_t distance_;
161 graph::StateSelectorPtr_t selector_;
162 RoadmapNodePtr_t from_;
163
164 }; // class Astar
165 } // namespace manipulation
166 } // namespace hpp
167
168 #endif // HPP_MANIPULATION_ASTAR_HH
169