gps.h

#include <flatland_plugins/update_timer.h>
#include <flatland_server/model_plugin.h>
#include <flatland_server/timekeeper.h>
#include <flatland_server/types.h>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>

#ifndef FLATLAND_PLUGINS_GPS_H
#define FLATLAND_PLUGINS_GPS_H

using namespace flatland_server;

namespace flatland_plugins {

class Gps : public ModelPlugin {
 public:
  std::string topic_;     
  std::string frame_id_;  
  Body *body_;            
  Pose origin_;           
  double ref_lat_rad_;  
  double ref_lon_rad_;  
  double ref_ecef_x_;   
  double ref_ecef_y_;   
  double ref_ecef_z_;   
  double update_rate_;  
  bool broadcast_tf_;   

  static double WGS84_A;   
  static double WGS84_E2;  

  ros::Publisher fix_publisher_;             
  tf::TransformBroadcaster tf_broadcaster_;  
  geometry_msgs::TransformStamped gps_tf_;   
  sensor_msgs::NavSatFix gps_fix_;           
  UpdateTimer update_timer_;                 

  Eigen::Matrix3f m_body_to_gps_;  

  void OnInitialize(const YAML::Node &config) override;

  void BeforePhysicsStep(const Timekeeper &timekeeper) override;

  void ParseParameters(const YAML::Node &config);

  void ComputeReferenceEcef();

  void UpdateFix();
};
}

#endif  // FLATLAND_PLUGINS_GPS_H