17 #ifndef GAZEBO_ROS_IMU_HH 18 #define GAZEBO_ROS_IMU_HH 21 #include <boost/thread.hpp> 22 #include <boost/thread/mutex.hpp> 23 #include <boost/bind.hpp> 28 #include <sensor_msgs/Imu.h> 29 #include <std_srvs/Empty.h> 31 #include <gazebo/physics/physics.hh> 32 #include <gazebo/transport/transport.hh> 33 #include <gazebo/common/common.hh> 49 public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
58 private: physics::LinkPtr
link;
88 private: ignition::math::Vector3d
apos_;
89 private: ignition::math::Vector3d
aeul_;
108 std_srvs::Empty::Response &res);
121 private: sdf::ElementPtr
sdf;
sensor_msgs::Imu imu_msg_
ros message
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
ignition::math::Vector3d last_veul_
common::Time last_time_
save last_time
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's inte...
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 ...
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