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
gazebo::GazeboRosIMU::aeul_
ignition::math::Vector3d aeul_
Definition: gazebo_ros_imu.h:89
gazebo::GazeboRosIMU::service_name_
std::string service_name_
Definition: gazebo_ros_imu.h:111
gazebo::GazeboRosIMU::rosnode_
ros::NodeHandle * rosnode_
pointer to ros node
Definition: gazebo_ros_imu.h:61
ros::Publisher
gazebo::GazeboRosIMU::offset_
ignition::math::Pose3d offset_
allow specifying constant xyz and rpy offsets
Definition: gazebo_ros_imu.h:78
gazebo::GazeboRosIMU::update_connection_
event::ConnectionPtr update_connection_
Definition: gazebo_ros_imu.h:118
gazebo
gazebo::GazeboRosIMU::world_
physics::WorldPtr world_
The parent World.
Definition: gazebo_ros_imu.h:55
gazebo::GazeboRosIMU::link
physics::LinkPtr link
The link referred to by this plugin.
Definition: gazebo_ros_imu.h:58
gazebo::GazeboRosIMU::callback_queue_thread_
boost::thread callback_queue_thread_
Definition: gazebo_ros_imu.h:115
gazebo::GazeboRosIMU::robot_namespace_
std::string robot_namespace_
for setting ROS name space
Definition: gazebo_ros_imu.h:104
ros.h
gazebo::GazeboRosIMU::GaussianKernel
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
Definition: gazebo_ros_imu.cpp:352
gazebo::GazeboRosIMU
Definition: gazebo_ros_imu.h:39
gazebo::GazeboRosIMU::GazeboRosIMU
GazeboRosIMU()
Constructor.
Definition: gazebo_ros_imu.cpp:36
ros::ServiceServer
gazebo::GazeboRosIMU::imu_queue_
ros::CallbackQueue imu_queue_
Definition: gazebo_ros_imu.h:113
gazebo::GazeboRosIMU::imu_msg_
sensor_msgs::Imu imu_msg_
ros message
Definition: gazebo_ros_imu.h:66
gazebo::GazeboRosIMU::pmq
PubMultiQueue pmq
Definition: gazebo_ros_imu.h:126
gazebo::GazeboRosIMU::last_veul_
ignition::math::Vector3d last_veul_
Definition: gazebo_ros_imu.h:87
gazebo::GazeboRosIMU::pub_Queue
PubQueue< sensor_msgs::Imu >::Ptr pub_Queue
Definition: gazebo_ros_imu.h:63
ros::CallbackQueue
gazebo::GazeboRosIMU::sdf
sdf::ElementPtr sdf
Definition: gazebo_ros_imu.h:121
gazebo::GazeboRosIMU::IMUQueueThread
void IMUQueueThread()
Definition: gazebo_ros_imu.cpp:374
gazebo::GazeboRosIMU::ServiceCallback
bool ServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
call back when using service
Definition: gazebo_ros_imu.cpp:212
gazebo::GazeboRosIMU::lock_
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
Definition: gazebo_ros_imu.h:82
gazebo::GazeboRosIMU::initial_pose_
ignition::math::Pose3d initial_pose_
: keep initial pose to offset orientation in imu message
Definition: gazebo_ros_imu.h:95
gazebo::GazeboRosIMU::LoadThread
void LoadThread()
Definition: gazebo_ros_imu.cpp:66
gazebo::GazeboRosIMU::pub_
ros::Publisher pub_
Definition: gazebo_ros_imu.h:62
callback_queue.h
advertise_options.h
gazebo::GazeboRosIMU::gaussian_noise_
double gaussian_noise_
Gaussian noise.
Definition: gazebo_ros_imu.h:98
gazebo::GazeboRosIMU::link_name_
std::string link_name_
store link name
Definition: gazebo_ros_imu.h:69
gazebo::GazeboRosIMU::Load
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
Definition: gazebo_ros_imu.cpp:53
PubMultiQueue
A collection of PubQueue objects, potentially of different types. This class is the programmer's inte...
Definition: PubQueue.h:96
gazebo::GazeboRosIMU::last_vpos_
ignition::math::Vector3d last_vpos_
Definition: gazebo_ros_imu.h:86
PubQueue.h
gazebo::GazeboRosIMU::last_time_
common::Time last_time_
save last_time
Definition: gazebo_ros_imu.h:85
gazebo::GazeboRosIMU::srv_
ros::ServiceServer srv_
Definition: gazebo_ros_imu.h:110
PubQueue
A queue of outgoing messages. Instead of calling publish() directly, you can push() messages here to ...
Definition: PubQueue.h:48
gazebo::GazeboRosIMU::update_rate_
double update_rate_
Definition: gazebo_ros_imu.h:92
gazebo::GazeboRosIMU::~GazeboRosIMU
virtual ~GazeboRosIMU()
Destructor.
Definition: gazebo_ros_imu.cpp:42
gazebo::GazeboRosIMU::frame_name_
std::string frame_name_
store frame name
Definition: gazebo_ros_imu.h:72
ros::NodeHandle
gazebo::GazeboRosIMU::deferred_load_thread_
boost::thread deferred_load_thread_
Definition: gazebo_ros_imu.h:123
gazebo::GazeboRosIMU::apos_
ignition::math::Vector3d apos_
Definition: gazebo_ros_imu.h:88
gazebo::GazeboRosIMU::UpdateChild
virtual void UpdateChild()
Update the controller.
Definition: gazebo_ros_imu.cpp:220
gazebo::GazeboRosIMU::topic_name_
std::string topic_name_
topic name
Definition: gazebo_ros_imu.h:75


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Nov 23 2023 03:50:28