gazebo_ros_imu.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2012 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 #ifndef GAZEBO_ROS_IMU_HH
00018 #define GAZEBO_ROS_IMU_HH
00019 
00020 #include <string>
00021 #include <boost/thread.hpp>
00022 #include <boost/thread/mutex.hpp>
00023 #include <boost/bind.hpp>
00024 
00025 #include <ros/ros.h>
00026 #include <ros/callback_queue.h>
00027 #include <ros/advertise_options.h>
00028 #include <sensor_msgs/Imu.h>
00029 #include <std_srvs/Empty.h>
00030 
00031 #include <gazebo/physics/physics.hh>
00032 #include <gazebo/transport/transport.hh>
00033 #include <gazebo/common/common.hh>
00034 
00035 #include <gazebo_plugins/PubQueue.h>
00036 
00037 namespace gazebo
00038 {
00039   class GazeboRosIMU : public ModelPlugin
00040   {
00042     public: GazeboRosIMU();
00043 
00045     public: virtual ~GazeboRosIMU();
00046 
00049     public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00050 
00052     protected: virtual void UpdateChild();
00053 
00055     private: physics::WorldPtr world_;
00056 
00058     private: physics::LinkPtr link;
00059 
00061     private: ros::NodeHandle* rosnode_;
00062     private: ros::Publisher pub_;
00063     private: PubQueue<sensor_msgs::Imu>::Ptr pub_Queue;
00064 
00066     private: sensor_msgs::Imu imu_msg_;
00067 
00069     private: std::string link_name_;
00070 
00072     private: std::string frame_name_;
00073 
00075     private: std::string topic_name_;
00076 
00078     private: math::Pose offset_;
00079 
00082     private: boost::mutex lock_;
00083 
00085     private: common::Time last_time_;
00086     private: math::Vector3 last_vpos_;
00087     private: math::Vector3 last_veul_;
00088     private: math::Vector3 apos_;
00089     private: math::Vector3 aeul_;
00090     
00091     // rate control
00092     private: double update_rate_;
00093 
00095     private: math::Pose initial_pose_;
00096 
00098     private: double gaussian_noise_;
00099 
00101     private: double GaussianKernel(double mu, double sigma);
00102 
00104     private: std::string robot_namespace_;
00105 
00107     private: bool ServiceCallback(std_srvs::Empty::Request &req,
00108                                   std_srvs::Empty::Response &res);
00109 
00110     private: ros::ServiceServer srv_;
00111     private: std::string service_name_;
00112 
00113     private: ros::CallbackQueue imu_queue_;
00114     private: void IMUQueueThread();
00115     private: boost::thread callback_queue_thread_;
00116 
00117     // Pointer to the update event connection
00118     private: event::ConnectionPtr update_connection_;
00119 
00120     // deferred load in case ros is blocking
00121     private: sdf::ElementPtr sdf;
00122     private: void LoadThread();
00123     private: boost::thread deferred_load_thread_;
00124     private: unsigned int seed;
00125 
00126     // ros publish multi queue, prevents publish() blocking
00127     private: PubMultiQueue pmq;
00128   };
00129 }
00130 #endif


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09