GCC Code Coverage Report


Directory: ./
File: src/path-optimizer.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 35 46 76.1%
Branches: 25 66 37.9%

Line Branch Exec Source
1 // Copyright (c) 2015, 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/path-optimizer.hh>
30 #include <hpp/core/path-projector.hh>
31 #include <hpp/core/problem.hh>
32 #include <hpp/core/steering-method.hh>
33
34 namespace bpt = boost::posix_time;
35
36 namespace hpp {
37 namespace core {
38 15 PathOptimizer::PathOptimizer(const ProblemConstPtr_t& problem)
39 15 : interrupt_(false),
40 15 problem_(problem),
41 15 maxIterations_(std::numeric_limits<unsigned long int>::infinity()),
42
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
30 timeOut_(std::numeric_limits<double>::infinity()) {
43 15 monitor_.enabled = false;
44
45
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
15 initFromParameters();
46 15 }
47
48 PathPtr_t PathOptimizer::steer(ConfigurationIn_t q1,
49 ConfigurationIn_t q2) const {
50 PathPtr_t dp = (*problem()->steeringMethod())(q1, q2);
51 if (dp) {
52 if (!problem()->pathProjector()) return dp;
53 PathPtr_t pp;
54 if (problem()->pathProjector()->apply(dp, pp)) return pp;
55 }
56 return PathPtr_t();
57 }
58
59 15 void PathOptimizer::monitorExecution() {
60 15 interrupt_ = false;
61 15 monitor_.enabled = true;
62 15 monitor_.iteration = 0;
63 15 monitor_.timeStart = bpt::microsec_clock::universal_time();
64 15 }
65
66 21 bool PathOptimizer::shouldStop() const {
67
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
21 if (interrupt_) return true;
68
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
21 if (!monitor_.enabled) return false;
69
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
21 if (monitor_.iteration >= maxIterations_) return true;
70
71
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
21 bpt::ptime timeStop(bpt::microsec_clock::universal_time());
72 21 if (static_cast<value_type>(
73
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
21 (timeStop - monitor_.timeStart).total_milliseconds()) >
74
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
21 timeOut_ * 1e3)
75 return true;
76 21 return false;
77 }
78
79 15 void PathOptimizer::initFromParameters() {
80 15 maxIterations_ =
81
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
15 problem()->getParameter("PathOptimizer/maxIterations").intValue();
82
3/6
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
15 timeOut_ = problem()->getParameter("PathOptimizer/timeOut").floatValue();
83 15 }
84
85 void PathOptimizer::maxIterations(const unsigned long int& n) {
86 maxIterations_ = n;
87 }
88
89 void PathOptimizer::timeOut(const double& timeOut) { timeOut_ = timeOut; }
90
91 // ----------- Declare parameters ------------------------------------- //
92
93 18 HPP_START_PARAMETER_DECLARATION(PathOptimizer)
94
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
18 Problem::declareParameter(
95
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
36 ParameterDescription(Parameter::INT, "PathOptimizer/maxIterations",
96 "Maximal number of iterations.",
97
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
36 Parameter(std::numeric_limits<size_type>::max())));
98
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
18 Problem::declareParameter(ParameterDescription(
99 Parameter::FLOAT, "PathOptimizer/timeOut",
100 "Duration in seconds above which execution will stop."
101 "The iteration at the moment the duration is elapsed will be completed.",
102
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
36 Parameter(std::numeric_limits<double>::infinity())));
103 18 HPP_END_PARAMETER_DECLARATION(PathOptimizer)
104 } // namespace core
105 } // namespace hpp
106