tf_handler.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, 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:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * Neither the name of Willow Garage, Inc. 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 OWNER 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.
 *
 * Author: Sebastian Klose
 *********************************************************************/

#ifndef IMU_FILTER_TF_BROADCAST_H
#define IMU_FILTER_TF_BROADCAST_H

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <imu_filter/imu_state.h>
#include <imu_filter/tf_helper.h>
#include <imu_filter/message_helper.h>

namespace imu_filter
{
        //* Singleton helper class for sending out TF messages
        class TfHandler
        {
                public:
                        static TfHandler & instance()
                        {
                                static TfHandler instance_;
                                return instance_;
                        }

                        void broadcastIMUFrame( const Eigen::Quaterniond & orientation, const Eigen::Vector3d & position, const ros::Time & time )
                        {
                                tf_helper::toTfTransform( orientation, position, imuFrame_ );

                                bc_.sendTransform( tf::StampedTransform( imuFrame_, time, baseFrame_, "imu_local" ) );
                        }

                        void setPseudoObservation( const Observation::Vector & vec, const ros::Time & time )
                        {
                                tf_helper::toTfTransform( vec, imuFrame_ );
                                bc_.sendTransform( tf::StampedTransform( imuFrame_, time, baseFrame_, "imu_pseudo_obs" ) );

                                qMsg_.x = vec[ 0 ];
                                qMsg_.y = vec[ 1 ];
                                qMsg_.z = vec[ 2 ];
                                qMsg_.w = vec[ 3 ];
                                vMsg_.x = vec[ 4 ];
                                vMsg_.y = vec[ 5 ];
                                vMsg_.z = vec[ 6 ];
                                pubPseudoRot_.publish( qMsg_ );
                                pubPseudoPos_.publish( vMsg_ );
                        }

                        void broadcastTfPose( const Eigen::Affine3d & pose, const ros::Time & t, const std::string & frame )
                        {
                                tf_helper::toTfTransform( pose, imuFrame_ );
                                bc_.sendTransform( tf::StampedTransform( imuFrame_, t, baseFrame_, frame ) );
                        }

                        
                        void publishImuState( const ImuState & state, const ros::Time & t )             
                        {
                                // get the imu position and orientation and turn it into a world estimate
                                Eigen::Affine3d world2Imu;
                                world2Imu.linear() = state.orientation().toRotationMatrix();
                                world2Imu.translation() = state.position();

                                Eigen::Affine3d body2Imu;
                                body2Imu.linear() = state.body2ImuRotation().toRotationMatrix();
                                body2Imu.translation() = state.body2ImuTranslation();

                                Eigen::Affine3d world2Body = world2Imu * body2Imu.inverse();

                                poseMsg_.header.stamp = t;
                                poseMsg_.header.frame_id = baseFrame_;                  
                                message_helper::eigenPose2Msg( world2Body, poseMsg_ );
                                pubImuPose_.publish( poseMsg_ );        

                                message_helper::quaternionToMsg( state.orientation(), qMsg_ );
                                pubImuRot_.publish( qMsg_ );

                                message_helper::vec3ToMsg( state.position(), vMsg_ );
                                pubImuPos_.publish( vMsg_ );

                                message_helper::vec3ToMsg( state.velocity(), vMsg_ );
                                pubImuVel_.publish( vMsg_ );

                                message_helper::vec3ToMsg( state.gyroBias(), vMsg_ );
                                pubImuGBias_.publish( vMsg_ );

                                message_helper::vec3ToMsg( state.accBias(), vMsg_ );
                                pubImuABias_.publish( vMsg_ );

                                message_helper::vec3ToMsg( state.gravity(), vMsg_ );
                                pubGravity_.publish( vMsg_ );
                        }

                        void setBaseFrame( const std::string & frame_id ){ baseFrame_ = frame_id; }

                        void publishKFRelativePose( const Eigen::Affine3d & pose, const ros::Time & t )
                        {
                                message_helper::eigenPose2Msg( pose, poseMsg_ );
                                poseMsg_.header.stamp = t;
                                poseMsg_.header.frame_id = baseFrame_;
                                pubPoseRelativeToKf_.publish( poseMsg_ );
                        }

                private:
                        ros::NodeHandle nh_;

                        TfHandler() :
                                baseFrame_( "map" )
                        {
                                imuFrame_.setOrigin( tf::Vector3( 0, 0, 0 ) );
                                pubPseudoRot_ = nh_.advertise<geometry_msgs::Quaternion>( "pseudoRot", 1 );
                                pubPseudoPos_ = nh_.advertise<geometry_msgs::Vector3>( "pseudoPos", 1 );
                                pubGtRot_         = nh_.advertise<geometry_msgs::Quaternion>( "gtRot", 1 );
                                pubGtPos_         = nh_.advertise<geometry_msgs::Vector3>( "gtPos", 1 );
                                pubImuPose_       = nh_.advertise<geometry_msgs::PoseStamped>( "imuCamPoseEstimate", 1 );
                                pubImuRot_        = nh_.advertise<geometry_msgs::Quaternion>( "imu_rot", 1 );
                                pubImuPos_        = nh_.advertise<geometry_msgs::Vector3>( "imu_pos", 1 );
                                pubImuVel_        = nh_.advertise<geometry_msgs::Vector3>( "imu_vel", 1 );
                                pubImuGBias_  = nh_.advertise<geometry_msgs::Vector3>( "imu_gyro_bias", 1 );
                                pubImuABias_  = nh_.advertise<geometry_msgs::Vector3>( "imu_acc_bias", 1 );
                                pubGravity_   = nh_.advertise<geometry_msgs::Vector3>( "imu_gravity", 1 );
                                pubPoseRelativeToKf_ = nh_.advertise<geometry_msgs::PoseStamped>( "imuCamPoseRelative2KF", 1 );
                        }

                        TfHandler( const TfHandler & );

                        std::string                                     baseFrame_;
                        tf::TransformBroadcaster        bc_;
                        tf::TransformListener           tfIn_;
                        tf::Transform                           imuFrame_;
                        tf::StampedTransform            tmpT_;

                        ros::Publisher                          pubPseudoRot_;
                        ros::Publisher                          pubPseudoPos_;
                        ros::Publisher                          pubGtRot_;
                        ros::Publisher                          pubGtPos_;
                        ros::Publisher                          pubImuPose_;
                        ros::Publisher                          pubImuRot_;
                        ros::Publisher                          pubImuPos_;
                        ros::Publisher                          pubImuVel_;
                        ros::Publisher                          pubImuGBias_;
                        ros::Publisher                          pubImuABias_;
                        ros::Publisher                          pubGravity_;
                        ros::Publisher                          pubPoseRelativeToKf_;

                        geometry_msgs::Quaternion       qMsg_;
                        geometry_msgs::Vector3          vMsg_;
                        geometry_msgs::PoseStamped      poseMsg_;
        };

}

#endif