00001 #include <ros/ros.h> 00002 00003 #include <nav_msgs/Odometry.h> 00004 #include <sensor_msgs/Imu.h> 00005 #include <sensor_msgs/NavSatFix.h> 00006 00007 #include <tf2/LinearMath/Transform.h> 00008 #include <tf2_ros/static_transform_broadcaster.h> 00009 #include <tf2_ros/buffer.h> 00010 #include <tf2_ros/transform_listener.h> 00011 00012 #include <Eigen/Dense> 00013 00014 namespace RobotLocalization 00015 { 00016 class NavSatTransformNew 00017 { 00018 public: 00019 NavSatTransformNew(); 00020 00021 ~NavSatTransformNew(); 00022 void run(); 00023 00024 protected: 00025 00026 void odomCallback(const nav_msgs::OdometryConstPtr& msg); 00027 void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg); 00028 void imuCallback(const sensor_msgs::ImuConstPtr& msg); 00029 00030 void localOdomCallback(const nav_msgs::OdometryConstPtr& msg); 00031 00032 void computeTransform(); 00033 bool prepareGpsOdometry(nav_msgs::Odometry &gpsOdom); 00034 00035 void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg); 00036 void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg); 00037 00038 //MEMBERS FROM NAVSAT_TRANSFORM OF ROBOT_LOCALIZATION 00039 00040 bool broadcastUtmTransform_; 00041 00042 double magneticDeclination_; 00043 double utmOdomTfYaw_; 00044 00045 bool hasTransformGps_; 00046 bool hasTransformOdom_; 00047 bool hasTransformImu_; 00048 bool transformGood_; 00049 00050 bool gpsUpdated_; 00051 bool odomUpdated_; 00052 00053 ros::Time gpsUpdateTime_; 00054 00055 ros::Time odomUpdateTime_; 00056 00057 double yawOffset_; 00058 00059 bool zeroAltitude_; 00060 00061 bool publishGps_; 00062 00063 std::string baseLinkFrameId_; 00064 std::string worldFrameId_; 00065 00066 std::string utmZone_; 00067 00068 tf2::Transform latestWorldPose_; 00069 tf2::Transform latestUtmPose_; 00070 00071 tf2::Transform transformUtmPose_; 00072 tf2::Transform transformWorldPose_; 00073 00074 tf2::Quaternion transformOrientation_; 00075 00076 Eigen::MatrixXd latestUtmCovariance_; 00077 Eigen::MatrixXd latestOdomCovariance_; 00078 00079 tf2_ros::Buffer tfBuffer_; 00080 tf2_ros::TransformListener tfListener_; 00081 tf2_ros::StaticTransformBroadcaster utmBroadcaster_; 00082 00083 tf2::Transform utmWorldTransform_; 00084 tf2::Transform utmWorldTransInverse_; 00085 00086 //NEW MEMBERS 00087 00090 double travelledDistance_; 00091 double previous_x_; 00092 double previous_y_; 00093 00095 std::string localOdomTopic_; 00096 bool hasToPublish_; 00097 }; 00098 }