imu_transformer_nodelet.h

Go to the documentation of this file.

#ifndef IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET
#define IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET
 
#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "nodelet/nodelet.h"
#include "message_filters/subscriber.h"
#include "topic_tools/shape_shifter.h"
 
#include <string>
 
namespace imu_transformer
{
  typedef sensor_msgs::Imu ImuMsg;
  typedef sensor_msgs::MagneticField MagMsg;
  typedef message_filters::Subscriber<ImuMsg> ImuSubscriber;
//  typedef message_filters::Subscriber<MagMsg> MagSubscriber;
  typedef message_filters::Subscriber<topic_tools::ShapeShifter> MagSubscriber;
  typedef tf2_ros::MessageFilter<ImuMsg> ImuFilter;
  typedef tf2_ros::MessageFilter<MagMsg> MagFilter;
 
  class ImuTransformerNodelet : public nodelet::Nodelet
  {
 
  public:
    ImuTransformerNodelet() {};
 
  private:
 
    std::string target_frame_;
 
    ros::NodeHandle nh_in_, nh_out_, private_nh_;
    boost::shared_ptr<tf2_ros::Buffer> tf2_;
    boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
    ImuSubscriber imu_sub_;
    MagSubscriber mag_sub_;
 
    boost::shared_ptr<ImuFilter> imu_filter_;
    boost::shared_ptr<MagFilter> mag_filter_;
 
    ros::Publisher imu_pub_, mag_pub_;
 
    virtual void onInit();
    void imuCallback(const ImuMsg::ConstPtr &imu_in);
//    void magCallback(const MagMsg::ConstPtr &mag_in);
    void magCallback(const topic_tools::ShapeShifter::ConstPtr &msg);
    void failureCb(tf2_ros::filter_failure_reasons::FilterFailureReason reason);
 
  };
 
}  // namespace imu_transformer
 
#endif  // IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET