00001 #ifndef PHIDGETS_IMU_IMU_ROS_I_H 00002 #define PHIDGETS_IMU_IMU_ROS_I_H 00003 00004 #include <ros/ros.h> 00005 #include <ros/service_server.h> 00006 #include <boost/thread/mutex.hpp> 00007 #include <tf/transform_datatypes.h> 00008 #include <sensor_msgs/Imu.h> 00009 #include <std_srvs/Empty.h> 00010 #include <std_msgs/Bool.h> 00011 #include <geometry_msgs/Vector3Stamped.h> 00012 #include <phidgets_api/imu.h> 00013 00014 namespace phidgets { 00015 00016 const float G = 9.81; 00017 00018 class ImuRosI : public Imu 00019 { 00020 typedef sensor_msgs::Imu ImuMsg; 00021 typedef geometry_msgs::Vector3Stamped MagMsg; 00022 00023 public: 00024 00025 ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private); 00026 00027 bool calibrateService(std_srvs::Empty::Request &req, 00028 std_srvs::Empty::Response &res); 00029 00030 private: 00031 00032 ros::NodeHandle nh_; 00033 ros::NodeHandle nh_private_; 00034 ros::Publisher imu_publisher_; 00035 ros::Publisher mag_publisher_; 00036 ros::Publisher cal_publisher_; 00037 ros::ServiceServer cal_srv_; 00038 00039 bool initialized_; 00040 boost::mutex mutex_; 00041 ros::Time last_imu_time_; 00042 00043 ImuMsg imu_msg_; 00044 00045 ros::Time time_zero_; 00046 00047 // params 00048 00049 std::string frame_id_; 00050 int period_; // rate in ms 00051 00052 double angular_velocity_stdev_; 00053 double linear_acceleration_stdev_; 00054 00055 // compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) 00056 double cc_mag_field_; 00057 double cc_offset0_; 00058 double cc_offset1_; 00059 double cc_offset2_; 00060 double cc_gain0_; 00061 double cc_gain1_; 00062 double cc_gain2_; 00063 double cc_T0_; 00064 double cc_T1_; 00065 double cc_T2_; 00066 double cc_T3_; 00067 double cc_T4_; 00068 double cc_T5_; 00069 00070 void calibrate(); 00071 void initDevice(); 00072 void dataHandler(CPhidgetSpatial_SpatialEventDataHandle* data, int count); 00073 void processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i); 00074 }; 00075 00076 } //namespace phidgets 00077 00078 #endif // PHIDGETS_IMU_IMU_ROS_I_H