gazebo_ros_imu.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
30 #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
31 
32 // #define USE_CBQ
33 #ifdef USE_CBQ
34 #include <ros/callback_queue.h>
35 #include <ros/advertise_options.h>
36 #endif
37 
38 #include <gazebo/common/Plugin.hh>
39 
40 #include <ros/ros.h>
41 #include <boost/thread/mutex.hpp>
42 #include <sensor_msgs/Imu.h>
43 #include <std_srvs/Empty.h>
44 #include <hector_gazebo_plugins/SetBias.h>
47 
48 #include <dynamic_reconfigure/server.h>
49 
50 namespace gazebo
51 {
52  class GazeboRosIMU : public ModelPlugin
53  {
54  public:
56  GazeboRosIMU();
57 
59  virtual ~GazeboRosIMU();
60 
61  protected:
62  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
63  virtual void Reset();
64  virtual void Update();
65 
66  private:
68  physics::WorldPtr world;
69 
71  physics::LinkPtr link;
72 
77 
79  sensor_msgs::Imu imuMsg;
80  sensor_msgs::Imu biasMsg;
81 
83  std::string link_name_;
84 
86  std::string frame_id_;
87 
89  std::string topic_;
90  std::string bias_topic_;
91 
93 #if (GAZEBO_MAJOR_VERSION >= 8)
94  ignition::math::Pose3d offset_;
95 #else
96  math::Pose offset_;
97 #endif
98 
103 
105  boost::mutex lock;
106 
108 #if (GAZEBO_MAJOR_VERSION >= 8)
109  ignition::math::Quaterniond orientation;
110  ignition::math::Vector3d velocity;
111  ignition::math::Vector3d accel;
112  ignition::math::Vector3d rate;
113  ignition::math::Vector3d gravity;
114 #else
115  math::Quaternion orientation;
116  math::Vector3 velocity;
117  math::Vector3 accel;
118  math::Vector3 rate;
119  math::Vector3 gravity;
120 #endif
121 
123  double GaussianKernel(double mu,double sigma);
124 
126  std::string namespace_;
127 
129  bool ServiceCallback(std_srvs::Empty::Request &req,
130  std_srvs::Empty::Response &res);
132  std::string serviceName;
133 
135  bool SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
136  bool SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res);
139 
140 #ifdef USE_CBQ
141  ros::CallbackQueue callback_queue_;
142  void CallbackQueueThread();
143  boost::thread callback_queue_thread_;
144 #endif
145 
147  event::ConnectionPtr updateConnection;
148 
150  };
151 }
152 
153 #endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H
ros::ServiceServer accelBiasService
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_yaw_
std::string namespace_
for setting ROS name space
ros::Publisher pub_
ros::ServiceServer srv_
bool SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
Bias service callbacks.
physics::WorldPtr world
The parent World.
boost::mutex lock
A mutex to lock access to fields that are used in message callbacks.
SensorModel3 rateModel
std::string bias_topic_
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_rate_
std::string frame_id_
frame id
std::string link_name_
store link name
math::Vector3 velocity
sensor_msgs::Imu biasMsg
std::string topic_
topic name
event::ConnectionPtr updateConnection
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
ros::NodeHandle * node_handle_
pointer to ros node
virtual ~GazeboRosIMU()
Destructor.
sensor_msgs::Imu imuMsg
ros message
ros::Publisher bias_pub_
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_accel_
GazeboRosIMU()
Constructor.
bool SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
math::Pose offset_
allow specifying constant xyz and rpy offsets
SensorModel3 accelModel
Sensor models.
physics::LinkPtr link
The link referred to by this plugin.
math::Quaternion orientation
save current body/physics state
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
ros::ServiceServer rateBiasService
bool ServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
call back when using service
math::Vector3 gravity


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30