00001 #ifndef MAV_ODOM_INTERFACE_ODOM_INTERFACE_H 00002 #define MAV_ODOM_INTERFACE_ODOM_INTERFACE_H 00003 00004 #include <ros/ros.h> 00005 #include <boost/thread/mutex.hpp> 00006 #include <sensor_msgs/Imu.h> 00007 #include <std_msgs/Float64.h> 00008 #include <tf/transform_datatypes.h> 00009 #include <tf/transform_broadcaster.h> 00010 #include <nav_msgs/Odometry.h> 00011 #include <geometry_msgs/PoseStamped.h> 00012 #include <geometry_msgs/TwistStamped.h> 00013 00014 #include <mav_msgs/Height.h> 00015 00016 namespace mav { 00017 00018 class OdomInterface 00019 { 00020 00021 // for campatibility b/n ROS Electric and Fuerte 00022 #if ROS_VERSION_MINIMUM(1, 8, 0) 00023 typedef tf::Matrix3x3 MyMatrix; 00024 #else 00025 typedef btMatrix3x3 MyMatrix; 00026 #endif 00027 00028 public: 00029 00030 OdomInterface(ros::NodeHandle nh, ros::NodeHandle nh_private); 00031 virtual ~OdomInterface(); 00032 00033 private: 00034 00035 typedef geometry_msgs::PoseStamped PoseStamped; 00036 typedef geometry_msgs::TwistStamped TwistStamped; 00037 00038 // **** ros-related variables 00039 00040 ros::NodeHandle nh_; 00041 ros::NodeHandle nh_private_; 00042 ros::Publisher pose_publisher_; 00043 ros::Publisher odom_publisher_; 00044 00045 ros::Subscriber laser_pose_subscriber_; 00046 ros::Subscriber imu_subscriber_; 00047 ros::Subscriber height_subscriber_; 00048 tf::TransformBroadcaster tf_broadcaster_; 00049 00050 // **** parameters 00051 00052 std::string base_frame_; 00053 std::string fixed_frame_; 00054 00055 // **** state variables 00056 00057 boost::mutex pose_mutex_; 00058 00059 PoseStamped pose_; 00060 00061 // **** member functions 00062 00063 void laserPoseCallback(const PoseStamped::ConstPtr& laser_pose_msg); 00064 void imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg); 00065 void heightCallback (const mav_msgs::Height::ConstPtr& height_msg); 00066 00067 void publishPose(); 00068 }; 00069 00070 } // namespace mav 00071 00072 #endif // MAV_ODOM_INTERFACE_ODOM_INTERFACE_H