00001 00034 #ifndef MULTISENSE_ROS_IMU_H 00035 #define MULTISENSE_ROS_IMU_H 00036 00037 #include <boost/shared_ptr.hpp> 00038 #include <boost/thread.hpp> 00039 #include <ros/ros.h> 00040 00041 #include <sensor_msgs/Imu.h> 00042 #include <geometry_msgs/Vector3Stamped.h> 00043 00044 #include <multisense_lib/MultiSenseChannel.hh> 00045 00046 namespace multisense_ros { 00047 00048 class Imu { 00049 public: 00050 00051 Imu(crl::multisense::Channel* driver, std::string tf_prefix); 00052 ~Imu(); 00053 00054 void imuCallback(const crl::multisense::imu::Header& header); 00055 00056 private: 00057 00058 // 00059 // CRL sensor API 00060 00061 crl::multisense::Channel* driver_; 00062 00063 // 00064 // Driver nodes 00065 00066 ros::NodeHandle device_nh_; 00067 ros::NodeHandle imu_nh_; 00068 00069 // 00070 // multisense_ros/RawImuData publishers 00071 00072 ros::Publisher accelerometer_pub_; 00073 ros::Publisher gyroscope_pub_; 00074 ros::Publisher magnetometer_pub_; 00075 00076 // 00077 // sensor_msgs/Imu publisher 00078 ros::Publisher imu_pub_; 00079 00080 // 00081 // geometry_msgs/Vector3Stamped publishers 00082 ros::Publisher accelerometer_vector_pub_; 00083 ros::Publisher gyroscope_vector_pub_; 00084 ros::Publisher magnetometer_vector_pub_; 00085 00086 // 00087 // IMU message 00088 sensor_msgs::Imu imu_message_; 00089 00090 // 00091 // Publish control 00092 00093 boost::mutex sub_lock_; 00094 int32_t total_subscribers_; 00095 void startStreams(); 00096 void stopStreams(); 00097 00098 // 00099 // TF prefix and frame ID's 00100 const std::string tf_prefix_; 00101 const std::string accel_frameId_; 00102 const std::string gyro_frameId_; 00103 const std::string mag_frameId_; 00104 00105 }; 00106 00107 } 00108 00109 #endif