Program Listing for File imu_filter_ros.h
↰ Return to documentation for file (/tmp/ws/src/imu_tools/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h
)
/*
* Copyright (C) 2010, CCNY Robotics Lab
* Ivan Dryanovski <ivan.dryanovski@gmail.com>
*
* http://robotics.ccny.cuny.edu
*
* Based on implementation of Madgwick's IMU and AHRS algorithms.
* http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include "imu_filter_madgwick/imu_filter.h"
#include "imu_filter_madgwick/base_node.hpp"
#include "imu_filter_madgwick/visibility_control.h"
class ImuFilterMadgwickRos : public imu_filter::BaseNode
{
typedef sensor_msgs::msg::Imu ImuMsg;
typedef sensor_msgs::msg::MagneticField MagMsg;
typedef geometry_msgs::msg::Vector3Stamped RpyVectorMsg;
typedef message_filters::sync_policies::ApproximateTime<ImuMsg, MagMsg>
SyncPolicy;
typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
typedef message_filters::Subscriber<ImuMsg> ImuSubscriber;
typedef message_filters::Subscriber<MagMsg> MagSubscriber;
public:
IMU_FILTER_MADGWICK_CPP_PUBLIC
explicit ImuFilterMadgwickRos(const rclcpp::NodeOptions& options);
// Callbacks are public so they can be called when used as a library
void imuCallback(ImuMsg::ConstSharedPtr imu_msg_raw);
void imuMagCallback(ImuMsg::ConstSharedPtr imu_msg_raw,
MagMsg::ConstSharedPtr mag_msg);
private:
std::shared_ptr<ImuSubscriber> imu_subscriber_;
std::shared_ptr<MagSubscriber> mag_subscriber_;
std::shared_ptr<Synchronizer> sync_;
rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_filtered_debug_publisher_;
rclcpp::Publisher<RpyVectorMsg>::SharedPtr rpy_raw_debug_publisher_;
rclcpp::Publisher<ImuMsg>::SharedPtr imu_publisher_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
rclcpp::TimerBase::SharedPtr check_topics_timer_;
// Subscription for parameter change
rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
parameter_event_sub_;
// **** paramaters
WorldFrame::WorldFrame world_frame_;
bool use_mag_{};
bool stateless_{};
bool publish_tf_{};
bool reverse_tf_{};
std::string fixed_frame_;
std::string imu_frame_;
double constant_dt_;
bool publish_debug_topics_{};
bool remove_gravity_vector_{};
geometry_msgs::msg::Vector3 mag_bias_;
double orientation_variance_;
double yaw_offset_total_;
// **** state variables
std::mutex mutex_;
bool initialized_;
rclcpp::Time last_time_;
tf2::Quaternion yaw_offsets_;
// **** filter implementation
ImuFilter filter_;
// **** member functions
void publishFilteredMsg(ImuMsg::ConstSharedPtr imu_msg_raw);
void publishTransform(ImuMsg::ConstSharedPtr imu_msg_raw);
void publishRawMsg(const rclcpp::Time& t, float roll, float pitch,
float yaw);
void reconfigCallback(rcl_interfaces::msg::ParameterEvent::SharedPtr event);
void checkTopicsTimerCallback();
void applyYawOffset(double& q0, double& q1, double& q2, double& q3);
};