asctec_hl_gps: gps_conversion.h Source File

Go to the documentation of this file.
00001 /*
00002  * gps_fusion.h
00003  *
00004  *  Created on: Jun 22, 2011
00005  *      Author: acmarkus
00006  */
00007 
00008 #ifndef GPS_CONVERSION_H_
00009 #define GPS_CONVERSION_H_
00010 
00011 #include <ros/ros.h>
00012 
00013 #include <message_filters/subscriber.h>
00014 #include <message_filters/synchronizer.h>
00015 #include <message_filters/sync_policies/approximate_time.h>
00016 
00017 #include <sensor_msgs/NavSatFix.h>
00018 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00019 #include <asctec_hl_comm/PositionWithCovarianceStamped.h>
00020 #include <asctec_hl_comm/mav_imu.h>
00021 #include <asctec_hl_comm/GpsCustom.h>
00022 #include <asctec_hl_comm/GpsCustomCartesian.h>
00023 #include <std_srvs/Empty.h>
00024 #include <Eigen/Eigen>
00025 
00026 namespace asctec_hl_gps{
00027 
00028 class GpsConversion
00029 {
00030   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, asctec_hl_comm::mav_imu> GpsImuSyncPolicy;
00031 private:
00032   ros::NodeHandle nh_;
00033   ros::Publisher gps_pose_pub_;
00034   ros::Publisher gps_position_pub_;
00035   ros::Publisher gps_custom_pub_;
00036   ros::ServiceServer zero_height_srv_;
00037 
00038   message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub_sync_;
00039   message_filters::Subscriber<asctec_hl_comm::mav_imu> imu_sub_sync_;
00040   message_filters::Synchronizer<GpsImuSyncPolicy> gps_imu_sync_;
00041 
00042   ros::Subscriber gps_sub_;
00043   ros::Subscriber gps_custom_sub_;
00044   ros::Subscriber imu_sub_;
00045   geometry_msgs::Point gps_position_;
00046 
00047   bool have_reference_;
00048   double ref_latitude_;
00049   double ref_longitude_;
00050   double ref_altitude_;
00051   Eigen::Vector3d ecef_ref_point_;
00052   Eigen::Quaterniond ecef_ref_orientation_;
00053 
00054   double height_offset_;
00055   bool set_height_zero_;
00056 
00057   bool use_pressure_height_;
00058 
00059   static const double DEG2RAD = M_PI/180.0;
00060   const Eigen::Quaterniond Q_90_DEG;
00061 
00062   void syncCallback(const sensor_msgs::NavSatFixConstPtr & gps, const asctec_hl_comm::mav_imuConstPtr & imu);
00063   void gpsCallback(const sensor_msgs::NavSatFixConstPtr & gps);
00064   void gpsCustomCallback(const asctec_hl_comm::GpsCustomConstPtr & gps);
00065   void imuCallback(const asctec_hl_comm::mav_imuConstPtr & imu);
00066   void initReference(const double & latitude, const double & longitude, const double & altitude);
00067   Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude);
00068   Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef);
00069   geometry_msgs::Point wgs84ToEnu(const double & latitude, const double & longitude, const double & altitude);
00070   geometry_msgs::Point wgs84ToNwu(const double & latitude, const double & longitude, const double & altitude);
00071   bool zeroHeightCb(std_srvs::EmptyRequest & req, std_srvs::EmptyResponse & resp);
00072 
00073 
00074 public:
00075   GpsConversion();
00076 };
00077 
00078 } // end namespace
00079 
00080 #endif /* GPS_FUSION_H_ */