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 00011 * Unless required by applicable law or agreed to in writing, software 00012 * distributed under the License is distributed on an "AS IS" BASIS, 00013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00014 * See the License for the specific language governing permissions and 00015 * limitations under the License. 00016 */ 00017 00018 #ifndef ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H 00019 #define ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H 00020 00021 // SYSTEM 00022 #include <random> 00023 00024 // 3RD PARTY 00025 #include <gazebo/gazebo.hh> 00026 #include <gazebo/physics/physics.hh> 00027 #include <gazebo/sensors/sensors.hh> 00028 00029 // USER 00030 #include "NavSatFix.pb.h" // GPS message type generated by protobuf .proto file 00031 #include "TwistStamped.pb.h" // GPS ground speed message 00032 #include "rotors_gazebo_plugins/common.h" 00033 00034 namespace gazebo { 00035 00036 // Default values 00037 static constexpr double kDefaultHorPosStdDev = 3.0; 00038 static constexpr double kDefaultVerPosStdDev = 6.0; 00039 static constexpr double kDefaultHorVelStdDev = 0.1; 00040 static constexpr double kDefaultVerVelStdDev = 0.1; 00041 00042 class GazeboGpsPlugin : public SensorPlugin { 00043 public: 00044 typedef std::normal_distribution<> NormalDistribution; 00045 00046 GazeboGpsPlugin(); 00047 virtual ~GazeboGpsPlugin(); 00048 00049 protected: 00050 void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); 00051 00053 void OnUpdate(); 00054 00055 private: 00056 00057 std::string namespace_; 00058 00061 bool pubs_and_subs_created_; 00062 00067 void CreatePubsAndSubs(); 00068 00069 gazebo::transport::NodePtr node_handle_; 00070 00071 gazebo::transport::PublisherPtr gz_gps_pub_; 00072 00073 gazebo::transport::PublisherPtr gz_ground_speed_pub_; 00074 00076 std::string gps_topic_; 00077 00079 std::string ground_speed_topic_; 00080 00082 sensors::GpsSensorPtr parent_sensor_; 00083 00085 physics::WorldPtr world_; 00086 00088 physics::LinkPtr link_; 00089 00091 event::ConnectionPtr updateConnection_; 00092 00094 gz_sensor_msgs::NavSatFix gz_gps_message_; 00095 00097 gz_geometry_msgs::TwistStamped gz_ground_speed_message_; 00098 00100 NormalDistribution ground_speed_n_[3]; 00101 00103 std::random_device random_device_; 00104 00105 std::mt19937 random_generator_; 00106 }; 00107 00108 } // namespace gazebo 00109 00110 #endif // ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H