gazebo_gps_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland
 * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
 *
 * 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_GPS_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H

// SYSTEM
#include <random>

// 3RD PARTY
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/sensors.hh>

// USER
#include "NavSatFix.pb.h"     // GPS message type generated by protobuf .proto file
#include "TwistStamped.pb.h"  // GPS ground speed message
#include "rotors_gazebo_plugins/common.h"

namespace gazebo {

// Default values
static constexpr double kDefaultHorPosStdDev = 3.0;
static constexpr double kDefaultVerPosStdDev = 6.0;
static constexpr double kDefaultHorVelStdDev = 0.1;
static constexpr double kDefaultVerVelStdDev = 0.1;

class GazeboGpsPlugin : public SensorPlugin {
 public:
  typedef std::normal_distribution<> NormalDistribution;

  GazeboGpsPlugin();
  virtual ~GazeboGpsPlugin();

 protected:
  void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);

  void OnUpdate();

 private:

  std::string namespace_;

  bool pubs_and_subs_created_;

  void CreatePubsAndSubs();

  gazebo::transport::NodePtr node_handle_;

  gazebo::transport::PublisherPtr gz_gps_pub_;

  gazebo::transport::PublisherPtr gz_ground_speed_pub_;

  std::string gps_topic_;

  std::string ground_speed_topic_;

  sensors::GpsSensorPtr parent_sensor_;

  physics::WorldPtr world_;

  physics::LinkPtr link_;

  event::ConnectionPtr updateConnection_;

  gz_sensor_msgs::NavSatFix gz_gps_message_;

  gz_geometry_msgs::TwistStamped gz_ground_speed_message_;

  NormalDistribution ground_speed_n_[3];

  std::random_device random_device_;

  std::mt19937 random_generator_;
};

} // namespace gazebo 

#endif // ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H