gazebo_imu_plugin.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #ifndef ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H
00023 #define ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H
00024 
00025 #include <random>
00026 
00027 #include <Eigen/Core>
00028 #include <gazebo/common/common.hh>
00029 #include <gazebo/common/Plugin.hh>
00030 #include <gazebo/gazebo.hh>
00031 #include <gazebo/physics/physics.hh>
00032 
00033 #include "Imu.pb.h"
00034 
00035 #include "rotors_gazebo_plugins/common.h"
00036 
00037 namespace gazebo {
00038 
00039 // Default values for use with ADIS16448 IMU
00040 static constexpr double kDefaultAdisGyroscopeNoiseDensity =
00041     2.0 * 35.0 / 3600.0 / 180.0 * M_PI;
00042 static constexpr double kDefaultAdisGyroscopeRandomWalk =
00043     2.0 * 4.0 / 3600.0 / 180.0 * M_PI;
00044 static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime =
00045     1.0e+3;
00046 static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma =
00047     0.5 / 180.0 * M_PI;
00048 static constexpr double kDefaultAdisAccelerometerNoiseDensity =
00049     2.0 * 2.0e-3;
00050 static constexpr double kDefaultAdisAccelerometerRandomWalk =
00051     2.0 * 3.0e-3;
00052 static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime =
00053     300.0;
00054 static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma =
00055     20.0e-3 * 9.8;
00056 // Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84)
00057 static constexpr double kDefaultGravityMagnitude = 9.8068;
00058 
00059 // A description of the parameters:
00060 // https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
00061 // TODO(burrimi): Should I have a minimalistic description of the params here?
00062 struct ImuParameters {
00064   double gyroscope_noise_density;
00066   double gyroscope_random_walk;
00068   double gyroscope_bias_correlation_time;
00070   double gyroscope_turn_on_bias_sigma;
00072   double accelerometer_noise_density;
00074   double accelerometer_random_walk;
00076   double accelerometer_bias_correlation_time;
00078   double accelerometer_turn_on_bias_sigma;
00080   double gravity_magnitude;
00081 
00082   ImuParameters()
00083       : gyroscope_noise_density(kDefaultAdisGyroscopeNoiseDensity),
00084         gyroscope_random_walk(kDefaultAdisGyroscopeRandomWalk),
00085         gyroscope_bias_correlation_time(
00086             kDefaultAdisGyroscopeBiasCorrelationTime),
00087         gyroscope_turn_on_bias_sigma(kDefaultAdisGyroscopeTurnOnBiasSigma),
00088         accelerometer_noise_density(kDefaultAdisAccelerometerNoiseDensity),
00089         accelerometer_random_walk(kDefaultAdisAccelerometerRandomWalk),
00090         accelerometer_bias_correlation_time(
00091             kDefaultAdisAccelerometerBiasCorrelationTime),
00092         accelerometer_turn_on_bias_sigma(
00093             kDefaultAdisAccelerometerTurnOnBiasSigma),
00094         gravity_magnitude(kDefaultGravityMagnitude) {}
00095 };
00096 
00097 class GazeboImuPlugin : public ModelPlugin {
00098  public:
00099 
00100   GazeboImuPlugin();
00101   ~GazeboImuPlugin();
00102 
00103   void InitializeParams();
00104   void Publish();
00105 
00106  protected:
00107   void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00108 
00111   void AddNoise(
00112       Eigen::Vector3d* linear_acceleration,
00113       Eigen::Vector3d* angular_velocity,
00114       const double dt);
00115 
00118   void OnUpdate(const common::UpdateInfo&);
00119 
00120  private:
00121 
00124   bool pubs_and_subs_created_;
00125 
00130   void CreatePubsAndSubs();
00131 
00132 
00133   std::string namespace_;
00134   std::string imu_topic_;
00135 
00137   transport::NodePtr node_handle_;
00138 
00139   transport::PublisherPtr imu_pub_;
00140 
00141   std::string frame_id_;
00142   std::string link_name_;
00143 
00144   std::default_random_engine random_generator_;
00145   std::normal_distribution<double> standard_normal_distribution_;
00146 
00148   physics::WorldPtr world_;
00149 
00151   physics::ModelPtr model_;
00152 
00154   physics::LinkPtr link_;
00155 
00157   event::ConnectionPtr updateConnection_;
00158 
00159   common::Time last_time_;
00160 
00163   //            and then published onto a topic
00164   gz_sensor_msgs::Imu imu_message_;
00165 
00166   ignition::math::Vector3d gravity_W_;
00167   ignition::math::Vector3d velocity_prev_W_;
00168 
00169   Eigen::Vector3d gyroscope_bias_;
00170   Eigen::Vector3d accelerometer_bias_;
00171 
00172   Eigen::Vector3d gyroscope_turn_on_bias_;
00173   Eigen::Vector3d accelerometer_turn_on_bias_;
00174 
00175   ImuParameters imu_parameters_;
00176 };
00177 
00178 }  // namespace gazebo
00179 
00180 #endif // ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43