25 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_ROS_H 26 #define IMU_FILTER_MADWICK_IMU_FILTER_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> 39 #include "imu_filter_madgwick/ImuFilterMadgwickConfig.h" 44 typedef sensor_msgs::MagneticField
MagMsg;
109 const MagMsg::ConstPtr& mav_msg);
111 void imuCallback(
const ImuMsg::ConstPtr& imu_msg_raw);
119 float roll,
float pitch,
float yaw);
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
bool use_magnetic_field_msg_
message_filters::Synchronizer< SyncPolicy > Synchronizer
message_filters::Subscriber< MagMsg > MagSubscriber
void checkTopicsTimerCallback(const ros::TimerEvent &)
boost::shared_ptr< FilterConfigServer > config_server_
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
ros::Publisher mag_republisher_
void publishTransform(const ImuMsg::ConstPtr &imu_msg_raw)
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_