00001 00012 #ifndef ORIENTATION_FILTER_H_ 00013 #define ORIENTATION_FILTER_H_ 00014 00015 #include <ros/ros.h> 00016 #include <sensor_msgs/Imu.h> 00017 #include <sensor_msgs/JointState.h> 00018 00019 //Arm Parameters 00025 #define G 9.81 00026 00027 class OrientationFilter 00028 { 00029 public: 00033 OrientationFilter(); 00034 00035 private: 00041 void baseImuCallback(const sensor_msgs::Imu::ConstPtr& data); 00042 00048 void topImuCallback(const sensor_msgs::Imu::ConstPtr& data); 00049 00050 ros::NodeHandle n; 00051 00052 ros::Publisher frameJointStatePublisher; 00053 ros::Subscriber baseImuSubscriber; 00054 ros::Subscriber topImuSubscriber; 00056 sensor_msgs::JointState jointStates; 00057 bool baseOrientationInitialized; 00058 bool output; 00059 ros::Time prevUpdateTimeTop; 00060 ros::Time prevUpdateTime; 00061 float PPrevTop; 00062 float PPrev[2]; 00063 }; 00064 00065 #endif