gazebo_ros_imu.cpp
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 
30 #include <gazebo/common/Events.hh>
31 #include <gazebo/physics/physics.hh>
32 
33 #include <ros/console.h>
34 
35 namespace gazebo
36 {
37 
38 // #define DEBUG_OUTPUT
39 #ifdef DEBUG_OUTPUT
40  #include <geometry_msgs/PoseStamped.h>
41  static ros::Publisher debugPublisher;
42 #endif // DEBUG_OUTPUT
43 
45 // Constructor
47 {
48 }
49 
51 // Destructor
53 {
55 
59 
61 #ifdef USE_CBQ
62  callback_queue_thread_.join();
63 #endif
64  delete node_handle_;
65 }
66 
68 // Load the controller
69 void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
70 {
71  // Get the world name.
72  world = _model->GetWorld();
73 
74  // load parameters
75  if (_sdf->HasElement("robotNamespace"))
76  namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
77  else
78  namespace_.clear();
79 
80  if (_sdf->HasElement("bodyName"))
81  {
82  link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
83  link = _model->GetLink(link_name_);
84  }
85  else
86  {
87  link = _model->GetLink();
88  link_name_ = link->GetName();
89  }
90 
91  // assert that the body by link_name_ exists
92  if (!link)
93  {
94  ROS_FATAL("GazeboRosIMU plugin error: bodyName: %s does not exist\n", link_name_.c_str());
95  return;
96  }
97 
98  // default parameters
100  topic_ = "imu";
101 
102  if (_sdf->HasElement("frameId"))
103  frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
104 
105  if (_sdf->HasElement("topicName"))
106  topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString();
107 
108  if (_sdf->HasElement("biasTopicName"))
109  bias_topic_ = _sdf->GetElement("biasTopicName")->GetValue()->GetAsString();
110  else
111  bias_topic_ = (!topic_.empty() ? topic_ + "/bias" : "");
112 
113  if (_sdf->HasElement("serviceName"))
114  serviceName = _sdf->GetElement("serviceName")->GetValue()->GetAsString();
115  else
116  serviceName = topic_ + "/calibrate";
117 
118  accelModel.Load(_sdf, "accel");
119  rateModel.Load(_sdf, "rate");
120  yawModel.Load(_sdf, "yaw");
121 
122  // also use old configuration variables from gazebo_ros_imu
123  if (_sdf->HasElement("gaussianNoise")) {
124  double gaussianNoise;
125  if (_sdf->GetElement("gaussianNoise")->GetValue()->Get(gaussianNoise) && gaussianNoise != 0.0) {
126  accelModel.gaussian_noise = gaussianNoise;
127  rateModel.gaussian_noise = gaussianNoise;
128  }
129  }
130 
131  if (_sdf->HasElement("xyzOffset")) {
132 #if (GAZEBO_MAJOR_VERSION >= 8)
133  this->offset_.Pos() = _sdf->Get<ignition::math::Vector3d>("xyzOffset");
134 #else
135  this->offset_.pos = _sdf->Get<math::Vector3>("xyzOffset");
136 #endif
137  } else {
138  ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
139 #if (GAZEBO_MAJOR_VERSION >= 8)
140  this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0);
141 #else
142  this->offset_.pos = math::Vector3(0, 0, 0);
143 #endif
144  }
145 
146  if (_sdf->HasElement("rpyOffset")) {
147 #if (GAZEBO_MAJOR_VERSION >= 8)
148  this->offset_.Rot() = _sdf->Get<ignition::math::Quaterniond>("rpyOffset");
149 #else
150  this->offset_.rot = _sdf->Get<math::Vector3>("rpyOffset");
151 #endif
152  } else {
153  ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
154 #if (GAZEBO_MAJOR_VERSION >= 8)
155  this->offset_.Rot() = ignition::math::Quaterniond(0, 0, 0);
156 #else
157  this->offset_.rot = math::Vector3(0, 0, 0);
158 #endif
159  }
160 
161  // fill in constant covariance matrix
162 #if (GAZEBO_MAJOR_VERSION >= 8)
163  imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.X()*rateModel.gaussian_noise.X();
164  imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.Y()*rateModel.gaussian_noise.Y();
165  imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.Z()*rateModel.gaussian_noise.Z();
166  imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.X()*accelModel.gaussian_noise.X();
167  imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.Y()*accelModel.gaussian_noise.Y();
168  imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.Z()*accelModel.gaussian_noise.Z();
169 #else
170  imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.x*rateModel.gaussian_noise.x;
171  imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.y*rateModel.gaussian_noise.y;
172  imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.z*rateModel.gaussian_noise.z;
173  imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x;
174  imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y;
175  imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.z*accelModel.gaussian_noise.z;
176 #endif
177 
178  // Make sure the ROS node for Gazebo has already been initialized
179  if (!ros::isInitialized())
180  {
181  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
182  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package.");
183  return;
184  }
185 
187 
188  // if topic name specified as empty, do not publish (then what is this plugin good for?)
189  if (!topic_.empty())
190  pub_ = node_handle_->advertise<sensor_msgs::Imu>(topic_, 10);
191  if (!bias_topic_.empty())
192  bias_pub_ = node_handle_->advertise<sensor_msgs::Imu>(bias_topic_, 10);
193 
194 #ifdef DEBUG_OUTPUT
195  debugPublisher = rosnode_->advertise<geometry_msgs::PoseStamped>(topic_ + "/pose", 10);
196 #endif // DEBUG_OUTPUT
197 
198  // advertise services for calibration and bias setting
199  if (!serviceName.empty())
201 
204 
205  // setup dynamic_reconfigure servers
206  if (!topic_.empty()) {
207  dynamic_reconfigure_server_accel_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/accel")));
208  dynamic_reconfigure_server_rate_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/rate")));
209  dynamic_reconfigure_server_yaw_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_ + "/yaw")));
213  }
214 
215 #ifdef USE_CBQ
216  // start custom queue for imu
217  callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::CallbackQueueThread,this ) );
218 #endif
219 
220  Reset();
221 
222  // connect Update function
223  updateTimer.Load(world, _sdf);
225 }
226 
228 {
229  updateTimer.Reset();
230 
231 #if (GAZEBO_MAJOR_VERSION >= 8)
232  orientation = ignition::math::Quaterniond();
233 #else
234  orientation = math::Quaternion();
235 #endif
236  velocity = 0.0;
237  accel = 0.0;
238 
239  accelModel.reset();
240  rateModel.reset();
241  yawModel.reset();
242 }
243 
245 // returns true always, imu is always calibrated in sim
246 bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req,
247  std_srvs::Empty::Response &res)
248 {
249  boost::mutex::scoped_lock scoped_lock(lock);
250 #if (GAZEBO_MAJOR_VERSION >= 8)
251  rateModel.reset(ignition::math::Vector3d(0.0, 0.0, 0.0));
252 #else
253  rateModel.reset(math::Vector3(0.0, 0.0, 0.0));
254 #endif
255  return true;
256 }
257 
258 bool GazeboRosIMU::SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
259 {
260  boost::mutex::scoped_lock scoped_lock(lock);
261 #if (GAZEBO_MAJOR_VERSION >= 8)
262  accelModel.reset(ignition::math::Vector3d(req.bias.x, req.bias.y, req.bias.z));
263 #else
264  accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
265 #endif
266  return true;
267 }
268 
269 bool GazeboRosIMU::SetRateBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
270 {
271  boost::mutex::scoped_lock scoped_lock(lock);
272 #if (GAZEBO_MAJOR_VERSION >= 8)
273  rateModel.reset(ignition::math::Vector3d(req.bias.x, req.bias.y, req.bias.z));
274 #else
275  rateModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z));
276 #endif
277  return true;
278 }
279 
281 // Update the controller
283 {
284  // Get Time Difference dt
285 #if (GAZEBO_MAJOR_VERSION >= 8)
286  common::Time cur_time = world->SimTime();
287 #else
288  common::Time cur_time = world->GetSimTime();
289 #endif
290  double dt = updateTimer.getTimeSinceLastUpdate().Double();
291  boost::mutex::scoped_lock scoped_lock(lock);
292 
293  // Get Pose/Orientation
294 #if (GAZEBO_MAJOR_VERSION >= 8)
295  ignition::math::Pose3d pose = link->WorldPose();
296  // ignition::math::Vector3d pos = pose.pos + this->offset_.pos;
297  ignition::math::Quaterniond rot = this->offset_.Rot() * pose.Rot();
298 #else
299  math::Pose pose = link->GetWorldPose();
300  // math::Vector3 pos = pose.pos + this->offset_.pos;
301  math::Quaternion rot = this->offset_.rot * pose.rot;
302 #endif
303  rot.Normalize();
304 
305  // get Gravity
306 #if (GAZEBO_MAJOR_VERSION >= 8)
307  gravity = world->Gravity();
308  double gravity_length = gravity.Length();
309  ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.X(), gravity.Y(), gravity.Z());
310 #else
311  gravity = world->GetPhysicsEngine()->GetGravity();
312  double gravity_length = gravity.GetLength();
313  ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
314 #endif
315 
316  // get Acceleration and Angular Rates
317  // the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
318  //accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
319 #if (GAZEBO_MAJOR_VERSION >= 8)
320  ignition::math::Vector3d temp = link->WorldLinearVel(); // get velocity in world frame
321 #else
322  math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
323 #endif
324  if (dt > 0.0) accel = rot.RotateVectorReverse((temp - velocity) / dt - gravity);
325  velocity = temp;
326 
327  // calculate angular velocity from delta quaternion
328  // note: link->GetRelativeAngularVel() sometimes return nan?
329  // rate = link->GetRelativeAngularVel(); // get angular rate in body frame
330 #if (GAZEBO_MAJOR_VERSION >= 8)
331  ignition::math::Quaterniond delta = this->orientation.Inverse() * rot;
332 #else
333  math::Quaternion delta = this->orientation.GetInverse() * rot;
334 #endif
335  this->orientation = rot;
336  if (dt > 0.0) {
337 #if (GAZEBO_MAJOR_VERSION >= 8)
338  rate = this->offset_.Rot().Inverse()
339  * (2.0 * acos(std::max(std::min(delta.W(), 1.0), -1.0)) * ignition::math::Vector3d(delta.X(), delta.Y(), delta.Z()).Normalize() / dt);
340 #else
341  rate = this->offset_.rot.GetInverse()
342  * (2.0 * acos(std::max(std::min(delta.w, 1.0), -1.0)) * math::Vector3(delta.x, delta.y, delta.z).Normalize() / dt);
343 #endif
344  }
345 
346  // update sensor models
347  accel = accelModel(accel, dt);
348  rate = rateModel(rate, dt);
349  yawModel.update(dt);
350 #if (GAZEBO_MAJOR_VERSION >= 8)
351  ROS_DEBUG_NAMED("gazebo_ros_imu", "Current bias errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
355  ROS_DEBUG_NAMED("gazebo_ros_imu", "Scale errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
359 #else
360  ROS_DEBUG_NAMED("gazebo_ros_imu", "Current bias errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
364  ROS_DEBUG_NAMED("gazebo_ros_imu", "Scale errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
368 #endif
369 
370  // apply accelerometer and yaw drift error to orientation (pseudo AHRS)
371 #if (GAZEBO_MAJOR_VERSION >= 8)
372  ignition::math::Vector3d accelDrift = pose.Rot().RotateVector(accelModel.getCurrentBias());
373  double yawError = yawModel.getCurrentBias();
374  ignition::math::Quaterniond orientationError(
375  ignition::math::Quaterniond(cos(yawError/2), 0.0, 0.0, sin(yawError/2)) * // yaw error
376  ignition::math::Quaterniond(1.0, 0.5 * accelDrift.Y() / gravity_length, 0.5 * -accelDrift.X() / gravity_length, 0.0) // roll and pitch error
377  );
378 #else
379  math::Vector3 accelDrift = pose.rot.RotateVector(accelModel.getCurrentBias());
380  double yawError = yawModel.getCurrentBias();
381  math::Quaternion orientationError(
382  math::Quaternion(cos(yawError/2), 0.0, 0.0, sin(yawError/2)) * // yaw error
383  math::Quaternion(1.0, 0.5 * accelDrift.y / gravity_length, 0.5 * -accelDrift.x / gravity_length, 0.0) // roll and pitch error
384  );
385 #endif
386  orientationError.Normalize();
387  rot = orientationError * rot;
388 
389  // copy data into pose message
390  imuMsg.header.frame_id = frame_id_;
391  imuMsg.header.stamp.sec = cur_time.sec;
392  imuMsg.header.stamp.nsec = cur_time.nsec;
393 
394  // orientation quaternion
395 #if (GAZEBO_MAJOR_VERSION >= 8)
396  imuMsg.orientation.x = rot.X();
397  imuMsg.orientation.y = rot.Y();
398  imuMsg.orientation.z = rot.Z();
399  imuMsg.orientation.w = rot.W();
400 #else
401  imuMsg.orientation.x = rot.x;
402  imuMsg.orientation.y = rot.y;
403  imuMsg.orientation.z = rot.z;
404  imuMsg.orientation.w = rot.w;
405 #endif
406 
407  // pass angular rates
408 #if (GAZEBO_MAJOR_VERSION >= 8)
409  imuMsg.angular_velocity.x = rate.X();
410  imuMsg.angular_velocity.y = rate.Y();
411  imuMsg.angular_velocity.z = rate.Z();
412 #else
413  imuMsg.angular_velocity.x = rate.x;
414  imuMsg.angular_velocity.y = rate.y;
415  imuMsg.angular_velocity.z = rate.z;
416 #endif
417 
418  // pass accelerations
419 #if (GAZEBO_MAJOR_VERSION >= 8)
420  imuMsg.linear_acceleration.x = accel.X();
421  imuMsg.linear_acceleration.y = accel.Y();
422  imuMsg.linear_acceleration.z = accel.Z();
423 #else
424  imuMsg.linear_acceleration.x = accel.x;
425  imuMsg.linear_acceleration.y = accel.y;
426  imuMsg.linear_acceleration.z = accel.z;
427 #endif
428 
429  // fill in covariance matrix
430  imuMsg.orientation_covariance[8] = yawModel.gaussian_noise*yawModel.gaussian_noise;
431  if (gravity_length > 0.0) {
432 #if (GAZEBO_MAJOR_VERSION >= 8)
433  imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.X()*accelModel.gaussian_noise.X()/(gravity_length*gravity_length);
434  imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.Y()*accelModel.gaussian_noise.Y()/(gravity_length*gravity_length);
435 #else
436  imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
437  imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
438 #endif
439  } else {
440  imuMsg.orientation_covariance[0] = -1;
441  imuMsg.orientation_covariance[4] = -1;
442  }
443 
444  // publish to ros
446  ROS_DEBUG_NAMED("gazebo_ros_imu", "Publishing IMU data at t = %f", cur_time.Double());
447 
448  // publish bias
449  if (bias_pub_) {
450  biasMsg.header = imuMsg.header;
451 #if (GAZEBO_MAJOR_VERSION >= 8)
452  biasMsg.orientation.x = orientationError.X();
453  biasMsg.orientation.y = orientationError.Y();
454  biasMsg.orientation.z = orientationError.Z();
455  biasMsg.orientation.w = orientationError.W();
456  biasMsg.angular_velocity.x = rateModel.getCurrentBias().X();
457  biasMsg.angular_velocity.y = rateModel.getCurrentBias().Y();
458  biasMsg.angular_velocity.z = rateModel.getCurrentBias().Z();
459  biasMsg.linear_acceleration.x = accelModel.getCurrentBias().X();
460  biasMsg.linear_acceleration.y = accelModel.getCurrentBias().Y();
461  biasMsg.linear_acceleration.z = accelModel.getCurrentBias().Z();
462 #else
463  biasMsg.orientation.x = orientationError.x;
464  biasMsg.orientation.y = orientationError.y;
465  biasMsg.orientation.z = orientationError.z;
466  biasMsg.orientation.w = orientationError.w;
467  biasMsg.angular_velocity.x = rateModel.getCurrentBias().x;
468  biasMsg.angular_velocity.y = rateModel.getCurrentBias().y;
469  biasMsg.angular_velocity.z = rateModel.getCurrentBias().z;
470  biasMsg.linear_acceleration.x = accelModel.getCurrentBias().x;
471  biasMsg.linear_acceleration.y = accelModel.getCurrentBias().y;
472  biasMsg.linear_acceleration.z = accelModel.getCurrentBias().z;
473 #endif
475  }
476 
477  // debug output
478 #ifdef DEBUG_OUTPUT
479  if (debugPublisher) {
480  geometry_msgs::PoseStamped debugPose;
481  debugPose.header = imuMsg.header;
482  debugPose.header.frame_id = "/map";
483  debugPose.pose.orientation.w = imuMsg.orientation.w;
484  debugPose.pose.orientation.x = imuMsg.orientation.x;
485  debugPose.pose.orientation.y = imuMsg.orientation.y;
486  debugPose.pose.orientation.z = imuMsg.orientation.z;
487 #if (GAZEBO_MAJOR_VERSION >= 8)
488  ignition::math::Pose3d pose = link->WorldPose();
489  debugPose.pose.position.x = pose.Pos().X();
490  debugPose.pose.position.y = pose.Pos().Y();
491  debugPose.pose.position.z = pose.Pos().Z();
492 #else
493  math::Pose pose = link->GetWorldPose();
494  debugPose.pose.position.x = pose.pos.x;
495  debugPose.pose.position.y = pose.pos.y;
496  debugPose.pose.position.z = pose.pos.z;
497 #endif
498  debugPublisher.publish(debugPose);
499  }
500 #endif // DEBUG_OUTPUT
501 }
502 
503 #ifdef USE_CBQ
504 void GazeboRosIMU::CallbackQueueThread()
505 {
506  static const double timeout = 0.01;
507 
508  while (rosnode_->ok())
509  {
510  callback_queue_.callAvailable(ros::WallDuration(timeout));
511  }
512 }
513 #endif
514 
515 // Register this plugin with the simulator
516 GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU)
517 
518 } // namespace gazebo
ros::ServiceServer accelBiasService
virtual T update(double dt)
Definition: sensor_model.h:157
#define ROS_FATAL(...)
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_yaw_
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
Definition: update_timer.h:52
std::string namespace_
for setting ROS name space
ros::Publisher pub_
void publish(const boost::shared_ptr< M > &message) const
ros::ServiceServer srv_
bool SetAccelBiasCallback(hector_gazebo_plugins::SetBias::Request &req, hector_gazebo_plugins::SetBias::Response &res)
Bias service callbacks.
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix=std::string())
Definition: sensor_model.h:101
physics::WorldPtr world
The parent World.
boost::mutex lock
A mutex to lock access to fields that are used in message callbacks.
ROSCPP_DECL bool isInitialized()
SensorModel3 rateModel
virtual void Reset()
Definition: update_timer.h:175
virtual T getCurrentBias() const
Definition: sensor_model.h:60
std::string bias_topic_
virtual event::ConnectionPtr Connect(const boost::function< void()> &_subscriber, bool connectToWorldUpdateBegin=true)
Definition: update_timer.h:85
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< dynamic_reconfigure::Server< SensorModelConfig > > dynamic_reconfigure_server_rate_
std::string frame_id_
frame id
std::string link_name_
store link name
#define ROS_DEBUG_NAMED(name,...)
math::Vector3 velocity
sensor_msgs::Imu biasMsg
std::string topic_
topic name
#define ROS_FATAL_STREAM(args)
virtual void Disconnect(event::ConnectionPtr const &_c=event::ConnectionPtr())
Definition: update_timer.h:95
virtual void dynamicReconfigureCallback(SensorModelConfig &config, uint32_t level)
event::ConnectionPtr updateConnection
#define ROS_INFO(...)
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
common::Time getTimeSinceLastUpdate() const
Definition: update_timer.h:132
ros::NodeHandle * node_handle_
pointer to ros node
virtual ~GazeboRosIMU()
Destructor.
sensor_msgs::Imu imuMsg
ros message
ros::Publisher bias_pub_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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.
virtual void reset()
Definition: sensor_model.h:191
physics::LinkPtr link
The link referred to by this plugin.
math::Quaternion orientation
save current body/physics state
virtual const T & getScaleError() const
Definition: sensor_model.h:62
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