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 void calibrate(); 00056 void initDevice(); 00057 void dataHandler(CPhidgetSpatial_SpatialEventDataHandle* data, int count); 00058 void processImuData(CPhidgetSpatial_SpatialEventDataHandle* data, int i); 00059 }; 00060 00061 } //namespace phidgets 00062 00063 #endif // PHIDGETS_IMU_IMU_ROS_I_H