00001 // 00002 // Created by tom on 15/05/16. 00003 // 00004 00005 #ifndef ROBOTICAN_HARDWARE_INTERFACE_IMU_H 00006 #define ROBOTICAN_HARDWARE_INTERFACE_IMU_H 00007 00008 00009 #define DEGs2RADs 0.0174532925 00010 00011 #define G2ms 9.80665 00012 00013 #define MILLI_GAUSS_2_TESLAS 1.0e-7 00014 00015 #include <sensor_msgs/Imu.h> 00016 #include <sensor_msgs/MagneticField.h> 00017 #include <robotican_hardware_interface/Device.h> 00018 #include <robotican_hardware_interface/imuClib.h> 00019 #include <robotican_hardware_interface/ros_utils.h> 00020 #include <robotican_hardware_interface/setImuClib.h> 00021 #include <robotican_hardware_interface/RiCBoardManager.h> 00022 #include <tf/transform_broadcaster.h> 00023 00024 namespace robotican_hardware { 00025 class Imu : public Device { 00026 private: 00027 byte _imuState; 00028 uint16_t _fusionHz; 00029 std::string _frameId; 00030 ros::ServiceServer _setImuClibService; 00031 ros::Publisher _clibPub; 00032 ros::Publisher _imuAMQ; 00033 ros::Publisher _imuM; 00034 tf::TransformBroadcaster _broadcaster; 00035 bool _enableGyro; 00036 bool _fuseCompass; 00037 bool _isStopClib; 00038 00039 double _imuLinearAccFix[3][3]; 00040 double _imuAngularVelocityFix[3][3]; 00041 double _imuMagnetometerFix[3][3]; 00042 double _imuRotationFix[3][3]; 00043 double _imuRotationOffset[3]; 00044 00045 bool _isStateChange; 00046 bool onSetImuClib(robotican_hardware_interface::setImuClib::Request &request, robotican_hardware_interface::setImuClib::Response &response); 00047 00048 void initArrays(); 00049 00050 public: 00051 00052 Imu(byte id, TransportLayer *transportLayer, uint16_t fusionHz, std::string frameId, bool enableGyro, 00053 bool fuseCompass, std::vector<double> imuLinearAccFix, std::vector<double> imuAngularVelocityFix, 00054 std::vector<double> imuMagnetometerFix, std::vector<double> imuRotationFix, 00055 std::vector<double> imuRotationOffset); 00056 00057 virtual void update(const DeviceMessage *deviceMessage); 00058 00059 virtual void write(); 00060 00061 virtual void buildDevice(); 00062 00063 virtual void deviceAck(const DeviceAck *ack); 00064 }; 00065 } 00066 00067 #endif //ROBOTICAN_HARDWARE_INTERFACE_IMU_H