00001 #ifndef IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET 00002 #define IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET 00003 00004 #include "ros/ros.h" 00005 #include "tf2_ros/buffer.h" 00006 #include "tf2_ros/transform_listener.h" 00007 #include "tf2_ros/message_filter.h" 00008 #include "sensor_msgs/Imu.h" 00009 #include "sensor_msgs/MagneticField.h" 00010 #include "nodelet/nodelet.h" 00011 #include "message_filters/subscriber.h" 00012 #include "topic_tools/shape_shifter.h" 00013 00014 #include <string> 00015 00016 namespace imu_transformer 00017 { 00018 typedef sensor_msgs::Imu ImuMsg; 00019 typedef sensor_msgs::MagneticField MagMsg; 00020 typedef message_filters::Subscriber<ImuMsg> ImuSubscriber; 00021 // typedef message_filters::Subscriber<MagMsg> MagSubscriber; 00022 typedef message_filters::Subscriber<topic_tools::ShapeShifter> MagSubscriber; 00023 typedef tf2_ros::MessageFilter<ImuMsg> ImuFilter; 00024 typedef tf2_ros::MessageFilter<MagMsg> MagFilter; 00025 00026 class ImuTransformerNodelet : public nodelet::Nodelet 00027 { 00028 00029 public: 00030 ImuTransformerNodelet() {}; 00031 00032 private: 00033 00034 std::string target_frame_; 00035 00036 ros::NodeHandle nh_in_, nh_out_, private_nh_; 00037 boost::shared_ptr<tf2_ros::Buffer> tf2_; 00038 boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_; 00039 00040 ImuSubscriber imu_sub_; 00041 MagSubscriber mag_sub_; 00042 00043 boost::shared_ptr<ImuFilter> imu_filter_; 00044 boost::shared_ptr<MagFilter> mag_filter_; 00045 00046 ros::Publisher imu_pub_, mag_pub_; 00047 00048 virtual void onInit(); 00049 void imuCallback(const ImuMsg::ConstPtr &imu_in); 00050 // void magCallback(const MagMsg::ConstPtr &mag_in); 00051 void magCallback(const topic_tools::ShapeShifter::ConstPtr &msg); 00052 void failureCb(tf2_ros::filter_failure_reasons::FilterFailureReason reason); 00053 00054 }; 00055 00056 } // namespace imu_transformer 00057 00058 #endif // IMU_TRANSFORMER_IMU_TRANSFORMER_NODELET