MagnetometerROSPlugin.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 hector_gazebo
17 // (https://github.com/tu-darmstadt-ros-pkg/hector_gazebo)
18 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt,
19 // licensed under the BSD 3-Clause license,
20 // cf. 3rd-party-licenses.txt file in the root directory of this source tree.
21 //
22 // The original code was modified to:
23 // - be more consistent with other sensor plugins within uuv_simulator,
24 // - adhere to Gazebo's coding standards.
25 
27 
28 namespace gazebo
29 {
32 { }
33 
36 { }
37 
39 void MagnetometerROSPlugin::Load(physics::ModelPtr _model,
40  sdf::ElementPtr _sdf)
41 {
42  ROSBaseModelPlugin::Load(_model, _sdf);
43 
44  GetSDFParam<double>(_sdf, "intensity", this->parameters.intensity, 1.0);
45  GetSDFParam<double>(_sdf, "reference_heading", this->parameters.heading,
46  M_PI);
47  GetSDFParam<double>(_sdf, "declination", this->parameters.declination, 0.0);
48  GetSDFParam<double>(_sdf, "inclination", this->parameters.inclination,
49  60.*M_PI/180.);
50  GetSDFParam<double>(_sdf, "noise_xy", this->parameters.noiseXY, 1.0);
51  GetSDFParam<double>(_sdf, "noise_z", this->parameters.noiseZ, 1.4);
52  GetSDFParam<double>(_sdf, "turn_on_bias", this->parameters.turnOnBias, 2.0);
53 
54  this->magneticFieldWorld.X() = this->parameters.intensity *
55  cos(this->parameters.inclination) *
56  cos(this->parameters.heading - this->parameters.declination);
57  this->magneticFieldWorld.Y() = this->parameters.intensity *
58  cos(this->parameters.inclination) *
59  sin(this->parameters.heading - this->parameters.declination);
60  this->magneticFieldWorld.Z() = this->parameters.intensity *
61  -1 * sin(this->parameters.inclination);
62 
63  this->AddNoiseModel("turn_on_bias", this->parameters.turnOnBias);
64 
65  // FIXME Add different options for noise amplitude for each noise model
66  this->turnOnBias.X() = this->GetGaussianNoise("turn_on_bias",
67  this->noiseAmp);
68  this->turnOnBias.Y() = this->GetGaussianNoise("turn_on_bias",
69  this->noiseAmp);
70  this->turnOnBias.Z() = this->GetGaussianNoise("turn_on_bias",
71  this->noiseAmp);
72 
73  // Initialize ROS message
74  if (this->enableLocalNEDFrame)
75  this->rosMsg.header.frame_id = tfLocalNEDFrame.child_frame_id_;
76  else
77  this->rosMsg.header.frame_id = this->link->GetName();
78 
79  this->AddNoiseModel("noise_xy", this->parameters.noiseXY);
80  this->AddNoiseModel("noise_z", this->parameters.noiseZ);
81 
82  this->rosMsg.magnetic_field_covariance[0] =
83  this->parameters.noiseXY * this->parameters.noiseXY;
84  this->rosMsg.magnetic_field_covariance[4] =
85  this->parameters.noiseXY * this->parameters.noiseXY;
86  this->rosMsg.magnetic_field_covariance[8] =
87  this->parameters.noiseZ * this->parameters.noiseZ;
88 
89  // Initialize the default magnetometer output
90  this->rosSensorOutputPub =
91  this->rosNode->advertise<sensor_msgs::MagneticField>(
92  this->sensorOutputTopic, 1);
93 
94  if (this->gazeboMsgEnabled)
95  {
96  this->gazeboSensorOutputPub =
97  this->gazeboNode->Advertise<sensor_msgs::msgs::Magnetic>(
98  this->robotNamespace + "/" + this->sensorOutputTopic, 1);
99  }
100 }
101 
103 bool MagnetometerROSPlugin::OnUpdate(const common::UpdateInfo& _info)
104 {
105  if (!this->EnableMeasurement(_info))
106  return false;
107 
108  if (this->enableLocalNEDFrame)
109  this->SendLocalNEDTransform();
110 
111  ignition::math::Pose3d pose;
112  // Read sensor link's current pose
113 #if GAZEBO_MAJOR_VERSION >= 8
114  pose = this->link->WorldPose();
115 #else
116  pose = this->link->GetWorldPose().Ign();
117 #endif
118 
119  ignition::math::Vector3d noise(
120  this->GetGaussianNoise("noise_xy", this->noiseAmp),
121  this->GetGaussianNoise("noise_xy", this->noiseAmp),
122  this->GetGaussianNoise("noise_z", this->noiseAmp));
123 
124  this->measMagneticField =
125  pose.Rot().RotateVectorReverse(this->magneticFieldWorld) +
126  this->turnOnBias +
127  noise;
128 
129  if (this->enableLocalNEDFrame)
130  this->measMagneticField = this->localNEDFrame.Rot().RotateVector(
131  this->measMagneticField);
132 
133  if (this->gazeboMsgEnabled)
134  {
135  sensor_msgs::msgs::Magnetic gazeboMsg;
136 
137  gazebo::msgs::Vector3d* field = new gazebo::msgs::Vector3d();
138  field->set_x(this->measMagneticField.X());
139  field->set_y(this->measMagneticField.Y());
140  field->set_z(this->measMagneticField.Z());
141  gazeboMsg.set_allocated_magnetic_field(field);
142 
143  this->gazeboSensorOutputPub->Publish(gazeboMsg);
144  }
145 
146  this->rosMsg.header.stamp = ros::Time::now();
147  this->rosMsg.magnetic_field.x = this->measMagneticField.X();
148  this->rosMsg.magnetic_field.y = this->measMagneticField.Y();
149  this->rosMsg.magnetic_field.z = this->measMagneticField.Z();
150 
151  this->rosSensorOutputPub.publish(this->rosMsg);
152 }
153 
156 
157 }
double GetGaussianNoise(double _amp)
Returns noise value for a function with zero mean from the default Gaussian noise model...
MagnetometerParameters parameters
Magnetometer configuration parameters:
void publish(const boost::shared_ptr< M > &message) const
tf::StampedTransform tfLocalNEDFrame
Local NED TF frame.
bool EnableMeasurement(const common::UpdateInfo &_info) const
Enables generation of simulated measurement if the timeout since the last update has been reached...
ignition::math::Vector3d measMagneticField
Last measurement of magnetic field.
bool enableLocalNEDFrame
True if a the local NED frame needs to be broadcasted.
void SendLocalNEDTransform()
Returns true if the base_link_ned frame exists.
double noiseAmp
Noise amplitude.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
bool AddNoiseModel(std::string _name, double _sigma)
Add noise normal distribution to the list.
std::string child_frame_id_
transport::NodePtr gazeboNode
Gazebo&#39;s node handle for transporting measurement messages.
double declination
Declination of reference earth magnetic field [rad].
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.
double heading
Heading angle of reference earth magnetic field [rad].
double intensity
Intensity of reference earth magnetic field [muT].
ignition::math::Vector3d turnOnBias
Constant turn-on bias [muT].
physics::LinkPtr link
Pointer to the link.
sensor_msgs::MagneticField rosMsg
ROS message.
double noiseZ
Discrete-time standard dev. of output noise in z-axis [muT].
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 magneticFieldWorld
Reference magnetic field in world frame:
double inclination
Inclination of reference earth magnetic field [rad].
MagnetometerROSPlugin()
Class constructor.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
double turnOnBias
Standard deviation of constant systematic offset of measurements [muT].
static Time now()
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
std::string robotNamespace
Robot namespace.
bool gazeboMsgEnabled
Flag set to true if the Gazebo sensors messages are supposed to be published as well (it can avoid un...
virtual ~MagnetometerROSPlugin()
Class destructor.
double noiseXY
Discrete-time standard dev. of output noise in xy-axis [muT].


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