imu_compass.h
Go to the documentation of this file.
00001 /*imu_compass.cpp
00002 Authors: Prasenjit (pmukherj@clearpathrobotics.com)
00003 Copyright (c) 2013, Clearpath Robotics, Inc., All rights reserved.
00004 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
00005 following conditions are met:
00006  * Redistributions of source code must retain the above copyright notice, this list of conditions and the following
00007    disclaimer.
00008  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
00009    disclaimer in the documentation and/or other materials provided with the distribution.
00010  * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote products
00011    derived from this software without specific prior written permission.
00012 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
00013 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00014 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00015 SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00016 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
00017 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00018 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00019 
00020 Description:
00021 Header file for IMU Compass Class that combines gyroscope and magnetometer data to get a clean estimate of yaw.
00022 */
00023 
00024 #include "ros/ros.h"
00025 #include "tf/tf.h"
00026 #include "tf/transform_listener.h"
00027 
00028 #include "sensor_msgs/Imu.h"
00029 #include "geometry_msgs/Vector3Stamped.h"
00030 #include "std_msgs/Float32.h"
00031 
00032 //typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
00033 
00034 class IMUCompass {
00035 
00036 private:
00037   ros::NodeHandle node_;
00038   ros::Subscriber imu_sub_;
00039   ros::Subscriber mag_sub_;
00040   ros::Subscriber decl_sub_;
00041   ros::Publisher imu_pub_;
00042   ros::Publisher mag_pub_;
00043   ros::Publisher compass_pub_;
00044   ros::Publisher raw_compass_pub_;
00045 
00046   tf::TransformListener listener_;
00047   ros::Timer debug_timer_;
00048 
00049   void imuCallback(sensor_msgs::ImuPtr data);
00050   void declCallback(const std_msgs::Float32& data);
00051   void magCallback(const geometry_msgs::Vector3StampedConstPtr& data);
00052   void debugCallback(const ros::TimerEvent&);
00053   void repackageImuPublish(tf::StampedTransform);
00054 
00055   //Heading Filter functions
00056   void initFilter(double heading_meas); //initialize heading fiter
00057   bool first_mag_reading_; //signifies receiving the first magnetometer message
00058   bool first_gyro_reading_; //signifies receiving the first gyroscope message
00059   bool filter_initialized_; //after receiving the first measurement, make sure the filter is initialized
00060   bool gyro_update_complete_; //sigfnifies that a gyro update (motion model update) has gone through
00061 
00062   double mag_zero_x_, mag_zero_y_, mag_zero_z_;
00063 
00064   sensor_msgs::ImuPtr curr_imu_reading_;
00065 
00066   //Heading Filter Variables
00067   //State and Variance
00068   double curr_heading_;
00069   double curr_heading_variance_;
00070   double sensor_timeout_;
00071 
00072   //Motion Update Variables
00073   double heading_prediction_;
00074   double heading_variance_prediction_;
00075   double heading_prediction_variance_;
00076   double mag_declination_;
00077   double last_motion_update_time_;
00078   double last_measurement_update_time_;
00079 
00080   //Measurement Update Variables
00081   double yaw_meas_variance_;
00082 
00083 public:
00084   IMUCompass(ros::NodeHandle &n);
00085   ~IMUCompass() {
00086   }
00087 };
00088 


imu_compass
Author(s):
autogenerated on Thu Jun 6 2019 20:40:40