00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "rotors_gazebo_plugins/gazebo_imu_plugin.h"
00024
00025
00026 #include <stdio.h>
00027 #include <boost/bind.hpp>
00028 #include <chrono>
00029 #include <cmath>
00030 #include <iostream>
00031
00032
00033 #include "mav_msgs/default_topics.h"
00034
00035
00036 #include "ConnectGazeboToRosTopic.pb.h"
00037
00038 namespace gazebo {
00039
00040 GazeboImuPlugin::GazeboImuPlugin()
00041 : ModelPlugin(),
00042 node_handle_(0),
00043 velocity_prev_W_(0, 0, 0),
00044 pubs_and_subs_created_(false) {}
00045
00046 GazeboImuPlugin::~GazeboImuPlugin() {
00047 }
00048
00049 void GazeboImuPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
00050 if (kPrintOnPluginLoad) {
00051 gzdbg << __FUNCTION__ << "() called." << std::endl;
00052 }
00053
00054 gzdbg << "_model = " << _model->GetName() << std::endl;
00055
00056
00057 model_ = _model;
00058 world_ = model_->GetWorld();
00059
00060
00061 namespace_.clear();
00062
00063
00064
00065
00066
00067 if (_sdf->HasElement("robotNamespace"))
00068 namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00069 else
00070 gzerr << "[gazebo_imu_plugin] Please specify a robotNamespace.\n";
00071
00072
00073 node_handle_ = transport::NodePtr(new transport::Node());
00074
00075
00076 node_handle_->Init();
00077
00078 if (_sdf->HasElement("linkName"))
00079 link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
00080 else
00081 gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n";
00082
00083 link_ = model_->GetLink(link_name_);
00084 if (link_ == NULL)
00085 gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_
00086 << "\".");
00087
00088 frame_id_ = link_name_;
00089
00090 getSdfParam<std::string>(_sdf, "imuTopic", imu_topic_,
00091 mav_msgs::default_topics::IMU);
00092 getSdfParam<double>(_sdf, "gyroscopeNoiseDensity",
00093 imu_parameters_.gyroscope_noise_density,
00094 imu_parameters_.gyroscope_noise_density);
00095 getSdfParam<double>(_sdf, "gyroscopeBiasRandomWalk",
00096 imu_parameters_.gyroscope_random_walk,
00097 imu_parameters_.gyroscope_random_walk);
00098 getSdfParam<double>(_sdf, "gyroscopeBiasCorrelationTime",
00099 imu_parameters_.gyroscope_bias_correlation_time,
00100 imu_parameters_.gyroscope_bias_correlation_time);
00101 assert(imu_parameters_.gyroscope_bias_correlation_time > 0.0);
00102 getSdfParam<double>(_sdf, "gyroscopeTurnOnBiasSigma",
00103 imu_parameters_.gyroscope_turn_on_bias_sigma,
00104 imu_parameters_.gyroscope_turn_on_bias_sigma);
00105 getSdfParam<double>(_sdf, "accelerometerNoiseDensity",
00106 imu_parameters_.accelerometer_noise_density,
00107 imu_parameters_.accelerometer_noise_density);
00108 getSdfParam<double>(_sdf, "accelerometerRandomWalk",
00109 imu_parameters_.accelerometer_random_walk,
00110 imu_parameters_.accelerometer_random_walk);
00111 getSdfParam<double>(_sdf, "accelerometerBiasCorrelationTime",
00112 imu_parameters_.accelerometer_bias_correlation_time,
00113 imu_parameters_.accelerometer_bias_correlation_time);
00114 assert(imu_parameters_.accelerometer_bias_correlation_time > 0.0);
00115 getSdfParam<double>(_sdf, "accelerometerTurnOnBiasSigma",
00116 imu_parameters_.accelerometer_turn_on_bias_sigma,
00117 imu_parameters_.accelerometer_turn_on_bias_sigma);
00118
00119 last_time_ = world_->SimTime();
00120
00121
00122
00123 this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00124 boost::bind(&GazeboImuPlugin::OnUpdate, this, _1));
00125
00126
00127
00128
00129
00130
00131 imu_message_.mutable_header()->set_frame_id(frame_id_);
00132
00133
00134
00135
00136
00137
00138 for (int i = 0; i < 9; i++) {
00139 switch (i) {
00140 case 0:
00141 imu_message_.add_angular_velocity_covariance(
00142 imu_parameters_.gyroscope_noise_density *
00143 imu_parameters_.gyroscope_noise_density);
00144
00145 imu_message_.add_orientation_covariance(-1.0);
00146
00147 imu_message_.add_linear_acceleration_covariance(
00148 imu_parameters_.accelerometer_noise_density *
00149 imu_parameters_.accelerometer_noise_density);
00150 break;
00151 case 1:
00152 case 2:
00153 case 3:
00154 imu_message_.add_angular_velocity_covariance(0.0);
00155
00156 imu_message_.add_orientation_covariance(-1.0);
00157
00158 imu_message_.add_linear_acceleration_covariance(0.0);
00159 break;
00160 case 4:
00161 imu_message_.add_angular_velocity_covariance(
00162 imu_parameters_.gyroscope_noise_density *
00163 imu_parameters_.gyroscope_noise_density);
00164
00165 imu_message_.add_orientation_covariance(-1.0);
00166
00167 imu_message_.add_linear_acceleration_covariance(
00168 imu_parameters_.accelerometer_noise_density *
00169 imu_parameters_.accelerometer_noise_density);
00170 break;
00171 case 5:
00172 case 6:
00173 case 7:
00174 imu_message_.add_angular_velocity_covariance(0.0);
00175
00176 imu_message_.add_orientation_covariance(-1.0);
00177
00178 imu_message_.add_linear_acceleration_covariance(0.0);
00179 break;
00180 case 8:
00181 imu_message_.add_angular_velocity_covariance(
00182 imu_parameters_.gyroscope_noise_density *
00183 imu_parameters_.gyroscope_noise_density);
00184
00185 imu_message_.add_orientation_covariance(-1.0);
00186
00187 imu_message_.add_linear_acceleration_covariance(
00188 imu_parameters_.accelerometer_noise_density *
00189 imu_parameters_.accelerometer_noise_density);
00190 break;
00191 }
00192 }
00193
00194 gravity_W_ = world_->Gravity();
00195 imu_parameters_.gravity_magnitude = gravity_W_.Length();
00196
00197 standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
00198
00199 double sigma_bon_g = imu_parameters_.gyroscope_turn_on_bias_sigma;
00200 double sigma_bon_a = imu_parameters_.accelerometer_turn_on_bias_sigma;
00201 for (int i = 0; i < 3; ++i) {
00202 gyroscope_turn_on_bias_[i] =
00203 sigma_bon_g * standard_normal_distribution_(random_generator_);
00204 accelerometer_turn_on_bias_[i] =
00205 sigma_bon_a * standard_normal_distribution_(random_generator_);
00206 }
00207
00208
00209 gyroscope_bias_.setZero();
00210 accelerometer_bias_.setZero();
00211 }
00212
00213 void GazeboImuPlugin::AddNoise(Eigen::Vector3d* linear_acceleration,
00214 Eigen::Vector3d* angular_velocity,
00215 const double dt) {
00216 GZ_ASSERT(linear_acceleration != nullptr, "Linear acceleration was null.");
00217 GZ_ASSERT(angular_velocity != nullptr, "Angular velocity was null.");
00218 GZ_ASSERT(dt > 0.0, "Change in time must be greater than 0.");
00219
00220
00221 double tau_g = imu_parameters_.gyroscope_bias_correlation_time;
00222
00223
00224 double sigma_g_d = 1 / sqrt(dt) * imu_parameters_.gyroscope_noise_density;
00225 double sigma_b_g = imu_parameters_.gyroscope_random_walk;
00226
00227 double sigma_b_g_d = sqrt(-sigma_b_g * sigma_b_g * tau_g / 2.0 *
00228 (exp(-2.0 * dt / tau_g) - 1.0));
00229
00230 double phi_g_d = exp(-1.0 / tau_g * dt);
00231
00232 for (int i = 0; i < 3; ++i) {
00233 gyroscope_bias_[i] =
00234 phi_g_d * gyroscope_bias_[i] +
00235 sigma_b_g_d * standard_normal_distribution_(random_generator_);
00236 (*angular_velocity)[i] =
00237 (*angular_velocity)[i] + gyroscope_bias_[i] +
00238 sigma_g_d * standard_normal_distribution_(random_generator_) +
00239 gyroscope_turn_on_bias_[i];
00240 }
00241
00242
00243 double tau_a = imu_parameters_.accelerometer_bias_correlation_time;
00244
00245
00246 double sigma_a_d = 1 / sqrt(dt) * imu_parameters_.accelerometer_noise_density;
00247 double sigma_b_a = imu_parameters_.accelerometer_random_walk;
00248
00249 double sigma_b_a_d = sqrt(-sigma_b_a * sigma_b_a * tau_a / 2.0 *
00250 (exp(-2.0 * dt / tau_a) - 1.0));
00251
00252 double phi_a_d = exp(-1.0 / tau_a * dt);
00253
00254
00255 for (int i = 0; i < 3; ++i) {
00256 accelerometer_bias_[i] =
00257 phi_a_d * accelerometer_bias_[i] +
00258 sigma_b_a_d * standard_normal_distribution_(random_generator_);
00259 (*linear_acceleration)[i] =
00260 (*linear_acceleration)[i] + accelerometer_bias_[i] +
00261 sigma_a_d * standard_normal_distribution_(random_generator_) +
00262 accelerometer_turn_on_bias_[i];
00263 }
00264 }
00265
00266 void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) {
00267 if (kPrintOnUpdates) {
00268 gzdbg << __FUNCTION__ << "() called." << std::endl;
00269 }
00270
00271 if (!pubs_and_subs_created_) {
00272 CreatePubsAndSubs();
00273 pubs_and_subs_created_ = true;
00274 }
00275
00276 common::Time current_time = world_->SimTime();
00277 double dt = (current_time - last_time_).Double();
00278 last_time_ = current_time;
00279 double t = current_time.Double();
00280
00281 ignition::math::Pose3d T_W_I = link_->WorldPose();
00282 ignition::math::Quaterniond C_W_I = T_W_I.Rot();
00283
00284 ignition::math::Vector3d acceleration_I =
00285 link_->RelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_);
00286
00287 ignition::math::Vector3d angular_vel_I = link_->RelativeAngularVel();
00288
00289 Eigen::Vector3d linear_acceleration_I(acceleration_I.X(), acceleration_I.Y(),
00290 acceleration_I.Z());
00291 Eigen::Vector3d angular_velocity_I(angular_vel_I.X(), angular_vel_I.Y(),
00292 angular_vel_I.Z());
00293
00294 AddNoise(&linear_acceleration_I, &angular_velocity_I, dt);
00295
00296
00297
00298 imu_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec);
00299
00300
00301 imu_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec);
00302
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00315 gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion();
00316 orientation->set_w(C_W_I.W());
00317 orientation->set_x(C_W_I.X());
00318 orientation->set_y(C_W_I.Y());
00319 orientation->set_z(C_W_I.Z());
00320 imu_message_.set_allocated_orientation(orientation);
00321
00322 gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d();
00323 linear_acceleration->set_x(linear_acceleration_I[0]);
00324 linear_acceleration->set_y(linear_acceleration_I[1]);
00325 linear_acceleration->set_z(linear_acceleration_I[2]);
00326 imu_message_.set_allocated_linear_acceleration(linear_acceleration);
00327
00328 gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d();
00329 angular_velocity->set_x(angular_velocity_I[0]);
00330 angular_velocity->set_y(angular_velocity_I[1]);
00331 angular_velocity->set_z(angular_velocity_I[2]);
00332 imu_message_.set_allocated_angular_velocity(angular_velocity);
00333
00334
00335 imu_pub_->Publish(imu_message_);
00336
00337
00338 }
00339
00340 void GazeboImuPlugin::CreatePubsAndSubs() {
00341
00342 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
00343 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00344 "~/" + kConnectGazeboToRosSubtopic, 1);
00345
00346
00347
00348
00349
00350 imu_pub_ = node_handle_->Advertise<gz_sensor_msgs::Imu>(
00351 "~/" + namespace_ + "/" + imu_topic_, 1);
00352
00353 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00354
00355 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00356 imu_topic_);
00357 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + imu_topic_);
00358 connect_gazebo_to_ros_topic_msg.set_msgtype(
00359 gz_std_msgs::ConnectGazeboToRosTopic::IMU);
00360 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00361 true);
00362 }
00363
00364 GZ_REGISTER_MODEL_PLUGIN(GazeboImuPlugin);
00365
00366 }