22 #ifndef ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H 23 #define ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H 28 #include <gazebo/common/common.hh> 29 #include <gazebo/common/Plugin.hh> 30 #include <gazebo/gazebo.hh> 31 #include <gazebo/physics/physics.hh> 41 2.0 * 35.0 / 3600.0 / 180.0 *
M_PI;
43 2.0 * 4.0 / 3600.0 / 180.0 *
M_PI;
83 : gyroscope_noise_density(kDefaultAdisGyroscopeNoiseDensity),
84 gyroscope_random_walk(kDefaultAdisGyroscopeRandomWalk),
85 gyroscope_bias_correlation_time(
86 kDefaultAdisGyroscopeBiasCorrelationTime),
87 gyroscope_turn_on_bias_sigma(kDefaultAdisGyroscopeTurnOnBiasSigma),
88 accelerometer_noise_density(kDefaultAdisAccelerometerNoiseDensity),
89 accelerometer_random_walk(kDefaultAdisAccelerometerRandomWalk),
90 accelerometer_bias_correlation_time(
91 kDefaultAdisAccelerometerBiasCorrelationTime),
92 accelerometer_turn_on_bias_sigma(
93 kDefaultAdisAccelerometerTurnOnBiasSigma),
94 gravity_magnitude(kDefaultGravityMagnitude) {}
103 void InitializeParams();
107 void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
112 Eigen::Vector3d* linear_acceleration,
113 Eigen::Vector3d* angular_velocity,
118 void OnUpdate(
const common::UpdateInfo&);
130 void CreatePubsAndSubs();
180 #endif // ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H
physics::LinkPtr link_
Pointer to the link.
Eigen::Vector3d gyroscope_bias_
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
Eigen::Vector3d accelerometer_turn_on_bias_
static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma
static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime
static constexpr double kDefaultGravityMagnitude
double gyroscope_random_walk
Gyroscope bias random walk [rad/s/s/sqrt(Hz)].
std::default_random_engine random_generator_
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
static constexpr double kDefaultAdisAccelerometerRandomWalk
physics::ModelPtr model_
Pointer to the model.
physics::WorldPtr world_
Pointer to the world.
gz_sensor_msgs::Imu imu_message_
IMU message.
ignition::math::Vector3d gravity_W_
static constexpr double kDefaultAdisGyroscopeNoiseDensity
static constexpr double kDefaultAdisAccelerometerNoiseDensity
static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma
double accelerometer_turn_on_bias_sigma
Accelerometer turn on bias standard deviation [m/s^2].
static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime
double accelerometer_random_walk
Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)].
double gyroscope_bias_correlation_time
Gyroscope bias correlation time constant [s].
Eigen::Vector3d gyroscope_turn_on_bias_
double gyroscope_turn_on_bias_sigma
Gyroscope turn on bias standard deviation [rad/s].
double accelerometer_noise_density
Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)].
ignition::math::Vector3d velocity_prev_W_
double gyroscope_noise_density
Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)].
std::normal_distribution< double > standard_normal_distribution_
transport::NodePtr node_handle_
Handle for the Gazebo node.
static constexpr double kDefaultAdisGyroscopeRandomWalk
Eigen::Vector3d accelerometer_bias_
ImuParameters imu_parameters_
double gravity_magnitude
Norm of the gravitational acceleration [m/s^2].
transport::PublisherPtr imu_pub_
double accelerometer_bias_correlation_time
Accelerometer bias correlation time constant [s].