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 // This code is based on the original gazebo_ros_imu plugin by Sachin Chitta and John Hsu:
00029 /*
00030  *  Gazebo - Outdoor Multi-Robot Simulator
00031  *  Copyright (C) 2003
00032  *     Nate Koenig & Andrew Howard
00033  *
00034  *  This program is free software; you can redistribute it and/or modify
00035  *  it under the terms of the GNU General Public License as published by
00036  *  the Free Software Foundation; either version 2 of the License, or
00037  *  (at your option) any later version.
00038  *
00039  *  This program is distributed in the hope that it will be useful,
00040  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00041  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00042  *  GNU General Public License for more details.
00043  *
00044  *  You should have received a copy of the GNU General Public License
00045  *  along with this program; if not, write to the Free Software
00046  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00047  *
00048  */
00049 /*
00050  * Desc: 3D position interface.
00051  * Author: Sachin Chitta and John Hsu
00052  * Date: 10 June 2008
00053  * SVN: $Id$
00054  */
00055 //=================================================================================================
00056 
00057 
00058 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
00059 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
00060 
00061 // #define USE_CBQ
00062 #ifdef USE_CBQ
00063 #include <ros/callback_queue.h>
00064 #include <ros/advertise_options.h>
00065 #endif
00066 
00067 #include <gazebo/common/Plugin.hh>
00068 
00069 #include <ros/ros.h>
00070 #include <boost/thread/mutex.hpp>
00071 #include <sensor_msgs/Imu.h>
00072 #include <std_srvs/Empty.h>
00073 #include <hector_gazebo_plugins/SetBias.h>
00074 #include <hector_gazebo_plugins/sensor_model.h>
00075 #include <hector_gazebo_plugins/update_timer.h>
00076 
00077 namespace gazebo
00078 {
00079    class GazeboRosIMU : public ModelPlugin
00080    {
00081    public:
00083       GazeboRosIMU();
00084 
00086       virtual ~GazeboRosIMU();
00087 
00088    protected:
00089       virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00090       virtual void Reset();
00091       virtual void Update();
00092 
00093    private:
00095       physics::WorldPtr world;
00096 
00098       physics::LinkPtr link;
00099 
00101       ros::NodeHandle* node_handle_;
00102       ros::Publisher pub_;
00103 
00105       sensor_msgs::Imu imuMsg;
00106 
00108       std::string linkName;
00109 
00111       std::string frameId;
00112 
00114       std::string topicName;
00115 
00117       SensorModel3 accelModel;
00118       SensorModel3 rateModel;
00119       SensorModel headingModel;
00120 
00122       boost::mutex lock;
00123 
00125       math::Quaternion orientation;
00126       math::Vector3 velocity;
00127       math::Vector3 accel;
00128       math::Vector3 rate;
00129       math::Vector3 gravity;
00130       math::Vector3 gravity_body;
00131 
00133       double GaussianKernel(double mu,double sigma);
00134 
00136       std::string robotNamespace;
00137 
00139       bool ServiceCallback(std_srvs::Empty::Request &req,
00140                                     std_srvs::Empty::Response &res);
00141       ros::ServiceServer srv_;
00142       std::string serviceName;
00143 
00145       bool SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
00146       bool SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
00147       ros::ServiceServer accelBiasService;
00148       ros::ServiceServer rateBiasService;
00149 
00150 #ifdef USE_CBQ
00151       ros::CallbackQueue callback_queue_;
00152       void CallbackQueueThread();
00153       boost::thread callback_queue_thread_;
00154 #endif
00155 
00156       UpdateTimer updateTimer;
00157       event::ConnectionPtr updateConnection;
00158    };
00159 }
00160 
00161 #endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30