GCC Code Coverage Report

Directory: ./
File: include/hpp/core/steering-method/reeds-shepp.hh
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 9 15 60.0%
Branches: 1 6 16.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Joseph Mirabel
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_STEERING_METHOD_REEDS_SHEPP_HH
31 #define HPP_CORE_STEERING_METHOD_REEDS_SHEPP_HH
32
33 #include <hpp/core/config.hh>
34 #include <hpp/core/fwd.hh>
35 #include <hpp/core/steering-method/car-like.hh>
36 #include <hpp/util/debug.hh>
37 #include <hpp/util/pointer.hh>
38
39 namespace hpp {
40 namespace core {
41 namespace steeringMethod {
42 /// \addtogroup steering_method
43 /// \{
44
45 /// Steering method that creates ReedsSheppPath instances
46 ///
47 class HPP_CORE_DLLAPI ReedsShepp : public CarLike {
48 public:
49 /// Create an instance
50 ///
51 /// This constructor assumes that:
52 /// - the 2 parameters of the configurations corresponds to the XY
53 /// translation joint,
54 /// - the 2 following parameters corresponds to the RZ unbounded
55 /// rotation joint.
56 /// Use Carlike::setWheelJoints to set the wheel joints.
57 3 static ReedsSheppPtr_t createWithGuess(const ProblemConstPtr_t& problem) {
58
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ReedsShepp* ptr = new ReedsShepp(problem);
59 3 ReedsSheppPtr_t shPtr(ptr);
60 3 ptr->init(shPtr);
61 3 return shPtr;
62 }
63
64 /// Create an instance
65 ///
66 /// This constructor does no assumption.
67 static ReedsSheppPtr_t create(
68 const ProblemConstPtr_t& problem, const value_type turningRadius,
69 JointPtr_t xyJoint, JointPtr_t rzJoint,
70 std::vector<JointPtr_t> wheels = std::vector<JointPtr_t>()) {
71 ReedsShepp* ptr =
72 new ReedsShepp(problem, turningRadius, xyJoint, rzJoint, wheels);
73 ReedsSheppPtr_t shPtr(ptr);
74 ptr->init(shPtr);
75 return shPtr;
76 }
77
78 /// Copy instance and return shared pointer
79 ✗ static ReedsSheppPtr_t createCopy(const ReedsSheppPtr_t& other) {
80 ✗ ReedsShepp* ptr = new ReedsShepp(*other);
81 ✗ ReedsSheppPtr_t shPtr(ptr);
82 ✗ ptr->init(shPtr);
83 ✗ return shPtr;
84 }
85
86 /// Copy instance and return shared pointer
87 ✗ virtual SteeringMethodPtr_t copy() const { return createCopy(weak_.lock()); }
88
89 /// create a path between two configurations
90 virtual PathPtr_t impl_compute(ConfigurationIn_t q1,
91 ConfigurationIn_t q2) const;
92
93 protected:
94 /// Constructor
95 ReedsShepp(const ProblemConstPtr_t& problem);
96
97 /// Constructor
98 ReedsShepp(const ProblemConstPtr_t& problem, const value_type turningRadius,
99 JointPtr_t xyJoint, JointPtr_t rzJoint,
100 std::vector<JointPtr_t> wheels);
101
102 /// Copy constructor
103 ReedsShepp(const ReedsShepp& other);
104
105 /// Store weak pointer to itself
106 3 void init(ReedsSheppWkPtr_t weak) {
107 3 CarLike::init(weak);
108 3 weak_ = weak;
109 3 }
110
111 private:
112 WeighedDistancePtr_t weighedDistance_;
113 ReedsSheppWkPtr_t weak_;
114 }; // class ReedsShepp
115
116 /// Create a Reeds and Shepp path and return shared pointer
117 /// \param device Robot corresponding to configurations,
118 /// \param init, end start and end configurations of the path,
119 /// \param extraLength the length of the path due to the non RS DoF,
120 /// \param rho The radius of a turn,
121 /// \param xyId, rzId indices in configuration vector of the joints
122 /// corresponding to the translation and rotation of the car.
123 PathVectorPtr_t reedsSheppPathOrDistance(
124 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
125 value_type extraLength, value_type rho, size_type xyId, size_type rzId,
126 const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints,
127 bool computeDistance, value_type& distance);
128
129 /// \}
130 } // namespace steeringMethod
131 } // namespace core
132 } // namespace hpp
133 #endif // HPP_CORE_STEERING_METHOD_REEDS_SHEPP_HH
134