gazebo_ros_imu.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
00030 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
00031 
00032 // #define USE_CBQ
00033 #ifdef USE_CBQ
00034 #include <ros/callback_queue.h>
00035 #include <ros/advertise_options.h>
00036 #endif
00037 
00038 #include <gazebo/common/Plugin.hh>
00039 
00040 #include <ros/ros.h>
00041 #include <boost/thread/mutex.hpp>
00042 #include <sensor_msgs/Imu.h>
00043 #include <std_srvs/Empty.h>
00044 #include <hector_gazebo_plugins/SetBias.h>
00045 #include <hector_gazebo_plugins/sensor_model.h>
00046 #include <hector_gazebo_plugins/update_timer.h>
00047 
00048 #include <dynamic_reconfigure/server.h>
00049 
00050 namespace gazebo
00051 {
00052    class GazeboRosIMU : public ModelPlugin
00053    {
00054    public:
00056       GazeboRosIMU();
00057 
00059       virtual ~GazeboRosIMU();
00060 
00061    protected:
00062       virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00063       virtual void Reset();
00064       virtual void Update();
00065 
00066    private:
00068       physics::WorldPtr world;
00069 
00071       physics::LinkPtr link;
00072 
00074       ros::NodeHandle* node_handle_;
00075       ros::Publisher pub_;
00076       ros::Publisher bias_pub_;
00077 
00079       sensor_msgs::Imu imuMsg;
00080       sensor_msgs::Imu biasMsg;
00081 
00083       std::string link_name_;
00084 
00086       std::string frame_id_;
00087 
00089       std::string topic_;
00090       std::string bias_topic_;
00091 
00093       math::Pose offset_;
00094 
00096       SensorModel3 accelModel;
00097       SensorModel3 rateModel;
00098       SensorModel yawModel;
00099 
00101       boost::mutex lock;
00102 
00104       math::Quaternion orientation;
00105       math::Vector3 velocity;
00106       math::Vector3 accel;
00107       math::Vector3 rate;
00108       math::Vector3 gravity;
00109 
00111       double GaussianKernel(double mu,double sigma);
00112 
00114       std::string namespace_;
00115 
00117       bool ServiceCallback(std_srvs::Empty::Request &req,
00118                                     std_srvs::Empty::Response &res);
00119       ros::ServiceServer srv_;
00120       std::string serviceName;
00121 
00123       bool SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
00124       bool SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
00125       ros::ServiceServer accelBiasService;
00126       ros::ServiceServer rateBiasService;
00127 
00128 #ifdef USE_CBQ
00129       ros::CallbackQueue callback_queue_;
00130       void CallbackQueueThread();
00131       boost::thread callback_queue_thread_;
00132 #endif
00133 
00134       UpdateTimer updateTimer;
00135       event::ConnectionPtr updateConnection;
00136 
00137       boost::shared_ptr<dynamic_reconfigure::Server<SensorModelConfig> > dynamic_reconfigure_server_accel_, dynamic_reconfigure_server_rate_, dynamic_reconfigure_server_yaw_;
00138    };
00139 }
00140 
00141 #endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Mon Jun 27 2016 04:58:09