IMUROSPlugin.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
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 // This source code is derived from rotors_simulator
17 // (https://github.com/ethz-asl/rotors_simulator)
18 // * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland,
19 // * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland,
20 // * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland,
21 // * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland,
22 // * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland,
23 // licensed under the Apache-2.0 license,
24 // cf. 3rd-party-licenses.txt file in the root directory of this source tree.
25 //
26 // The original code was modified to:
27 // - be more consistent with other sensor plugins within uuv_simulator,
28 // - adhere to Gazebo's coding standards.
29 
31 
32 namespace gazebo
33 {
36 { }
37 
40 { }
41 
43 void IMUROSPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
44 {
45  ROSBaseModelPlugin::Load(_model, _sdf);
46 
47  // Only need to load settings specific to this sensor.
48  GetSDFParam<double>(_sdf, "gyroscope_noise_density",
51  GetSDFParam<double>(_sdf, "gyroscope_bias_random_walk",
54  GetSDFParam<double>(_sdf, "gyroscope_bias_correlation_time",
57  GZ_ASSERT(this->imuParameters.gyroscopeBiasCorrelationTime > 0.0,
58  "Gyroscope bias correlation time must be greater than zero");
59  GetSDFParam<double>(_sdf, "gyroscope_turn_on_bias_sigma",
62  GetSDFParam<double>(_sdf, "accelerometer_noise_density",
65  GetSDFParam<double>(_sdf, "accelerometer_random_walk",
68  GetSDFParam<double>(_sdf, "accelerometer_bias_correlation_time",
71  GZ_ASSERT(this->imuParameters.accelerometerBiasCorrelationTime > 0.0,
72  "Accelerometer bias correlation time must be greater than zero");
73  GetSDFParam<double>(_sdf, "accelerometer_turn_on_bias_sigma",
76  GetSDFParam<double>(_sdf, "orientation_noise",
79 
80  this->imuROSMessage.header.frame_id = this->link->GetName();
81  // Fill IMU message.
82  // We assume uncorrelated noise on the 3 channels -> only set diagonal
83  // elements. Only the broadband noise component is considered, specified as
84  // a continuous-time density (two-sided spectrum); not the true covariance
85  // of the measurements.
86  // Angular velocity measurement covariance.
87  this->AddNoiseModel("gyro_noise_density", this->imuParameters.gyroscopeNoiseDensity);
88  double gyroVar = this->imuParameters.gyroscopeNoiseDensity *
90  this->imuROSMessage.angular_velocity_covariance[0] = gyroVar;
91  this->imuROSMessage.angular_velocity_covariance[4] = gyroVar;
92  this->imuROSMessage.angular_velocity_covariance[8] = gyroVar;
93 
94  // Linear acceleration measurement covariance.
95  this->AddNoiseModel("acc_noise_density", this->imuParameters.accelerometerNoiseDensity);
96  double accelVar = this->imuParameters.accelerometerNoiseDensity *
98  this->imuROSMessage.linear_acceleration_covariance[0] = accelVar;
99  this->imuROSMessage.linear_acceleration_covariance[4] = accelVar;
100  this->imuROSMessage.linear_acceleration_covariance[8] = accelVar;
101 
102  // Orientation estimate covariance
103  this->AddNoiseModel("orientation_noise_density", this->imuParameters.orientationNoise);
104  double orientationVar = this->imuParameters.orientationNoise *
106  this->imuROSMessage.orientation_covariance[0] = orientationVar;
107  this->imuROSMessage.orientation_covariance[4] = orientationVar;
108  this->imuROSMessage.orientation_covariance[8] = orientationVar;
109 
110  // Store the acc. gravity vector
111 #if GAZEBO_MAJOR_VERSION >= 8
112  this->gravityWorld = this->world->Gravity();
113 #else
114  this->gravityWorld = this->world->GetPhysicsEngine()->GetGravity().Ign();
115 #endif
116 
117  double sigmaBonG = this->imuParameters.gyroscopeTurnOnBiasSigma;
118  double sigmaBonA = this->imuParameters.accelerometerTurnOnBiasSigma;
119 
120  this->AddNoiseModel("gyro_turn_on_bias", sigmaBonG);
121  this->AddNoiseModel("acc_turn_on_bias", sigmaBonA);
122 
123  // FIXME Add the noise amplitude input for gyroscope
124  this->gyroscopeTurnOnBias = ignition::math::Vector3d(
125  this->GetGaussianNoise("gyro_turn_on_bias", this->noiseAmp),
126  this->GetGaussianNoise("gyro_turn_on_bias", this->noiseAmp),
127  this->GetGaussianNoise("gyro_turn_on_bias", this->noiseAmp));
128  // FIXME Add the noise amplitude input for accelerometer
129  this->accelerometerTurnOnBias = ignition::math::Vector3d(
130  this->GetGaussianNoise("acc_turn_on_bias", this->noiseAmp),
131  this->GetGaussianNoise("acc_turn_on_bias", this->noiseAmp),
132  this->GetGaussianNoise("acc_turn_on_bias", this->noiseAmp));
133 
134  // TODO(nikolicj) incorporate steady-state covariance of bias process
135  this->gyroscopeBias = ignition::math::Vector3d::Zero;
136  this->accelerometerBias = ignition::math::Vector3d::Zero;
137 
138  this->rosSensorOutputPub =
139  this->rosNode->advertise<sensor_msgs::Imu>(this->sensorOutputTopic, 1);
140 
141  if (this->gazeboMsgEnabled)
142  {
143  this->gazeboSensorOutputPub =
144  this->gazeboNode->Advertise<sensor_msgs::msgs::Imu>(
145  this->robotNamespace + "/" + this->sensorOutputTopic, 1);
146  }
147 }
148 
150 bool IMUROSPlugin::OnUpdate(const common::UpdateInfo& _info)
151 {
152  // Publish sensor state
153  this->PublishState();
154 
155  if (!this->EnableMeasurement(_info))
156  return false;
157 
158  if (this->enableLocalNEDFrame)
159  this->SendLocalNEDTransform();
160 
161  // Read the current simulation time
162 #if GAZEBO_MAJOR_VERSION >= 8
163  common::Time curTime = this->world->SimTime();
164 #else
165  common::Time curTime = this->world->GetSimTime();
166 #endif
167 
168  double dt = curTime.Double() - this->lastMeasurementTime.Double();
169 
170  ignition::math::Pose3d worldLinkPose;
171  ignition::math::Vector3d refLinVel;
172  ignition::math::Vector3d bodyAngVel;
173  ignition::math::Vector3d bodyLinAcc, worldLinAcc;
174  ignition::math::Vector3d refGravityWorld;
175 
176  // Read sensor link's current pose and velocity
177 #if GAZEBO_MAJOR_VERSION >= 8
178  bodyAngVel = this->link->RelativeAngularVel();
179  bodyLinAcc = this->link->RelativeLinearAccel();
180  worldLinkPose = this->link->WorldPose();
181 #else
182  bodyAngVel = this->link->GetRelativeAngularVel().Ign();
183  bodyLinAcc = this->link->GetRelativeLinearAccel().Ign();
184  worldLinkPose = this->link->GetWorldPose().Ign();
185 #endif
186 
187  this->UpdateReferenceFramePose();
188  if (this->referenceLink)
189  {
190 #if GAZEBO_MAJOR_VERSION >= 8
191  this->referenceFrame = this->referenceLink->WorldPose();
192 #else
193  this->referenceFrame = this->referenceLink->GetWorldPose().Ign();
194 #endif
195  }
196 
197  // Transform pose and velocity vectors to be represented wrt the
198  // reference link provided
199  worldLinkPose.Pos() = worldLinkPose.Pos() - this->referenceFrame.Pos();
200  worldLinkPose.Pos() = this->referenceFrame.Rot().RotateVectorReverse(
201  worldLinkPose.Pos());
202  worldLinkPose.Rot() *= this->referenceFrame.Rot().Inverse();
203 
204  ignition::math::Vector3d gravityBody =
205  worldLinkPose.Rot().RotateVectorReverse(this->gravityWorld);
206 
207  if (this->enableLocalNEDFrame)
208  {
209  bodyAngVel = this->localNEDFrame.Rot().RotateVector(bodyAngVel);
210  bodyLinAcc = this->localNEDFrame.Rot().RotateVector(bodyLinAcc);
211  gravityBody = this->localNEDFrame.Rot().RotateVector(gravityBody);
212  }
213 
214  // Compute the simulated measurements wrt the default world ENU frame
215  this->measLinearAcc = bodyLinAcc - gravityBody;
216  this->measAngularVel = bodyAngVel;
217  this->measOrientation = worldLinkPose.Rot();
218 
219  // Add noise and bias to the simulated data
220  this->AddNoise(this->measLinearAcc, this->measAngularVel,
221  this->measOrientation, dt);
222 
223  // Fill the ROS IMU message
224  this->imuROSMessage.header.stamp.sec = _info.simTime.sec;
225  this->imuROSMessage.header.stamp.nsec = _info.simTime.nsec;
226 
227  this->imuROSMessage.orientation.x = this->measOrientation.X();
228  this->imuROSMessage.orientation.y = this->measOrientation.Y();
229  this->imuROSMessage.orientation.z = this->measOrientation.Z();
230  this->imuROSMessage.orientation.w = this->measOrientation.W();
231 
232  this->imuROSMessage.linear_acceleration.x = this->measLinearAcc.X();
233  this->imuROSMessage.linear_acceleration.y = this->measLinearAcc.Y();
234  this->imuROSMessage.linear_acceleration.z = this->measLinearAcc.Z();
235 
236  this->imuROSMessage.angular_velocity.x = this->measAngularVel.X();
237  this->imuROSMessage.angular_velocity.y = this->measAngularVel.Y();
238  this->imuROSMessage.angular_velocity.z = this->measAngularVel.Z();
239 
241 
242  if (this->gazeboMsgEnabled)
243  {
244  sensor_msgs::msgs::Imu imuGazeboMessage;
245  // Fill the Gazebo IMU message
246  for (int i = 0; i < 9; i++)
247  {
248  switch (i)
249  {
250  case 0:
251  imuGazeboMessage.add_angular_velocity_covariance(
253  this->imuParameters.gyroscopeNoiseDensity);
254  imuGazeboMessage.add_orientation_covariance(-1.0);
255  imuGazeboMessage.add_linear_acceleration_covariance(
257  this->imuParameters.accelerometerNoiseDensity);
258  break;
259  case 1:
260  case 2:
261  case 3:
262  imuGazeboMessage.add_angular_velocity_covariance(0.0);
263  imuGazeboMessage.add_orientation_covariance(-1.0);
264  imuGazeboMessage.add_linear_acceleration_covariance(0.0);
265  break;
266  case 4:
267  imuGazeboMessage.add_angular_velocity_covariance(
269  this->imuParameters.gyroscopeNoiseDensity);
270  imuGazeboMessage.add_orientation_covariance(-1.0);
271  imuGazeboMessage.add_linear_acceleration_covariance(
273  this->imuParameters.accelerometerNoiseDensity);
274  break;
275  case 5:
276  case 6:
277  case 7:
278  imuGazeboMessage.add_angular_velocity_covariance(0.0);
279  imuGazeboMessage.add_orientation_covariance(-1.0);
280  imuGazeboMessage.add_linear_acceleration_covariance(0.0);
281  break;
282  case 8:
283  imuGazeboMessage.add_angular_velocity_covariance(
285  this->imuParameters.gyroscopeNoiseDensity);
286  imuGazeboMessage.add_orientation_covariance(-1.0);
287  imuGazeboMessage.add_linear_acceleration_covariance(
289  this->imuParameters.accelerometerNoiseDensity);
290  break;
291  }
292  }
293  // Copy math::Quaternion to gazebo::msgs::Quaternion
294  gazebo::msgs::Quaternion * orientation = new gazebo::msgs::Quaternion();
295  orientation->set_x(this->measOrientation.X());
296  orientation->set_y(this->measOrientation.Y());
297  orientation->set_z(this->measOrientation.Z());
298  orientation->set_w(this->measOrientation.W());
299 
300  // Copy Eigen::Vector3d to gazebo::msgs::Vector3d
301  gazebo::msgs::Vector3d * linAcc = new gazebo::msgs::Vector3d();
302  linAcc->set_x(this->measLinearAcc.X());
303  linAcc->set_y(this->measLinearAcc.Y());
304  linAcc->set_z(this->measLinearAcc.Z());
305 
306  // Copy Eigen::Vector3d to gazebo::msgs::Vector3d
307  gazebo::msgs::Vector3d * angVel = new gazebo::msgs::Vector3d();
308  angVel->set_x(this->measAngularVel.X());
309  angVel->set_y(this->measAngularVel.Y());
310  angVel->set_z(this->measAngularVel.Z());
311 
312  imuGazeboMessage.set_allocated_orientation(orientation);
313  imuGazeboMessage.set_allocated_linear_acceleration(linAcc);
314  imuGazeboMessage.set_allocated_angular_velocity(angVel);
315  this->gazeboSensorOutputPub->Publish(imuGazeboMessage);
316  }
317 
318  this->lastMeasurementTime = curTime;
319  return true;
320 }
321 
323 void IMUROSPlugin::AddNoise(ignition::math::Vector3d& _linAcc,
324  ignition::math::Vector3d& _angVel, ignition::math::Quaterniond& _orientation,
325  double _dt)
326 {
327  GZ_ASSERT(_dt > 0.0, "Invalid time step");
328 
330  double tauG = this->imuParameters.gyroscopeBiasCorrelationTime;
331  // Discrete-time standard deviation equivalent to an "integrating" sampler
332  // with integration time dt.
333  double sigmaGD = 1 / sqrt(_dt) * this->imuParameters.gyroscopeNoiseDensity;
334  double sigmaBG = this->imuParameters.gyroscopeRandomWalk;
335  // Compute exact covariance of the process after dt [Maybeck 4-114].
336  double sigmaBGD = sqrt(- sigmaBG * sigmaBG * tauG / 2.0 *
337  (exp(-2.0 * _dt / tauG) - 1.0));
338  // Compute state-transition.
339  double phiGD = exp(-1.0 / tauG * _dt);
340 
341  // FIXME Add the noise amplitude input for BGD
342  this->AddNoiseModel("bgd", sigmaBGD);
343  // FIXME Add the noise amplitude input for GD
344  this->AddNoiseModel("gd", sigmaGD);
345 
346  // Simulate gyroscope noise processes and add them to the true angular rate.
347  this->gyroscopeBias = phiGD * this->gyroscopeBias +
348  ignition::math::Vector3d(
349  this->GetGaussianNoise("bgd", this->noiseAmp),
350  this->GetGaussianNoise("bgd", this->noiseAmp),
351  this->GetGaussianNoise("bgd", this->noiseAmp));
352  _angVel = _angVel + this->gyroscopeBias + this->gyroscopeTurnOnBias +
353  ignition::math::Vector3d(
354  this->GetGaussianNoise("gd", this->noiseAmp),
355  this->GetGaussianNoise("gd", this->noiseAmp),
356  this->GetGaussianNoise("gd", this->noiseAmp));
357 
360  // Discrete-time standard deviation equivalent to an "integrating" sampler
361  // with integration time dt.
362  double sigmaAD = 1. / sqrt(_dt) * this->imuParameters.accelerometerNoiseDensity;
363  double sigmaBA = this->imuParameters.accelerometerRandomWalk;
364  // Compute exact covariance of the process after dt [Maybeck 4-114].
365  double sigmaBAD = sqrt(- sigmaBA * sigmaBA * tauA / 2.0 *
366  (exp(-2.0 * _dt / tauA) - 1.0));
367  // Compute state-transition.
368  double phiAD = exp(-1.0 / tauA * _dt);
369 
370  // FIXME Add the noise amplitude input for BAD
371  this->AddNoiseModel("bad", sigmaBAD);
372  // FIXME Add the noise amplitude input for BAD
373  this->AddNoiseModel("ad", sigmaAD);
374 
375  // Simulate accelerometer noise processes and add them to the true linear
376  // acceleration.
377  this->accelerometerBias = phiAD * this->accelerometerBias +
378  ignition::math::Vector3d(
379  this->GetGaussianNoise("bad", this->noiseAmp),
380  this->GetGaussianNoise("bad", this->noiseAmp),
381  this->GetGaussianNoise("bad", this->noiseAmp));
382  _linAcc = _linAcc + this->accelerometerBias + this->accelerometerTurnOnBias +
383  ignition::math::Vector3d(
384  this->GetGaussianNoise("ad", this->noiseAmp),
385  this->GetGaussianNoise("ad", this->noiseAmp),
386  this->GetGaussianNoise("ad", this->noiseAmp));
387 
389  // Construct error quaterion using small-angle approximation.
390  double scale = 0.5 * this->imuParameters.orientationNoise;
391 
392  // Attention: w-xyz
393  ignition::math::Quaterniond error(1.0,
394  this->GetGaussianNoise("orientation_noise_density", scale),
395  this->GetGaussianNoise("orientation_noise_density", scale),
396  this->GetGaussianNoise("orientation_noise_density", scale));
397 
398  error.Normalize();
399  _orientation = _orientation * error;
400 }
401 
404 }
physics::LinkPtr referenceLink
Reference link.
double GetGaussianNoise(double _amp)
Returns noise value for a function with zero mean from the default Gaussian noise model...
void publish(const boost::shared_ptr< M > &message) const
bool EnableMeasurement(const common::UpdateInfo &_info) const
Enables generation of simulated measurement if the timeout since the last update has been reached...
IMUROSPlugin()
Class constructor.
Definition: IMUROSPlugin.cc:35
ignition::math::Pose3d referenceFrame
Pose of the reference frame wrt world frame.
physics::WorldPtr world
Pointer to the world.
IMUParameters imuParameters
IMU model parameters.
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
double orientationNoise
Standard deviation of orientation noise per axis [rad].
Definition: IMUROSPlugin.hh:81
double accelerometerNoiseDensity
Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)].
Definition: IMUROSPlugin.hh:69
bool enableLocalNEDFrame
True if a the local NED frame needs to be broadcasted.
double gyroscopeBiasCorrelationTime
Gyroscope bias correlation time constant [s].
Definition: IMUROSPlugin.hh:63
void SendLocalNEDTransform()
Returns true if the base_link_ned frame exists.
ignition::math::Vector3d gravityWorld
Gravity vector wrt. reference frame.
double noiseAmp
Noise amplitude.
void AddNoise(ignition::math::Vector3d &_linAcc, ignition::math::Vector3d &_angVel, ignition::math::Quaterniond &_orientation, double _dt)
Apply and add nosie model to ideal measurements.
double accelerometerTurnOnBiasSigma
Accelerometer turn on bias standard deviation [m/s^2].
Definition: IMUROSPlugin.hh:78
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
void UpdateReferenceFramePose()
Updates the pose of the reference frame wrt the world frame.
bool AddNoiseModel(std::string _name, double _sigma)
Add noise normal distribution to the list.
transport::NodePtr gazeboNode
Gazebo&#39;s node handle for transporting measurement messages.
double gyroscopeNoiseDensity
Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)].
Definition: IMUROSPlugin.hh:57
virtual ~IMUROSPlugin()
Class destructor.
Definition: IMUROSPlugin.cc:39
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
transport::PublisherPtr gazeboSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
ignition::math::Pose3d localNEDFrame
Pose of the local NED frame wrt link frame.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
Definition: IMUROSPlugin.cc:43
ignition::math::Quaterniond measOrientation
(Simulation) time when the last sensor measurement was generated.
double gyroscopeTurnOnBiasSigma
Gyroscope turn on bias standard deviation [rad/s].
Definition: IMUROSPlugin.hh:66
ignition::math::Vector3d gyroscopeBias
Current (drifting) gyroscope bias.
ignition::math::Vector3d measLinearAcc
Last measurement of linear acceleration..
physics::LinkPtr link
Pointer to the link.
double gyroscopeRandomWalk
Gyroscope bias random walk [rad/s/s/sqrt(Hz)].
Definition: IMUROSPlugin.hh:60
ignition::math::Vector3d measAngularVel
Last measurement of angular velocity.
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
ros::Publisher rosSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
ignition::math::Vector3d gyroscopeTurnOnBias
Constant turn-on gyroscope bias.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
std::string robotNamespace
Robot namespace.
ignition::math::Vector3d accelerometerTurnOnBias
Constant turn-on accelerometer bias.
bool gazeboMsgEnabled
Flag set to true if the Gazebo sensors messages are supposed to be published as well (it can avoid un...
double accelerometerRandomWalk
Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)].
Definition: IMUROSPlugin.hh:72
void PublishState()
Publish the current state of the plugin.
ignition::math::Vector3d accelerometerBias
Current (drifting) accelerometer bias.
sensor_msgs::Imu imuROSMessage
ROS IMU message.
double accelerometerBiasCorrelationTime
Accelerometer bias correlation time constant [s].
Definition: IMUROSPlugin.hh:75


uuv_sensor_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Mon Jul 1 2019 19:39:20