imu.h
Go to the documentation of this file.
00001 
00034 #ifndef MULTISENSE_ROS_IMU_H
00035 #define MULTISENSE_ROS_IMU_H
00036 
00037 #include <boost/shared_ptr.hpp>
00038 #include <boost/thread.hpp>
00039 #include <ros/ros.h>
00040 
00041 #include <sensor_msgs/Imu.h>
00042 #include <geometry_msgs/Vector3Stamped.h>
00043 
00044 #include <multisense_lib/MultiSenseChannel.hh>
00045 
00046 namespace multisense_ros {
00047 
00048 class Imu {
00049 public:
00050 
00051     Imu(crl::multisense::Channel* driver, std::string tf_prefix);
00052     ~Imu();
00053 
00054     void imuCallback(const crl::multisense::imu::Header& header);
00055 
00056 private:
00057 
00058     //
00059     // CRL sensor API
00060 
00061     crl::multisense::Channel* driver_;
00062 
00063     //
00064     // Driver nodes
00065 
00066     ros::NodeHandle device_nh_;
00067     ros::NodeHandle imu_nh_;
00068 
00069     //
00070     // multisense_ros/RawImuData publishers
00071 
00072     ros::Publisher accelerometer_pub_;
00073     ros::Publisher gyroscope_pub_;
00074     ros::Publisher magnetometer_pub_;
00075 
00076     //
00077     // sensor_msgs/Imu publisher
00078     ros::Publisher imu_pub_;
00079 
00080     //
00081     // geometry_msgs/Vector3Stamped publishers
00082     ros::Publisher accelerometer_vector_pub_;
00083     ros::Publisher gyroscope_vector_pub_;
00084     ros::Publisher magnetometer_vector_pub_;
00085 
00086     //
00087     // IMU message
00088     sensor_msgs::Imu imu_message_;
00089 
00090     //
00091     // Publish control
00092 
00093     boost::mutex sub_lock_;
00094     int32_t total_subscribers_;
00095     void startStreams();
00096     void stopStreams();
00097 
00098     //
00099     // TF prefix and frame ID's
00100     const std::string tf_prefix_;
00101     const std::string accel_frameId_;
00102     const std::string gyro_frameId_;
00103     const std::string mag_frameId_;
00104 
00105 };
00106 
00107 }
00108 
00109 #endif


multisense_ros
Author(s):
autogenerated on Fri Apr 5 2019 02:28:29