All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ImuSensor.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
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 */
17 
18 #ifndef _IMUSENSOR_HH_
19 #define _IMUSENSOR_HH_
20 
21 #include <vector>
22 #include <string>
23 
25 #include "gazebo/sensors/Sensor.hh"
26 #include "gazebo/util/system.hh"
27 
28 namespace gazebo
29 {
30  namespace sensors
31  {
34 
38  {
40  public: ImuSensor();
41 
43  public: virtual ~ImuSensor();
44 
45  // Documentation inherited.
46  protected: void Load(const std::string &_worldName, sdf::ElementPtr _sdf);
47 
48  // Documentation inherited.
49  protected: virtual void Load(const std::string &_worldName);
50 
52  public: virtual void Init();
53 
54  // Documentation inherited
55  protected: virtual bool UpdateImpl(bool _force);
56 
57  // Documentation inherited
58  protected: virtual void Fini();
59 
62  public: msgs::IMU GetImuMessage() const;
63 
66  public: math::Vector3 GetAngularVelocity() const;
67 
70  public: math::Vector3 GetLinearAcceleration() const;
71 
75  public: math::Quaternion GetOrientation() const;
76 
78  public: void SetReferencePose();
79 
80  // Documentation inherited.
81  public: virtual bool IsActive();
82 
85  private: void OnLinkData(ConstLinkDataPtr &_msg);
86 
88  private: math::Pose referencePose;
89 
91  private: math::Vector3 lastLinearVel;
92 
94  private: math::Vector3 linearAcc;
95 
97  private: math::Vector3 gravity;
98 
100  private: transport::PublisherPtr pub;
101 
103  private: transport::SubscriberPtr linkDataSub;
104 
106  private: physics::LinkPtr parentEntity;
107 
109  private: msgs::IMU imuMsg;
110 
112  private: mutable boost::mutex mutex;
113 
115  private: boost::shared_ptr<msgs::LinkData const> incomingLinkData[2];
116 
118  private: unsigned int dataIndex;
119 
121  private: bool dataDirty;
122 
124  private: enum NoiseModelType
125  {
126  NONE,
127  GAUSSIAN
128  };
129 
132  private: bool noiseActive;
133 
135  private: enum NoiseModelType noiseType;
136 
139  private: double accelNoiseMean;
140 
143  private: double accelNoiseStdDev;
144 
146  private: double accelBias;
147 
150  private: double rateNoiseMean;
151 
154  private: double rateNoiseStdDev;
155 
157  private: double rateBias;
158  };
160  }
161 }
162 #endif