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 <boost/shared_ptr.hpp> 00008 #include <tf/transform_datatypes.h> 00009 #include <sensor_msgs/Imu.h> 00010 #include <std_srvs/Empty.h> 00011 #include <std_msgs/Bool.h> 00012 #include <geometry_msgs/Vector3Stamped.h> 00013 #include <diagnostic_updater/diagnostic_updater.h> 00014 #include <diagnostic_updater/publisher.h> 00015 #include <phidgets_api/imu.h> 00016 00017 using namespace std; 00018 00019 namespace phidgets { 00020 00021 const float G = 9.81; 00022 00023 class ImuRosI : public Imu 00024 { 00025 typedef sensor_msgs::Imu ImuMsg; 00026 typedef geometry_msgs::Vector3Stamped MagMsg; 00027 00028 public: 00029 00030 ImuRosI(ros::NodeHandle nh, ros::NodeHandle nh_private); 00031 00032 bool calibrateService(std_srvs::Empty::Request &req, 00033 std_srvs::Empty::Response &res); 00034 00035 private: 00036 00037 ros::NodeHandle nh_; 00038 ros::NodeHandle nh_private_; 00039 ros::Publisher imu_publisher_; 00040 ros::Publisher mag_publisher_; 00041 ros::Publisher cal_publisher_; 00042 ros::ServiceServer cal_srv_; 00043 00046 diagnostic_updater::Updater diag_updater_; 00047 boost::shared_ptr<diagnostic_updater::TopicDiagnostic> imu_publisher_diag_ptr_; 00048 00049 // diagnostics 00050 bool is_connected_; 00051 int error_number_; 00052 double target_publish_freq_; 00053 00054 bool initialized_; 00055 boost::mutex mutex_; 00056 ros::Time last_imu_time_; 00057 00058 ImuMsg imu_msg_; 00059 00060 ros::Time time_zero_; 00061 00062 // params 00063 00064 std::string frame_id_; 00065 int period_; // rate in ms 00066 00067 double angular_velocity_stdev_; 00068 double linear_acceleration_stdev_; 00069 00070 // compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) 00071 double cc_mag_field_; 00072 double cc_offset0_; 00073 double cc_offset1_; 00074 double cc_offset2_; 00075 double cc_gain0_; 00076 double cc_gain1_; 00077 double cc_gain2_; 00078 double cc_T0_; 00079 double cc_T1_; 00080 double cc_T2_; 00081 double cc_T3_; 00082 double cc_T4_; 00083 double cc_T5_; 00084 00085 void calibrate(); 00086 void initDevice(); 00087 void dataHandler(CPhidgetSpatial_SpatialEventDataHandle* data, int count); 00088 void attachHandler(); 00089 void detachHandler(); 00090 void errorHandler(int error); 00091 void processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i); 00092 00097 void phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00098 }; 00099 00100 } //namespace phidgets 00101 00102 #endif // PHIDGETS_IMU_IMU_ROS_I_H