00001 /* 00002 @author Roberto G. Valenti <robertogl.valenti@gmail.com> 00003 00004 @section LICENSE 00005 Copyright (c) 2015, City University of New York 00006 CCNY Robotics Lab <http://robotics.ccny.cuny.edu> 00007 All rights reserved. 00008 00009 Redistribution and use in source and binary forms, with or without 00010 modification, are permitted provided that the following conditions are met: 00011 1. Redistributions of source code must retain the above copyright 00012 notice, this list of conditions and the following disclaimer. 00013 2. Redistributions in binary form must reproduce the above copyright 00014 notice, this list of conditions and the following disclaimer in the 00015 documentation and/or other materials provided with the distribution. 00016 3. Neither the name of the City College of New York nor the 00017 names of its contributors may be used to endorse or promote products 00018 derived from this software without specific prior written permission. 00019 00020 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00021 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00022 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00023 DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY 00024 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00025 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00026 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00027 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00028 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00029 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00030 */ 00031 00032 #ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 00033 #define IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H 00034 00035 #include <sensor_msgs/MagneticField.h> 00036 #include <geometry_msgs/Vector3Stamped.h> 00037 #include <message_filters/subscriber.h> 00038 #include <message_filters/sync_policies/approximate_time.h> 00039 #include <message_filters/synchronizer.h> 00040 #include <ros/ros.h> 00041 #include <sensor_msgs/Imu.h> 00042 #include <tf/transform_datatypes.h> 00043 #include <tf/transform_broadcaster.h> 00044 00045 #include "imu_complementary_filter/complementary_filter.h" 00046 00047 namespace imu_tools { 00048 00049 class ComplementaryFilterROS 00050 { 00051 public: 00052 ComplementaryFilterROS(const ros::NodeHandle& nh, 00053 const ros::NodeHandle& nh_private); 00054 virtual ~ComplementaryFilterROS(); 00055 00056 private: 00057 00058 // Convenience typedefs 00059 typedef sensor_msgs::Imu ImuMsg; 00060 typedef sensor_msgs::MagneticField MagMsg; 00061 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, 00062 MagMsg> MySyncPolicy; 00063 typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> 00064 SyncPolicy; 00065 typedef message_filters::Synchronizer<SyncPolicy> Synchronizer; 00066 typedef message_filters::Subscriber<ImuMsg> ImuSubscriber; 00067 typedef message_filters::Subscriber<MagMsg> MagSubscriber; 00068 00069 // ROS-related variables. 00070 ros::NodeHandle nh_; 00071 ros::NodeHandle nh_private_; 00072 00073 boost::shared_ptr<Synchronizer> sync_; 00074 boost::shared_ptr<ImuSubscriber> imu_subscriber_; 00075 boost::shared_ptr<MagSubscriber> mag_subscriber_; 00076 00077 ros::Publisher imu_publisher_; 00078 ros::Publisher rpy_publisher_; 00079 ros::Publisher state_publisher_; 00080 tf::TransformBroadcaster tf_broadcaster_; 00081 00082 // Parameters: 00083 bool use_mag_; 00084 bool publish_tf_; 00085 bool reverse_tf_; 00086 double constant_dt_; 00087 bool publish_debug_topics_; 00088 std::string fixed_frame_; 00089 00090 // State variables: 00091 ComplementaryFilter filter_; 00092 ros::Time time_prev_; 00093 bool initialized_filter_; 00094 00095 void initializeParams(); 00096 void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw); 00097 void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, 00098 const MagMsg::ConstPtr& mav_msg); 00099 void publish(const sensor_msgs::Imu::ConstPtr& imu_msg_raw); 00100 00101 tf::Quaternion hamiltonToTFQuaternion( 00102 double q0, double q1, double q2, double q3) const; 00103 }; 00104 00105 } // namespace imu_tools 00106 00107 #endif // IMU_TOOLS_COMPLEMENTARY_FILTER_ROS_H