00001 /* 00002 * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland 00003 * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com> 00004 * 00005 * Licensed under the Apache License, Version 2.0 (the "License"); 00006 * you may not use this file except in compliance with the License. 00007 * You may obtain a copy of the License at 00008 * 00009 * http://www.apache.org/licenses/LICENSE-2.0 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #ifndef ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H 00018 #define ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H 00019 00020 #include <random> 00021 00022 #include <gazebo/common/common.hh> 00023 #include <gazebo/common/Plugin.hh> 00024 #include <gazebo/gazebo.hh> 00025 #include <gazebo/physics/physics.hh> 00026 00027 #include "MagneticField.pb.h" 00028 00029 #include "rotors_gazebo_plugins/common.h" 00030 #include "rotors_gazebo_plugins/sdf_api_wrapper.hpp" 00031 00032 namespace gazebo { 00033 00034 // Default magnetic field [Tesla] in NED frame, obtained from World Magnetic 00035 // Model: (http://www.ngdc.noaa.gov/geomag-web/#igrfwmm) for Zurich: 00036 // lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84 00037 static constexpr double kDefaultRefMagNorth = 0.000021493; 00038 static constexpr double kDefaultRefMagEast = 0.000000815; 00039 static constexpr double kDefaultRefMagDown = 0.000042795; 00040 00041 class GazeboMagnetometerPlugin : public ModelPlugin { 00042 00043 public: 00044 typedef std::normal_distribution<> NormalDistribution; 00045 typedef std::uniform_real_distribution<> UniformDistribution; 00046 00047 GazeboMagnetometerPlugin(); 00048 virtual ~GazeboMagnetometerPlugin(); 00049 00050 protected: 00051 void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 00052 void OnUpdate(const common::UpdateInfo&); 00053 00054 private: 00055 00058 bool pubs_and_subs_created_; 00059 00064 void CreatePubsAndSubs(); 00065 00066 std::string namespace_; 00067 std::string magnetometer_topic_; 00068 gazebo::transport::NodePtr node_handle_; 00069 gazebo::transport::PublisherPtr magnetometer_pub_; 00070 std::string frame_id_; 00071 00073 physics::WorldPtr world_; 00074 00076 physics::ModelPtr model_; 00077 00079 physics::LinkPtr link_; 00080 00082 event::ConnectionPtr updateConnection_; 00083 00084 ignition::math::Vector3d mag_W_; 00085 00089 gz_sensor_msgs::MagneticField mag_message_; 00090 00091 NormalDistribution noise_n_[3]; 00092 00093 std::random_device random_device_; 00094 std::mt19937 random_generator_; 00095 }; 00096 00097 } // namespace gazebo 00098 00099 #endif // ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H