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 2012 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 
27 namespace gazebo
28 {
29  namespace sensors
30  {
33 
36  class ImuSensor: public Sensor
37  {
39  public: ImuSensor();
40 
42  public: virtual ~ImuSensor();
43 
44  // Documentation inherited.
45  protected: void Load(const std::string &_worldName, sdf::ElementPtr _sdf);
46 
47  // Documentation inherited.
48  protected: virtual void Load(const std::string &_worldName);
49 
51  public: virtual void Init();
52 
53  // Documentation inherited
54  protected: virtual void UpdateImpl(bool _force);
55 
56  // Documentation inherited
57  protected: virtual void Fini();
58 
61  public: msgs::IMU GetImuMessage() const;
62 
65  public: math::Vector3 GetAngularVelocity() const;
66 
69  public: math::Vector3 GetLinearAcceleration() const;
70 
74  public: math::Quaternion GetOrientation() const;
75 
77  public: void SetReferencePose();
78 
79  // Documentation inherited.
80  public: virtual bool IsActive();
81 
84  private: void OnLinkData(ConstLinkDataPtr &_msg);
85 
87  private: math::Pose referencePose;
88 
90  private: math::Vector3 lastLinearVel;
91 
93  private: math::Vector3 linearAcc;
94 
96  private: math::Vector3 gravity;
97 
99  private: transport::PublisherPtr pub;
100 
102  private: transport::SubscriberPtr linkDataSub;
103 
105  private: physics::LinkPtr parentEntity;
106 
108  private: msgs::IMU imuMsg;
109 
111  private: mutable boost::mutex mutex;
112 
114  private: boost::shared_ptr<msgs::LinkData const> incomingLinkData[2];
115 
117  private: unsigned int dataIndex;
118 
120  private: bool dataDirty;
121 
123  private: enum NoiseModelType
124  {
125  NONE,
126  GAUSSIAN
127  };
128 
131  private: bool noiseActive;
132 
134  private: enum NoiseModelType noiseType;
135 
138  private: double accelNoiseMean;
139 
142  private: double accelNoiseStdDev;
143 
145  private: double accelBias;
146 
149  private: double rateNoiseMean;
150 
153  private: double rateNoiseStdDev;
154 
156  private: double rateBias;
157  };
159  }
160 }
161 #endif