IMUROSPlugin.hh

Go to the documentation of this file.

// Copyright (c) 2016 The UUV Simulator Authors.
// All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// This source code is derived from rotors_simulator
//   (https://github.com/ethz-asl/rotors_simulator)
// * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland,
// * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland,
// licensed under the Apache-2.0 license,
// cf. 3rd-party-licenses.txt file in the root directory of this source tree.
//
// The original code was modified to:
// - be more consistent with other sensor plugins within uuv_simulator,
// - adhere to Gazebo's coding standards.

#ifndef __UUV_IMU_ROS_PLUGIN_HH__
#define __UUV_IMU_ROS_PLUGIN_HH__

#include <gazebo/gazebo.hh>
#include <ros/ros.h>
#include <uuv_sensor_ros_plugins/ROSBaseModelPlugin.hh>
#include <sensor_msgs/Imu.h>
#include "SensorImu.pb.h"

// Default values for use with ADIS16448 IMU
#define K_DEFAULT_ADIS_GYROSCOPE_NOISE_DENSITY              2.0 * 35.0 / 3600.0 / 180.0 * M_PI
#define K_DEFAULT_ADIS_GYROSCOPE_RANDOM_WALK                2.0 * 4.0 / 3600.0 / 180.0 * M_PI
#define K_DEFAULT_ADIS_GYROSCOPE_BIAS_CORRELATION_TIME      1.0e+3
#define K_DEFAULT_ADIS_GYROSCOPE_TURN_ON_BIAS_SIGMA         0.5 / 180.0 * M_PI
#define K_DEFAULT_ADIS_ACCELEROMETER_NOISE_DENSITY          2.0 * 2.0e-3
#define K_DEFAULT_ADIS_ACCELEROMETER_RANDOM_WALK            2.0 * 3.0e-3
#define K_DEFAULT_ADIS_ACCELEROMETER_BIAS_CORRELATION_TIME  300.0
#define K_DEFAULT_ADIS_ACCELEROMETER_TURN_ON_BIAS_SIGMA     20.0e-3 * 9.8
#define K_DEFAULT_ORIENTATION_NOISE                         0.5

namespace gazebo
{
  struct IMUParameters {
    double gyroscopeNoiseDensity;

    double gyroscopeRandomWalk;

    double gyroscopeBiasCorrelationTime;

    double gyroscopeTurnOnBiasSigma;

    double accelerometerNoiseDensity;

    double accelerometerRandomWalk;

    double accelerometerBiasCorrelationTime;

    double accelerometerTurnOnBiasSigma;

    double orientationNoise;

    IMUParameters()
        : gyroscopeNoiseDensity(
            K_DEFAULT_ADIS_GYROSCOPE_NOISE_DENSITY),
          gyroscopeRandomWalk(
            K_DEFAULT_ADIS_GYROSCOPE_RANDOM_WALK),
          gyroscopeBiasCorrelationTime(
            K_DEFAULT_ADIS_GYROSCOPE_BIAS_CORRELATION_TIME),
          gyroscopeTurnOnBiasSigma(
            K_DEFAULT_ADIS_GYROSCOPE_TURN_ON_BIAS_SIGMA),
          accelerometerNoiseDensity(
            K_DEFAULT_ADIS_ACCELEROMETER_NOISE_DENSITY),
          accelerometerRandomWalk(
            K_DEFAULT_ADIS_ACCELEROMETER_RANDOM_WALK),
          accelerometerBiasCorrelationTime(
            K_DEFAULT_ADIS_ACCELEROMETER_BIAS_CORRELATION_TIME),
          accelerometerTurnOnBiasSigma(
            K_DEFAULT_ADIS_ACCELEROMETER_TURN_ON_BIAS_SIGMA),
          orientationNoise(
            K_DEFAULT_ORIENTATION_NOISE)
    {}
  };

  class IMUROSPlugin : public ROSBaseModelPlugin
  {
    public: IMUROSPlugin();

    public: virtual ~IMUROSPlugin();

    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

    protected: virtual bool OnUpdate(const common::UpdateInfo& _info);

    protected: void AddNoise(ignition::math::Vector3d& _linAcc,
                             ignition::math::Vector3d& _angVel,
                             ignition::math::Quaterniond& _orientation,
                             double _dt);

    protected: ignition::math::Vector3d measLinearAcc;

    protected: ignition::math::Vector3d measAngularVel;

    protected: ignition::math::Quaterniond measOrientation;

    protected: ignition::math::Vector3d gravityWorld;

    protected: ignition::math::Vector3d gyroscopeBias;

    protected: ignition::math::Vector3d accelerometerBias;

    protected: ignition::math::Vector3d gyroscopeTurnOnBias;

    protected: ignition::math::Vector3d accelerometerTurnOnBias;

    protected: IMUParameters imuParameters;

    protected: sensor_msgs::Imu imuROSMessage;
  };
}

#endif // __UUV_IMU_ROS_PLUGIN_HH__