Common.hh
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 // This source code is from rotors_simulator
17 // (https://github.com/ethz-asl/rotors_simulator)
18 // * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
19 // * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
20 // * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
21 // * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
22 // * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
23 // This source code is licensed under the Apache-2.0 license found in the
24 // open_source_licenses.txt file in the root directory of this source tree.
25 //
26 // The original code was modified by the UUV Simulator Authors to adhere to
27 // Gazebo's coding standards.
28 
29 #ifndef UUV_SENSOR_PLUGINS_COMMON_H_
30 #define UUV_SENSOR_PLUGINS_COMMON_H_
31 
32 #include <string>
33 #include <Eigen/Dense>
34 #include <gazebo/gazebo.hh>
35 
36 namespace gazebo {
37 
46 template<class T>
47 bool GetSDFParam(sdf::ElementPtr sdf, const std::string& name, T& param,
48  const T& default_value, const bool& verbose = false)
49 {
50  if (sdf->HasElement(name))
51  {
52  param = sdf->GetElement(name)->Get<T>();
53  return true;
54  }
55  else
56  {
57  param = default_value;
58  if (verbose)
59  gzerr << "[uuv_sensor_plugins] Please specify a value for parameter \""
60  << name << "\".\n";
61  }
62  return false;
63 }
64 } // namespace gazebo
65 
66 template <typename T>
68 /*
69 This class can be used to apply a first order filter on a signal.
70 It allows different acceleration and deceleration time constants.
71 
72 Short reveiw of discrete time implementation of firest order system:
73 Laplace:
74  X(s)/U(s) = 1/(tau*s + 1)
75 continous time system:
76  dx(t) = (-1/tau)*x(t) + (1/tau)*u(t)
77 discretized system (ZoH):
78  x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k)
79 */
80 
81  public:
82  FirstOrderFilter(double timeConstantUp, double timeConstantDown,
83  T initialState):
84  timeConstantUp_(timeConstantUp),
85  timeConstantDown_(timeConstantDown),
86  previousState_(initialState) {}
87 
88  T updateFilter(T inputState, double samplingTime)
89  {
90  /*
91  This method will apply a first order filter on the inputState.
92  */
93  T outputState;
94  if (inputState > previousState_)
95  {
96  // Calcuate the outputState if accelerating.
97  double alphaUp = exp(- samplingTime / timeConstantUp_);
98  // x(k+1) = Ad*x(k) + Bd*u(k)
99  outputState = alphaUp * previousState_ + (1 - alphaUp) * inputState;
100  }
101  else
102  {
103  // Calculate the outputState if decelerating.
104  double alphaDown = exp(- samplingTime / timeConstantDown_);
105  outputState = alphaDown * previousState_ + (1 - alphaDown) * inputState;
106  }
107  previousState_ = outputState;
108  return outputState;
109  }
111 
112  protected:
116 };
117 
118 
119 
121 template<class Derived>
122 Eigen::Quaternion<typename Derived::Scalar> QuaternionFromSmallAngle(
123  const Eigen::MatrixBase<Derived> & theta)
124 {
125  typedef typename Derived::Scalar Scalar;
126  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
127  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
128  const Scalar q_squared = theta.squaredNorm() / 4.0;
129 
130  if (q_squared < 1)
131  {
132  return Eigen::Quaternion<Scalar>(sqrt(1 - q_squared), theta[0] * 0.5,
133  theta[1] * 0.5, theta[2] * 0.5);
134  }
135  else
136  {
137  const Scalar w = 1.0 / sqrt(1 + q_squared);
138  const Scalar f = w * 0.5;
139  return Eigen::Quaternion<Scalar>(w, theta[0] * f, theta[1] * f,
140  theta[2] * f);
141  }
142 }
143 
144 template<class In, class Out>
145 void copyPosition(const In& in, Out* out)
146 {
147  out->x = in.x;
148  out->y = in.y;
149  out->z = in.z;
150 }
151 
152 #endif // UUV_SENSOR_PLUGINS_COMMON_H_
double timeConstantDown_
Definition: Common.hh:114
f
bool GetSDFParam(sdf::ElementPtr sdf, const std::string &name, T &param, const T &default_value, const bool &verbose=false)
Obtains a parameter from sdf.
Definition: Common.hh:47
void copyPosition(const In &in, Out *out)
Definition: Common.hh:145
FirstOrderFilter(double timeConstantUp, double timeConstantDown, T initialState)
Definition: Common.hh:82
TFSIMD_FORCE_INLINE const tfScalar & w() const
T updateFilter(T inputState, double samplingTime)
Definition: Common.hh:88
Eigen::Quaternion< typename Derived::Scalar > QuaternionFromSmallAngle(const Eigen::MatrixBase< Derived > &theta)
Computes a quaternion from the 3-element small angle approximation theta.
Definition: Common.hh:122
double timeConstantUp_
Definition: Common.hh:113


uuv_sensor_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Mon Jul 1 2019 19:39:20