GCC Code Coverage Report


Directory: ./
File: src/steering-method/reeds-shepp.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 16 21 76.2%
Branches: 20 42 47.6%

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/distance.hh>
30 #include <hpp/core/path-vector.hh>
31 #include <hpp/core/problem.hh>
32 #include <hpp/core/steering-method/reeds-shepp.hh>
33 #include <hpp/core/weighed-distance.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/pinocchio/joint.hh>
36 #include <pinocchio/multibody/joint/fwd.hpp>
37
38 namespace hpp {
39 namespace core {
40 namespace steeringMethod {
41 6 PathPtr_t ReedsShepp::impl_compute(ConfigurationIn_t q1,
42 ConfigurationIn_t q2) const {
43 // TODO this should not be done here.
44 // See todo in class ConstantCurvature
45
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
6 Configuration_t qEnd(q2);
46
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 qEnd.segment<2>(xyId_) = q1.segment<2>(xyId_);
47
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
6 qEnd.segment<2>(rzId_) = q1.segment<2>(rzId_);
48 // Do not take into account wheel joints in additional distance.
49 6 for (std::vector<JointPtr_t>::const_iterator it = wheels_.begin();
50
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 6 times.
8 it != wheels_.end(); ++it) {
51
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
2 size_type i = (*it)->rankInConfiguration();
52
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
2 qEnd[i] = q1[i];
53 }
54 // The length corresponding to the non RS DoF
55
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
6 value_type extraL = (*weighedDistance_)(q1, qEnd);
56
57 value_type distance;
58 PathVectorPtr_t path(
59 12 reedsSheppPathOrDistance(device_.lock(), q1, q2, extraL, rho_, xyId_,
60
4/8
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6 times.
✗ Branch 13 not taken.
18 rzId_, wheels_, constraints(), false, distance));
61 12 return path;
62 6 }
63
64 3 ReedsShepp::ReedsShepp(const ProblemConstPtr_t& problem)
65 : CarLike(problem),
66
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
3 weighedDistance_(WeighedDistance::create(problem->robot())),
67 6 weak_() {}
68
69 ReedsShepp::ReedsShepp(const ProblemConstPtr_t& problem,
70 const value_type turningRadius, JointPtr_t xyJoint,
71 JointPtr_t rzJoint, std::vector<JointPtr_t> wheels)
72 : CarLike(problem, turningRadius, xyJoint, rzJoint, wheels),
73 weighedDistance_(WeighedDistance::create(problem->robot())) {}
74
75 /// Copy constructor
76 ReedsShepp::ReedsShepp(const ReedsShepp& other)
77 : CarLike(other), weighedDistance_(other.weighedDistance_) {}
78
79 } // namespace steeringMethod
80 } // namespace core
81 } // namespace hpp
82