imu_filter_ros.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010, CCNY Robotics Lab
3  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
4  *
5  * http://robotics.ccny.cuny.edu
6  *
7  * Based on implementation of Madgwick's IMU and AHRS algorithms.
8  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, either version 3 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see <http://www.gnu.org/licenses/>.
23  */
24 
25 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
26 #define IMU_FILTER_MADWICK_IMU_FILTER_ROS_H
27 
28 #include <ros/ros.h>
29 #include <sensor_msgs/Imu.h>
30 #include <sensor_msgs/MagneticField.h>
31 #include <geometry_msgs/Vector3Stamped.h>
36 #include <dynamic_reconfigure/server.h>
37 
39 #include "imu_filter_madgwick/ImuFilterMadgwickConfig.h"
40 
42 {
43  typedef sensor_msgs::Imu ImuMsg;
44  typedef sensor_msgs::MagneticField MagMsg;
45  typedef geometry_msgs::Vector3Stamped MagVectorMsg;
46 
52 
53  typedef imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig;
54  typedef dynamic_reconfigure::Server<FilterConfig> FilterConfigServer;
55 
56  public:
57 
59  virtual ~ImuFilterRos();
60 
61  private:
62 
63  // **** ROS-related
64 
67 
71 
72  // Adapter to support the use_magnetic_field_msg param.
75 
80 
83 
84  // **** paramaters
86  bool use_mag_;
88  bool stateless_;
91  std::string fixed_frame_;
92  std::string imu_frame_;
93  double constant_dt_;
96  geometry_msgs::Vector3 mag_bias_;
98 
99  // **** state variables
100  boost::mutex mutex_;
103 
104  // **** filter implementation
106 
107  // **** member functions
108  void imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
109  const MagMsg::ConstPtr& mav_msg);
110 
111  void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw);
112 
113  void imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg);
114 
115  void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw);
116  void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw);
117 
118  void publishRawMsg(const ros::Time& t,
119  float roll, float pitch, float yaw);
120 
121  void reconfigCallback(FilterConfig& config, uint32_t level);
123 };
124 
125 #endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H
geometry_msgs::Vector3Stamped MagVectorMsg
boost::shared_ptr< MagVectorSubscriber > vector_mag_subscriber_
message_filters::Subscriber< ImuMsg > ImuSubscriber
message_filters::sync_policies::ApproximateTime< ImuMsg, MagMsg > SyncPolicy
bool publish_debug_topics_
boost::shared_ptr< Synchronizer > sync_
double orientation_variance_
void publishRawMsg(const ros::Time &t, float roll, float pitch, float yaw)
void imuMagVectorCallback(const MagVectorMsg::ConstPtr &mag_vector_msg)
void imuMagCallback(const ImuMsg::ConstPtr &imu_msg_raw, const MagMsg::ConstPtr &mav_msg)
ros::Publisher rpy_raw_debug_publisher_
imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig
sensor_msgs::Imu ImuMsg
bool use_magnetic_field_msg_
double constant_dt_
message_filters::Synchronizer< SyncPolicy > Synchronizer
std::string imu_frame_
message_filters::Subscriber< MagMsg > MagSubscriber
ros::Time last_time_
void checkTopicsTimerCallback(const ros::TimerEvent &)
boost::shared_ptr< FilterConfigServer > config_server_
std::string fixed_frame_
ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
ros::NodeHandle nh_private_
boost::shared_ptr< ImuSubscriber > imu_subscriber_
sensor_msgs::MagneticField MagMsg
bool remove_gravity_vector_
message_filters::Subscriber< MagVectorMsg > MagVectorSubscriber
ros::Publisher imu_publisher_
dynamic_reconfigure::Server< FilterConfig > FilterConfigServer
boost::mutex mutex_
ros::Publisher mag_republisher_
virtual ~ImuFilterRos()
void publishTransform(const ImuMsg::ConstPtr &imu_msg_raw)
ros::NodeHandle nh_
ImuFilter filter_
ros::Timer check_topics_timer_
void reconfigCallback(FilterConfig &config, uint32_t level)
void publishFilteredMsg(const ImuMsg::ConstPtr &imu_msg_raw)
WorldFrame::WorldFrame world_frame_
tf2_ros::TransformBroadcaster tf_broadcaster_
void imuCallback(const ImuMsg::ConstPtr &imu_msg_raw)
boost::shared_ptr< MagSubscriber > mag_subscriber_
ros::Publisher rpy_filtered_debug_publisher_
geometry_msgs::Vector3 mag_bias_


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00