GCC Code Coverage Report

Directory: ./
File: src/astar.hh
Date: 2024-08-10 11:29:48
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 7 Astar(const RoadmapPtr_t& roadmap, const DistancePtr_t distance)
57 7 : roadmap_(roadmap), distance_(distance) {}
58
59 7 void solution(PathVectorPtr_t sol) {
60
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 NodePtr_t node = findPath();
61 7 Edges_t edges;
62
63
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 7 times.
 35 while (node) {
64
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 Parent_t::const_iterator itEdge = parent_.find(node);
65
2/2
✓ Branch 3 taken 21 times.
✓ Branch 4 taken 7 times.
 28 if (itEdge != parent_.end()) {
66 21 EdgePtr_t edge = itEdge->second;
67
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 edges.push_front(edge);
68 21 node = edge->from();
69 } else
70 7 node = NodePtr_t(0x0);
71 }
72
2/2
✓ Branch 5 taken 21 times.
✓ Branch 6 taken 7 times.
 28 for (Edges_t::const_iterator itEdge = edges.begin(); itEdge != edges.end();
73 21 ++itEdge) {
74 21 const PathPtr_t& path((*itEdge)->path());
75
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 sol->appendPath(path);
76 21 }
77 7 }
78
79 private:
80 struct SortFunctor {
81 std::map<NodePtr_t, value_type>& cost_;
82 50 SortFunctor(std::map<NodePtr_t, value_type>& cost) : cost_(cost) {}
83 89 bool operator()(const NodePtr_t& n1, const NodePtr_t& n2) {
84 89 return cost_[n1] < cost_[n2];
85 }
86 }; // struc SortFunctor
87
88 50 bool isGoal(const NodePtr_t node) {
89 50 for (NodeVector_t::const_iterator itGoal = roadmap_->goalNodes().begin();
90
2/2
✓ Branch 5 taken 50 times.
✓ Branch 6 taken 43 times.
 93 itGoal != roadmap_->goalNodes().end(); ++itGoal) {
91
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 43 times.
 50 if (*itGoal == node) {
92 7 return true;
93 }
94 }
95 43 return false;
96 }
97
98 7 NodePtr_t findPath() {
99 7 closed_.clear();
100 7 open_.clear();
101 7 parent_.clear();
102 7 estimatedCostToGoal_.clear();
103 7 costFromStart_.clear();
104
105
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 open_.push_back(roadmap_->initNode());
106
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 while (!open_.empty()) {
107
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 open_.sort(SortFunctor(estimatedCostToGoal_));
108 50 Nodes_t::iterator itv = open_.begin();
109 50 NodePtr_t current(*itv);
110
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 43 times.
 50 if (isGoal(current)) {
111 7 return current;
112 }
113 43 open_.erase(itv);
114
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 closed_.push_back(current);
115
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 for (Edges_t::const_iterator itEdge = current->outEdges().begin();
116
3/4
✓ Branch 2 taken 125 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 82 times.
✓ Branch 7 taken 43 times.
 125 itEdge != current->outEdges().end(); ++itEdge) {
117
1/2
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
 82 value_type transitionCost = edgeCost(*itEdge);
118 82 NodePtr_t child((*itEdge)->to());
119
3/4
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46 times.
✓ Branch 8 taken 36 times.
 82 if (std::find(closed_.begin(), closed_.end(), child) == closed_.end()) {
120 // node is not in closed set
121
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 value_type tmpCost = costFromStart_[current] + transitionCost;
122 bool childNotInOpenSet =
123
1/2
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
 46 (std::find(open_.begin(), open_.end(), child) == open_.end());
124
2/8
✗ Branch 0 not taken.
✓ Branch 1 taken 46 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 46 times.
✗ Branch 8 not taken.
 46 if ((childNotInOpenSet) || (tmpCost < costFromStart_[child])) {
125
1/2
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
 46 parent_[child] = *itEdge;
126
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 costFromStart_[child] = tmpCost;
127 92 estimatedCostToGoal_[child] =
128
3/6
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 46 times.
✗ Branch 8 not taken.
 46 costFromStart_[child] + heuristic(child);
129
2/4
✓ Branch 0 taken 46 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 46 times.
✗ Branch 4 not taken.
 46 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 46 value_type heuristic(const NodePtr_t node) const {
138
2/4
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
 46 Configuration_t config = node->configuration();
139 46 value_type res = std::numeric_limits<value_type>::infinity();
140 46 for (NodeVector_t::const_iterator itGoal = roadmap_->goalNodes().begin();
141
2/2
✓ Branch 5 taken 46 times.
✓ Branch 6 taken 46 times.
 92 itGoal != roadmap_->goalNodes().end(); ++itGoal) {
142
2/4
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46 times.
✗ Branch 6 not taken.
 46 Configuration_t goal = (*itGoal)->configuration();
143
3/6
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 46 times.
✗ Branch 9 not taken.
 46 value_type dist = (*distance_)(config, goal);
144
1/2
✓ Branch 0 taken 46 times.
✗ Branch 1 not taken.
 46 if (dist < res) {
145 46 res = dist;
146 }
147 46 }
148 46 return res;
149 46 }
150
151
1/2
✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
 82 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