00001 /* 00002 * Copyright (C) 2010, CCNY Robotics Lab 00003 * Ivan Dryanovski <ivan.dryanovski@gmail.com> 00004 * 00005 * http://robotics.ccny.cuny.edu 00006 * 00007 * Based on implementation of Madgwick's IMU and AHRS algorithms. 00008 * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 00009 * 00010 * 00011 * This program is free software: you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation, either version 3 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU General Public License for more details. 00020 * 00021 * You should have received a copy of the GNU General Public License 00022 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00023 */ 00024 00025 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H 00026 #define IMU_FILTER_MADWICK_IMU_FILTER_ROS_H 00027 00028 #include <ros/ros.h> 00029 #include <sensor_msgs/Imu.h> 00030 #include <sensor_msgs/MagneticField.h> 00031 #include <geometry_msgs/Vector3Stamped.h> 00032 #include "tf2_ros/transform_broadcaster.h" 00033 #include <message_filters/subscriber.h> 00034 #include <message_filters/synchronizer.h> 00035 #include <message_filters/sync_policies/approximate_time.h> 00036 #include <dynamic_reconfigure/server.h> 00037 00038 #include "imu_filter_madgwick/imu_filter.h" 00039 #include "imu_filter_madgwick/ImuFilterMadgwickConfig.h" 00040 00041 class ImuFilterRos 00042 { 00043 typedef sensor_msgs::Imu ImuMsg; 00044 typedef sensor_msgs::MagneticField MagMsg; 00045 typedef geometry_msgs::Vector3Stamped MagVectorMsg; 00046 00047 typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg> SyncPolicy; 00048 typedef message_filters::Synchronizer<SyncPolicy> Synchronizer; 00049 typedef message_filters::Subscriber<ImuMsg> ImuSubscriber; 00050 typedef message_filters::Subscriber<MagMsg> MagSubscriber; 00051 typedef message_filters::Subscriber<MagVectorMsg> MagVectorSubscriber; 00052 00053 typedef imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig; 00054 typedef dynamic_reconfigure::Server<FilterConfig> FilterConfigServer; 00055 00056 public: 00057 00058 ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private); 00059 virtual ~ImuFilterRos(); 00060 00061 private: 00062 00063 // **** ROS-related 00064 00065 ros::NodeHandle nh_; 00066 ros::NodeHandle nh_private_; 00067 00068 boost::shared_ptr<ImuSubscriber> imu_subscriber_; 00069 boost::shared_ptr<MagSubscriber> mag_subscriber_; 00070 boost::shared_ptr<Synchronizer> sync_; 00071 00072 // Adapter to support the use_magnetic_field_msg param. 00073 boost::shared_ptr<MagVectorSubscriber> vector_mag_subscriber_; 00074 ros::Publisher mag_republisher_; 00075 00076 ros::Publisher rpy_filtered_debug_publisher_; 00077 ros::Publisher rpy_raw_debug_publisher_; 00078 ros::Publisher imu_publisher_; 00079 tf2_ros::TransformBroadcaster tf_broadcaster_; 00080 00081 boost::shared_ptr<FilterConfigServer> config_server_; 00082 ros::Timer check_topics_timer_; 00083 00084 // **** paramaters 00085 WorldFrame::WorldFrame world_frame_; 00086 bool use_mag_; 00087 bool use_magnetic_field_msg_; 00088 bool stateless_; 00089 bool publish_tf_; 00090 bool reverse_tf_; 00091 std::string fixed_frame_; 00092 std::string imu_frame_; 00093 double constant_dt_; 00094 bool publish_debug_topics_; 00095 geometry_msgs::Vector3 mag_bias_; 00096 double orientation_variance_; 00097 00098 // **** state variables 00099 boost::mutex mutex_; 00100 bool initialized_; 00101 ros::Time last_time_; 00102 00103 // **** filter implementation 00104 ImuFilter filter_; 00105 00106 // **** member functions 00107 void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw, 00108 const MagMsg::ConstPtr& mav_msg); 00109 00110 void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw); 00111 00112 void imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg); 00113 00114 void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw); 00115 void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw); 00116 00117 void publishRawMsg(const ros::Time& t, 00118 float roll, float pitch, float yaw); 00119 00120 void reconfigCallback(FilterConfig& config, uint32_t level); 00121 void checkTopicsTimerCallback(const ros::TimerEvent&); 00122 }; 00123 00124 #endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H