gazebo_ros_imu.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_ROS_IMU_HH
18 #define GAZEBO_ROS_IMU_HH
19 
20 #include <string>
21 #include <boost/thread.hpp>
22 #include <boost/thread/mutex.hpp>
23 #include <boost/bind.hpp>
24 
25 #include <ros/ros.h>
26 #include <ros/callback_queue.h>
27 #include <ros/advertise_options.h>
28 #include <sensor_msgs/Imu.h>
29 #include <std_srvs/Empty.h>
30 
31 #include <gazebo/physics/physics.hh>
32 #include <gazebo/transport/transport.hh>
33 #include <gazebo/common/common.hh>
34 
36 
37 namespace gazebo
38 {
39  class GazeboRosIMU : public ModelPlugin
40  {
42  public: GazeboRosIMU();
43 
45  public: virtual ~GazeboRosIMU();
46 
49  public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
50 
52  protected: virtual void UpdateChild();
53 
55  private: physics::WorldPtr world_;
56 
58  private: physics::LinkPtr link;
59 
62  private: ros::Publisher pub_;
64 
66  private: sensor_msgs::Imu imu_msg_;
67 
69  private: std::string link_name_;
70 
72  private: std::string frame_name_;
73 
75  private: std::string topic_name_;
76 
78  private: ignition::math::Pose3d offset_;
79 
82  private: boost::mutex lock_;
83 
85  private: common::Time last_time_;
86  private: ignition::math::Vector3d last_vpos_;
87  private: ignition::math::Vector3d last_veul_;
88  private: ignition::math::Vector3d apos_;
89  private: ignition::math::Vector3d aeul_;
90 
91  // rate control
92  private: double update_rate_;
93 
95  private: ignition::math::Pose3d initial_pose_;
96 
98  private: double gaussian_noise_;
99 
101  private: double GaussianKernel(double mu, double sigma);
102 
104  private: std::string robot_namespace_;
105 
107  private: bool ServiceCallback(std_srvs::Empty::Request &req,
108  std_srvs::Empty::Response &res);
109 
111  private: std::string service_name_;
112 
114  private: void IMUQueueThread();
115  private: boost::thread callback_queue_thread_;
116 
117  // Pointer to the update event connection
118  private: event::ConnectionPtr update_connection_;
119 
120  // deferred load in case ros is blocking
121  private: sdf::ElementPtr sdf;
122  private: void LoadThread();
123  private: boost::thread deferred_load_thread_;
124 
125  // ros publish multi queue, prevents publish() blocking
126  private: PubMultiQueue pmq;
127  };
128 }
129 #endif
sensor_msgs::Imu imu_msg_
ros message
sdf::ElementPtr sdf
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
ignition::math::Vector3d last_veul_
ros::Publisher pub_
common::Time last_time_
save last_time
ros::ServiceServer srv_
double gaussian_noise_
Gaussian noise.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
A collection of PubQueue objects, potentially of different types. This class is the programmer&#39;s inte...
Definition: PubQueue.h:96
std::string frame_name_
store frame name
ignition::math::Vector3d last_vpos_
ignition::math::Vector3d apos_
A queue of outgoing messages. Instead of calling publish() directly, you can push() messages here to ...
Definition: PubQueue.h:48
std::string link_name_
store link name
boost::thread deferred_load_thread_
boost::thread callback_queue_thread_
std::string topic_name_
topic name
virtual void UpdateChild()
Update the controller.
virtual ~GazeboRosIMU()
Destructor.
physics::WorldPtr world_
The parent World.
ignition::math::Vector3d aeul_
ros::NodeHandle * rosnode_
pointer to ros node
event::ConnectionPtr update_connection_
std::string service_name_
std::string robot_namespace_
for setting ROS name space
GazeboRosIMU()
Constructor.
ros::CallbackQueue imu_queue_
ignition::math::Pose3d offset_
allow specifying constant xyz and rpy offsets
physics::LinkPtr link
The link referred to by this plugin.
PubQueue< sensor_msgs::Imu >::Ptr pub_Queue
ignition::math::Pose3d initial_pose_
: keep initial pose to offset orientation in imu message
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
bool ServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
call back when using service


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52