GCC Code Coverage Report


Directory: ./
File: src/problem-target/goal-configurations.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 31 34 91.2%
Branches: 15 38 39.5%

Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/core/config-validations.hh>
30 #include <hpp/core/connected-component.hh>
31 #include <hpp/core/problem-target/goal-configurations.hh>
32 #include <hpp/core/problem.hh>
33 #include <hpp/core/roadmap.hh>
34 #include <hpp/util/debug.hh>
35 #include <stdexcept>
36
37 #include "../astar.hh"
38
39 namespace hpp {
40 namespace core {
41 namespace problemTarget {
42 62 GoalConfigurationsPtr_t GoalConfigurations::create(
43 const ProblemPtr_t& problem) {
44 62 GoalConfigurations* gc = new GoalConfigurations(problem);
45 62 GoalConfigurationsPtr_t shPtr(gc);
46 62 gc->init(shPtr);
47 62 return shPtr;
48 }
49
50 8 void GoalConfigurations::check(const RoadmapPtr_t& /*roadmap*/) const {}
51
52 32 bool GoalConfigurations::reached(const RoadmapPtr_t& roadmap) const {
53
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
32 if (!roadmap->initNode()) return false;
54 const ConnectedComponentPtr_t ccInit =
55
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
32 roadmap->initNode()->connectedComponent();
56 32 const NodeVector_t& goals = roadmap->goalNodes();
57
2/2
✓ Branch 3 taken 32 times.
✓ Branch 4 taken 24 times.
56 for (NodeVector_t::const_iterator _goal = goals.begin(); _goal != goals.end();
58 24 ++_goal) {
59
4/6
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✓ Branch 10 taken 24 times.
32 if (ccInit->canReach((*_goal)->connectedComponent())) {
60 8 return true;
61 }
62 }
63 24 return false;
64 32 }
65
66 8 PathVectorPtr_t GoalConfigurations::computePath(
67 const RoadmapPtr_t& roadmap) const {
68 8 ProblemPtr_t problem(problem_.lock());
69
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
8 assert(problem);
70 8 Astar astar(roadmap, problem->distance());
71 8 PathVectorPtr_t sol = PathVector::create(problem->robot()->configSize(),
72
3/6
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 8 times.
✗ Branch 13 not taken.
16 problem->robot()->numberDof());
73
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
8 astar.solution(sol);
74 // This happens when q_init == q_goal
75
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
8 if (sol->numberPaths() == 0) {
76 Configuration_t q(roadmap->initNode()->configuration());
77 sol->appendPath((*problem->steeringMethod())(q, q));
78 }
79 16 return sol;
80 8 }
81 // ======================================================================
82
83 26 const Configurations_t& GoalConfigurations::configurations() const {
84 26 return configurations_;
85 }
86
87 // ======================================================================
88
89 9 void GoalConfigurations::addConfiguration(ConfigurationIn_t config) {
90
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
9 configurations_.push_back(config);
91 9 }
92
93 // ======================================================================
94
95 void GoalConfigurations::resetConfigurations() { configurations_.clear(); }
96
97 } // namespace problemTarget
98 } // namespace core
99 } // namespace hpp
100