GCC Code Coverage Report

Directory: ./
File: src/astar.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 68 69 98.6%
Branches: 50 88 56.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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_CORE_ASTAR_HH
31 #define HPP_CORE_ASTAR_HH
32
33 #include <hpp/core/config.hh>
34 #include <hpp/core/distance.hh>
35 #include <hpp/core/edge.hh>
36 #include <hpp/core/fwd.hh>
37 #include <hpp/core/node.hh>
38 #include <hpp/core/path-vector.hh>
39 #include <limits>
40
41 namespace hpp {
42 namespace core {
43 class HPP_CORE_LOCAL Astar {
44 typedef std::list<NodePtr_t> Nodes_t;
45 typedef std::list<EdgePtr_t> Edges_t;
46 typedef std::map<NodePtr_t, EdgePtr_t> Parent_t;
47 Nodes_t closed_;
48 Nodes_t open_;
49 std::map<NodePtr_t, value_type> costFromStart_;
50 std::map<NodePtr_t, value_type> estimatedCostToGoal_;
51 Parent_t parent_;
52 RoadmapPtr_t roadmap_;
53 DistancePtr_t distance_;
54
55 public:
56 8 Astar(const RoadmapPtr_t& roadmap, const DistancePtr_t distance)
57 8 : roadmap_(roadmap), distance_(distance) {}
58
59 8 void solution(PathVectorPtr_t sol) {
60
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 NodePtr_t node = findPath();
61 8 Edges_t edges;
62
63
2/2
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 8 times.
 41 while (node) {
64
1/2
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
 33 Parent_t::const_iterator itEdge = parent_.find(node);
65
2/2
✓ Branch 3 taken 25 times.
✓ Branch 4 taken 8 times.
 33 if (itEdge != parent_.end()) {
66 25 EdgePtr_t edge = itEdge->second;
67
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 edges.push_front(edge);
68 25 node = edge->from();
69 } else
70 8 node = NodePtr_t(0x0);
71 }
72
2/2
✓ Branch 5 taken 25 times.
✓ Branch 6 taken 8 times.
 33 for (Edges_t::const_iterator itEdge = edges.begin(); itEdge != edges.end();
73 25 ++itEdge) {
74 25 const PathPtr_t& path((*itEdge)->path());
75
1/2
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
 25 sol->appendPath(path);
76 25 }
77 8 }
78
79 private:
80 struct SortFunctor {
81 std::map<NodePtr_t, value_type>& cost_;
82 55 SortFunctor(std::map<NodePtr_t, value_type>& cost) : cost_(cost) {}
83 91 bool operator()(const NodePtr_t& n1, const NodePtr_t& n2) {
84 91 return cost_[n1] < cost_[n2];
85 }
86 }; // struc SortFunctor
87
88 55 bool isGoal(const NodePtr_t node) {
89 55 for (NodeVector_t::const_iterator itGoal = roadmap_->goalNodes().begin();
90
2/2
✓ Branch 5 taken 55 times.
✓ Branch 6 taken 47 times.
 102 itGoal != roadmap_->goalNodes().end(); ++itGoal) {
91
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 47 times.
 55 if (*itGoal == node) {
92 8 return true;
93 }
94 }
95 47 return false;
96 }
97
98 8 NodePtr_t findPath() {
99 8 closed_.clear();
100 8 open_.clear();
101 8 parent_.clear();
102 8 estimatedCostToGoal_.clear();
103 8 costFromStart_.clear();
104
105
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 open_.push_back(roadmap_->initNode());
106
1/2
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
 55 while (!open_.empty()) {
107
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
 55 open_.sort(SortFunctor(estimatedCostToGoal_));
108 55 Nodes_t::iterator itv = open_.begin();
109 55 NodePtr_t current(*itv);
110
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 47 times.
 55 if (isGoal(current)) {
111 8 return current;
112 }
113 47 open_.erase(itv);
114
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 closed_.push_back(current);
115
1/2
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
 47 for (Edges_t::const_iterator itEdge = current->outEdges().begin();
116
3/4
✓ Branch 2 taken 136 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 89 times.
✓ Branch 7 taken 47 times.
 136 itEdge != current->outEdges().end(); ++itEdge) {
117
1/2
✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
 89 value_type transitionCost = edgeCost(*itEdge);
118 89 NodePtr_t child((*itEdge)->to());
119
3/4
✓ Branch 4 taken 89 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✓ Branch 8 taken 38 times.
 89 if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
120 // node is not in closed set
121
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 value_type tmpCost = costFromStart_[current] + transitionCost;
122 bool childNotInOpenSet =
123
1/2
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 (std::find(open_.begin(), open_.end(), child) == open_.end());
124
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 51 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 if ((childNotInOpenSet) || (tmpCost < costFromStart_[child])) {
125
1/2
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
 51 parent_[child] = *itEdge;
126
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 costFromStart_[child] = tmpCost;
127 102 estimatedCostToGoal_[child] =
128
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 costFromStart_[child] + heuristic(child);
129
2/4
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 51 if (childNotInOpenSet) open_.push_back(child);
130 }
131 }
132 }
133 }
134 ✗ throw std::runtime_error("A* failed to find a solution to the goal.");
135 }
136
137 51 value_type heuristic(const NodePtr_t node) const {
138
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 Configuration_t config = node->configuration();
139 51 value_type res = std::numeric_limits<value_type>::infinity();
140 51 for (NodeVector_t::const_iterator itGoal = roadmap_->goalNodes().begin();
141
2/2
✓ Branch 5 taken 51 times.
✓ Branch 6 taken 51 times.
 102 itGoal != roadmap_->goalNodes().end(); ++itGoal) {
142
2/4
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 Configuration_t goal = (*itGoal)->configuration();
143
3/6
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 51 times.
✗ Branch 9 not taken.
 51 value_type dist = (*distance_)(config, goal);
144
1/2
✓ Branch 0 taken 51 times.
✗ Branch 1 not taken.
 51 if (dist < res) {
145 51 res = dist;
146 }
147 51 }
148 51 return res;
149 51 }
150
151
1/2
✓ Branch 3 taken 89 times.
✗ Branch 4 not taken.
 89 value_type edgeCost(const EdgePtr_t& edge) { return edge->path()->length(); }
152 }; // class Astar
153 } // namespace core
154 } // namespace hpp
155
156 #endif // HPP_CORE_ASTAR_HH
157