00001 /* 00002 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions 00007 * are met: 00008 * 00009 * 1. Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * 2. Redistributions in binary form must reproduce the above 00012 * copyright notice, this list of conditions and the following 00013 * disclaimer in the documentation and/or other materials provided 00014 * with the distribution. 00015 * 3. Neither the name of the copyright holder nor the names of its 00016 * contributors may be used to endorse or promote products derived 00017 * from this software without specific prior written permission. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00020 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00021 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00022 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00023 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00024 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00025 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00026 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00027 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00028 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00029 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00030 * POSSIBILITY OF SUCH DAMAGE. 00031 */ 00032 00033 #ifndef ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H 00034 #define ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H 00035 00036 #include <robot_localization/SetDatum.h> 00037 00038 #include <ros/ros.h> 00039 00040 #include <nav_msgs/Odometry.h> 00041 #include <sensor_msgs/Imu.h> 00042 #include <sensor_msgs/NavSatFix.h> 00043 00044 #include <tf2/LinearMath/Transform.h> 00045 #include <tf2_ros/static_transform_broadcaster.h> 00046 #include <tf2_ros/buffer.h> 00047 #include <tf2_ros/transform_listener.h> 00048 00049 #include <Eigen/Dense> 00050 00051 #include <string> 00052 00053 namespace RobotLocalization 00054 { 00055 00056 class NavSatTransform 00057 { 00058 public: 00061 NavSatTransform(); 00062 00065 ~NavSatTransform(); 00066 00069 void run(); 00070 00071 private: 00074 void computeTransform(); 00075 00078 bool datumCallback(robot_localization::SetDatum::Request& request, robot_localization::SetDatum::Response&); 00079 00083 void getRobotOriginUtmPose(const tf2::Transform &gps_utm_pose, 00084 tf2::Transform &robot_utm_pose, 00085 const ros::Time &transform_time); 00086 00090 void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, 00091 tf2::Transform &robot_odom_pose, 00092 const ros::Time &transform_time); 00093 00097 void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg); 00098 00102 void imuCallback(const sensor_msgs::ImuConstPtr& msg); 00103 00107 void odomCallback(const nav_msgs::OdometryConstPtr& msg); 00108 00112 bool prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps); 00113 00117 bool prepareGpsOdometry(nav_msgs::Odometry &gps_odom); 00118 00122 void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg); 00123 00127 void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg); 00128 00133 std::string base_link_frame_id_; 00134 00137 bool broadcast_utm_transform_; 00138 00141 std::string gps_frame_id_; 00142 00147 ros::Time gps_update_time_; 00148 00153 bool gps_updated_; 00154 00157 bool has_transform_gps_; 00158 00161 bool has_transform_imu_; 00162 00165 bool has_transform_odom_; 00166 00169 Eigen::MatrixXd latest_odom_covariance_; 00170 00173 Eigen::MatrixXd latest_utm_covariance_; 00174 00177 tf2::Transform latest_utm_pose_; 00178 00181 tf2::Transform latest_world_pose_; 00182 00185 double magnetic_declination_; 00186 00191 ros::Time odom_update_time_; 00192 00197 bool odom_updated_; 00198 00201 bool publish_gps_; 00202 00205 tf2_ros::Buffer tf_buffer_; 00206 00209 tf2_ros::TransformListener tf_listener_; 00210 00213 bool transform_good_; 00214 00217 tf2::Quaternion transform_orientation_; 00218 00221 tf2::Transform transform_utm_pose_; 00222 00225 tf2::Transform transform_world_pose_; 00226 00230 bool use_manual_datum_; 00231 00234 bool use_odometry_yaw_; 00235 00238 tf2_ros::StaticTransformBroadcaster utm_broadcaster_; 00239 00242 double utm_odom_tf_yaw_; 00243 00246 tf2::Transform utm_world_transform_; 00247 00250 tf2::Transform utm_world_trans_inverse_; 00251 00254 std::string utm_zone_; 00255 00260 std::string world_frame_id_; 00261 00267 double yaw_offset_; 00268 00271 ros::Duration transform_timeout_; 00272 00277 bool zero_altitude_; 00278 }; 00279 00280 } // namespace RobotLocalization 00281 00282 #endif // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H