GCC Code Coverage Report

Directory: ./
File: src/steering-method/car-like.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 35 60 58.3%
Branches: 27 76 35.5%
Line Branch Exec Source
1 // Copyright (c) 2017, CNRS
2 // Authors: Florent Lamiraux
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 <boost/algorithm/string.hpp>
30 #include <hpp/core/problem.hh>
31 #include <hpp/core/steering-method/car-like.hh>
32 #include <hpp/pinocchio/device.hh>
33 #include <hpp/pinocchio/joint-collection.hh>
34 #include <hpp/pinocchio/joint.hh>
35 #include <pinocchio/multibody/joint/joint-generic.hpp>
36 #include <pinocchio/spatial/se3.hpp>
37
38 namespace hpp {
39 namespace core {
40 namespace steeringMethod {
41 3 CarLike::CarLike(const ProblemConstPtr_t& problem)
42 : SteeringMethod(problem),
43 3 device_(problem->robot()),
44 3 rho_(1.),
45 3 xyId_(0),
46 6 rzId_(2) {
47 3 DevicePtr_t d(device_.lock());
48
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 xy_ = d->getJointAtConfigRank(0);
49
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 rz_ = d->getJointAtConfigRank(2);
50
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 wheels_ = getWheelsFromParameter(problem, rz_);
51
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 6 turningRadius(problem->getParameter("SteeringMethod/Carlike/turningRadius")
52
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 .floatValue());
53 3 }
54
55 ✗ CarLike::CarLike(const ProblemConstPtr_t& problem,
56 const value_type turningRadius, JointPtr_t xyJoint,
57 ✗ JointPtr_t rzJoint, std::vector<JointPtr_t> wheels)
58 : SteeringMethod(problem),
59 ✗ device_(problem->robot()),
60 ✗ rho_(turningRadius),
61 ✗ xy_(xyJoint),
62 ✗ rz_(rzJoint),
63 ✗ xyId_(xy_->rankInConfiguration()),
64 ✗ wheels_(wheels),
65 ✗ weak_() {
66 ✗ if (rz_->jointModel().shortname() == "JointModelPlanar") {
67 ✗ rzId_ = rz_->rankInConfiguration() + 2;
68 } else {
69 ✗ rzId_ = rz_->rankInConfiguration();
70 }
71 }
72
73 /// Copy constructor
74 ✗ CarLike::CarLike(const CarLike& other)
75 : SteeringMethod(other),
76 ✗ device_(other.device_),
77 ✗ rho_(other.rho_),
78 ✗ xy_(other.xy_),
79 ✗ rz_(other.rz_),
80 ✗ xyId_(other.xyId_),
81 ✗ rzId_(other.rzId_) {}
82
83 3 void CarLike::turningRadius(const value_type& rho) {
84
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (rho <= 0)
85 ✗ throw std::invalid_argument("Turning radius must be strictly positive.");
86 3 rho_ = rho;
87 3 }
88
89 4 std::vector<JointPtr_t> getWheelsFromParameter(const ProblemConstPtr_t& problem,
90 const JointPtr_t& rz) {
91 4 std::vector<JointPtr_t> wheels;
92 std::string p(
93
3/6
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 problem->getParameter("SteeringMethod/Carlike/wheels").stringValue());
94 4 std::vector<std::string> wheelNames;
95
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 92 boost::split(wheelNames, p, [](char c) { return c == ','; });
96
97 4 wheels.clear();
98
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 4 times.
 10 for (const std::string& name : wheelNames) {
99
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 4 times.
 6 if (name == "") continue;
100 4 bool found(false);
101
3/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✓ Branch 5 taken 4 times.
 12 for (std::size_t i = 0; i < rz->numberChildJoints(); ++i) {
102
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 JointPtr_t j = rz->childJoint(i);
103
3/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✓ Branch 6 taken 4 times.
 8 if (j->name() == name) {
104 4 found = true;
105
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
 4 if (j->configSize() != 1) {
106 ✗ throw std::runtime_error(
107 ✗ "Carlike: wheel joint should be of dimension 1.");
108 }
109
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 wheels.push_back(j);
110 hppDout(info, "wheel: " << name);
111 }
112 8 }
113
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (!found) {
114 ✗ std::ostringstream os;
115 ✗ os << "CarLike: no joint with name \"" << name << "\".";
116 ✗ throw std::runtime_error(os.str());
117 }
118 }
119 8 return wheels;
120 4 }
121
122 } // namespace steeringMethod
123 } // namespace core
124 } // namespace hpp
125