Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
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
00057 static constexpr double kDefaultGravityMagnitude = 9.8068;
00058
00059
00060
00061
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
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 }
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