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 <asctec_hl_comm/Wgs84ToEnu.h>
00024 #include <std_srvs/Empty.h>
00025 #include <Eigen/Eigen>
00026 
00027 namespace asctec_hl_gps{
00028 
00029 class GpsConversion
00030 {
00031   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, asctec_hl_comm::mav_imu> GpsImuSyncPolicy;
00032 private:
00033   ros::NodeHandle nh_;
00034   ros::Publisher gps_pose_pub_;
00035   ros::Publisher gps_position_pub_;
00036   ros::Publisher gps_custom_pub_;
00037   ros::ServiceServer zero_height_srv_;
00038   ros::ServiceServer gps_to_enu_srv_;
00039 
00040   message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub_sync_;
00041   message_filters::Subscriber<asctec_hl_comm::mav_imu> imu_sub_sync_;
00042   message_filters::Synchronizer<GpsImuSyncPolicy> gps_imu_sync_;
00043 
00044   ros::Subscriber gps_sub_;
00045   ros::Subscriber gps_custom_sub_;
00046   ros::Subscriber imu_sub_;
00047   geometry_msgs::Point gps_position_;
00048 
00049   bool have_reference_;
00050   double ref_latitude_;
00051   double ref_longitude_;
00052   double ref_altitude_;
00053   Eigen::Vector3d ecef_ref_point_;
00054   Eigen::Quaterniond ecef_ref_orientation_;
00055 
00056   double height_offset_;
00057   bool set_height_zero_;
00058 
00059   bool use_pressure_height_;
00060 
00061   static const double DEG2RAD = M_PI/180.0;
00062   const Eigen::Quaterniond Q_90_DEG;
00063 
00064   void syncCallback(const sensor_msgs::NavSatFixConstPtr & gps, const asctec_hl_comm::mav_imuConstPtr & imu);
00065   void gpsCallback(const sensor_msgs::NavSatFixConstPtr & gps);
00066   void gpsCustomCallback(const asctec_hl_comm::GpsCustomConstPtr & gps);
00067   void imuCallback(const asctec_hl_comm::mav_imuConstPtr & imu);
00068   void initReference(const double & latitude, const double & longitude, const double & altitude);
00069   Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude);
00070   Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef);
00071   bool wgs84ToEnuSrv(asctec_hl_comm::Wgs84ToEnuRequest & wgs84Pt,
00072                      asctec_hl_comm::Wgs84ToEnuResponse & enuPt);
00073   geometry_msgs::Point wgs84ToEnu(const double & latitude, const double & longitude, const double & altitude);
00074   geometry_msgs::Point wgs84ToNwu(const double & latitude, const double & longitude, const double & altitude);
00075   bool zeroHeightCb(std_srvs::EmptyRequest & req, std_srvs::EmptyResponse & resp);
00076 
00077 
00078 public:
00079   GpsConversion();
00080 };
00081 
00082 } // end namespace
00083 
00084 #endif /* GPS_FUSION_H_ */