ThrusterConversionFcn_TEST.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #include <string>
17 #include <memory>
18 #include <gtest/gtest.h>
20 
21 std::shared_ptr<gazebo::ConversionFunction> ConversionFromString(
22  const std::string& description)
23 {
24  std::stringstream stream;
25  stream << "<sdf version='" << SDF_VERSION << "'>" << std::endl
26  << "<model name='test_model'>" << std::endl
27  << "<plugin name='test_plugin' filename='test_file.so'>" << std::endl
28  << description
29  << "</plugin>" << std::endl
30  << "</model>" << std::endl
31  << "</sdf>" << std::endl;
32 
33  sdf::SDF sdfParsed;
34  sdfParsed.SetFromString(stream.str());
35 
36  sdf::ElementPtr conversion = sdfParsed.Root()->GetElement("model")
37  ->GetElement("plugin")->GetElement("conversion");
38 
39  std::shared_ptr<gazebo::ConversionFunction> func;
41  CreateConversionFunction(conversion));
42 
43  return func;
44 }
45 
46 TEST(ThrusterConversionFcn, Basic)
47 {
48  std::string description =
49  "<conversion> \n"
50  " <type>Basic</type> \n"
51  " <rotorConstant>0.0049</rotorConstant> \n"
52  "</conversion>";
53 
54  std::shared_ptr<gazebo::ConversionFunction> func;
55  func = ConversionFromString(description);
56 
57  EXPECT_TRUE(func != NULL);
58  EXPECT_EQ(func->GetType(), "Basic");
59 
60  EXPECT_EQ(func->convert(0.0), 0.0);
61  EXPECT_EQ(func->convert(50.), 50.0*50.0*0.0049);
62  EXPECT_EQ(func->convert(-50.), -50.0*50.0*0.0049);
63 }
64 
65 TEST(ThrusterConversionFcn, Bessa)
66 {
67  double cl = 0.001;
68  double cr = 0.002;
69  double dl = -50;
70  double dr = 25;
71 
72  double delta = 1e-6;
73 
74  std::stringstream stream;
75  stream << "<conversion> \n"
76  << " <type>Bessa</type> \n"
77  << " <rotorConstantL>" << cl << "</rotorConstantL> \n"
78  << " <rotorConstantR>" << cr << "</rotorConstantR> \n"
79  << " <deltaL>" << dl << "</deltaL> \n"
80  << " <deltaR>" << dr << "</deltaR> \n"
81  << "</conversion>";
82 
83  std::shared_ptr<gazebo::ConversionFunction> func;
84  func = ConversionFromString(stream.str());
85 
86  EXPECT_TRUE(func != NULL);
87  EXPECT_EQ(func->GetType(), "Bessa");
88 
89  // Test dead-zone and its boundaries
90  EXPECT_EQ(0.0, func->convert(0.0));
91  EXPECT_EQ(0.0, func->convert(sqrt(dr) - delta));
92  EXPECT_EQ(0.0, func->convert(-sqrt(-dl) + delta));
93 
94  // Values left and right of the dead-zone
95  double cmdl = -50.0;
96  double cmdr = 50.0;
97  EXPECT_EQ(cl*(cmdl*std::abs(cmdl)-dl), func->convert(cmdl));
98  EXPECT_EQ(cr*(cmdr*std::abs(cmdr)-dr), func->convert(cmdr));
99 }
100 
101 TEST(ThrusterConversionFcn, LinearInterp)
102 {
103  std::vector<double> input = {-5.0, 0, 2.0, 5.0};
104  std::vector<double> output = {-100, -10, 20, 120};
105  std::vector<double> alpha = {0.1, 0.5, 0.9};
106 
107  std::stringstream stream;
108  stream << "<conversion> \n"
109  << " <type>LinearInterp</type> \n"
110  << " <inputValues>";
111  for (double d : input)
112  stream << d << " ";
113  stream << "</inputValues> \n"
114  << "<outputValues>";
115  for (double d : output)
116  stream << d << " ";
117  stream << "</outputValues> \n"
118  << "</conversion>";
119 
120  std::shared_ptr<gazebo::ConversionFunction> func;
121  func = ConversionFromString(stream.str());
122 
123  EXPECT_TRUE(func != NULL);
124  EXPECT_EQ(func->GetType(), "LinearInterp");
125 
126  // Make sure the result is exactly correct for the provided values.
127  for (int i = 0; i < input.size(); i++)
128  {
129  EXPECT_EQ(output[i], func->convert(input[i]));
130  }
131 
132  // Outside of defined range: return closest value
133  EXPECT_EQ(output[0], func->convert(input[0] - 0.5));
134  EXPECT_EQ(output.back(), func->convert(input.back() + 0.5));
135 
136  // In between: make sure linear interpolation is working properly
137  for (int i = 0; i < input.size()-1; i++)
138  {
139  double in = alpha[i]*input[i] + (1-alpha[i])*input[i+1];
140  double out = alpha[i]*output[i] + (1-alpha[i])*output[i+1];
141  EXPECT_NEAR(out, func->convert(in), 1e-7);
142  }
143 }
144 
145 int main(int argc, char **argv)
146 {
147  testing::InitGoogleTest(&argc, argv);
148  return RUN_ALL_TESTS();
149 }
Description of the conversion function fo a thruster.
std::shared_ptr< gazebo::ConversionFunction > ConversionFromString(const std::string &description)
int main(int argc, char **argv)
TEST(ThrusterConversionFcn, Basic)
static ConversionFunctionFactory & GetInstance()
Return the singleton instance of this factory.


uuv_gazebo_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:24