gazebo_pressure_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland
 *
 * 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.
 */

#ifndef ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H

#include <random>

#include <glog/logging.h>

#include <gazebo/common/Plugin.hh>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

#include "FluidPressure.pb.h"

#include "rotors_gazebo_plugins/common.h"

namespace gazebo {
// Constants
static constexpr double kGasConstantNmPerKmolKelvin = 8314.32;
static constexpr double kMeanMolecularAirWeightKgPerKmol = 28.9644;
static constexpr double kGravityMagnitude = 9.80665;
static constexpr double kEarthRadiusMeters = 6356766.0;
static constexpr double kPressureOneAtmospherePascals = 101325.0;
static constexpr double kSeaLevelTempKelvin = 288.15;
static constexpr double kTempLapseKelvinPerMeter = 0.0065;
static constexpr double kAirConstantDimensionless = kGravityMagnitude *
    kMeanMolecularAirWeightKgPerKmol /
        (kGasConstantNmPerKmolKelvin * -kTempLapseKelvinPerMeter);

// Default values
static const std::string kDefaultPressurePubTopic = "air_pressure";
static constexpr double kDefaultRefAlt = 500.0; /* m, Zurich: h=+500m, WGS84) */
static constexpr double kDefaultPressureVar = 0.0; /* Pa^2, pressure variance */

class GazeboPressurePlugin : public ModelPlugin {
 public:
  GazeboPressurePlugin();

  virtual ~GazeboPressurePlugin();

  typedef std::normal_distribution<> NormalDistribution;

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

  void OnUpdate(const common::UpdateInfo&);

 private:
  bool pubs_and_subs_created_;

  void CreatePubsAndSubs();

  gazebo::transport::NodePtr node_handle_;

  gazebo::transport::PublisherPtr pressure_pub_;

  std::string namespace_;

  std::string pressure_topic_;

  std::string frame_id_;

  physics::WorldPtr world_;

  physics::ModelPtr model_;

  physics::LinkPtr link_;

  event::ConnectionPtr updateConnection_;

  double ref_alt_;

  double pressure_var_;

  NormalDistribution pressure_n_[1];

  //            and then published onto a topic
  gz_sensor_msgs::FluidPressure pressure_message_;

  std::mt19937 random_generator_;
};
}

#endif // ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H