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 <sensor_msgs/MagneticField.h> 00011 #include <std_srvs/Empty.h> 00012 #include <std_msgs/Bool.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 sensor_msgs::MagneticField 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 int serial_number_; 00058 00059 ImuMsg imu_msg_; 00060 MagMsg mag_msg_; 00061 00062 ros::Time time_zero_; 00063 00064 // params 00065 00066 std::string frame_id_; 00067 int period_; // rate in ms 00068 00069 double angular_velocity_stdev_; 00070 double linear_acceleration_stdev_; 00071 double magnetic_field_stdev_; 00072 00073 // compass correction params (see http://www.phidgets.com/docs/1044_User_Guide) 00074 double cc_mag_field_; 00075 double cc_offset0_; 00076 double cc_offset1_; 00077 double cc_offset2_; 00078 double cc_gain0_; 00079 double cc_gain1_; 00080 double cc_gain2_; 00081 double cc_T0_; 00082 double cc_T1_; 00083 double cc_T2_; 00084 double cc_T3_; 00085 double cc_T4_; 00086 double cc_T5_; 00087 00088 void calibrate(); 00089 void initDevice(); 00090 void dataHandler(CPhidgetSpatial_SpatialEventDataHandle* data, int count); 00091 void attachHandler(); 00092 void detachHandler(); 00093 void errorHandler(int error); 00094 void processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i); 00095 00100 void phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00101 }; 00102 00103 } //namespace phidgets 00104 00105 #endif // PHIDGETS_IMU_IMU_ROS_I_H