navsat_transform.h

Go to the documentation of this file.

/*
 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived
 * from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H
#define ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H

#include <robot_localization/SetDatum.h>

#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Dense>

#include <string>

namespace RobotLocalization
{

class NavSatTransform
{
  public:
    NavSatTransform();

    ~NavSatTransform();

    void run();

  private:
    void computeTransform();

    bool datumCallback(robot_localization::SetDatum::Request& request, robot_localization::SetDatum::Response&);

    void getRobotOriginUtmPose(const tf2::Transform &gps_utm_pose,
                               tf2::Transform &robot_utm_pose,
                               const ros::Time &transform_time);

    void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose,
                                 tf2::Transform &robot_odom_pose,
                                 const ros::Time &transform_time);

    void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg);

    void imuCallback(const sensor_msgs::ImuConstPtr& msg);

    void odomCallback(const nav_msgs::OdometryConstPtr& msg);

    bool prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps);

    bool prepareGpsOdometry(nav_msgs::Odometry &gps_odom);

    void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg);

    void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg);

    std::string base_link_frame_id_;

    bool broadcast_utm_transform_;

    bool broadcast_utm_transform_as_parent_frame_;

    std::string gps_frame_id_;

    ros::Time gps_update_time_;

    bool gps_updated_;

    bool has_transform_gps_;

    bool has_transform_imu_;

    bool has_transform_odom_;

    Eigen::MatrixXd latest_odom_covariance_;

    Eigen::MatrixXd latest_utm_covariance_;

    tf2::Transform latest_utm_pose_;

    tf2::Transform latest_world_pose_;

    double magnetic_declination_;

    ros::Time odom_update_time_;

    bool odom_updated_;

    bool publish_gps_;

    tf2_ros::Buffer tf_buffer_;

    tf2_ros::TransformListener tf_listener_;

    bool transform_good_;

    tf2::Quaternion transform_orientation_;

    tf2::Transform transform_utm_pose_;

    tf2::Transform transform_world_pose_;

    bool use_manual_datum_;

    bool use_odometry_yaw_;

    tf2_ros::StaticTransformBroadcaster utm_broadcaster_;

    double utm_odom_tf_yaw_;

    tf2::Transform utm_world_transform_;

    tf2::Transform utm_world_trans_inverse_;

    std::string utm_zone_;

    std::string world_frame_id_;

    double yaw_offset_;

    double utm_meridian_convergence_;

    ros::Duration transform_timeout_;

    bool zero_altitude_;

    bool world_frame_param_loaded_;

    bool base_link_frame_param_loaded_;
};

}  // namespace RobotLocalization

#endif  // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H