SubseaPressureROSPlugin.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 
17 
18 namespace gazebo
19 {
22 { }
23 
26 { }
27 
29 void SubseaPressureROSPlugin::Load(physics::ModelPtr _model,
30  sdf::ElementPtr _sdf)
31 {
32  ROSBaseModelPlugin::Load(_model, _sdf);
33 
34  GetSDFParam<double>(_sdf, "saturation", this->saturation, 3000);
35  GetSDFParam<bool>(_sdf, "estimate_depth_on", this->estimateDepth, false);
36  GetSDFParam<double>(_sdf, "standard_pressure", this->standardPressure,
37  101.325);
38  GetSDFParam<double>(_sdf, "kPa_per_meter", this->kPaPerM, 9.80638);
39 
40  this->rosSensorOutputPub =
41  this->rosNode->advertise<sensor_msgs::FluidPressure>(
42  this->sensorOutputTopic, 1);
43 
44  if (this->gazeboMsgEnabled)
45  {
46  this->gazeboSensorOutputPub =
47  this->gazeboNode->Advertise<sensor_msgs::msgs::Pressure>(
48  this->robotNamespace + "/" + this->sensorOutputTopic, 1);
49  }
50 }
51 
53 bool SubseaPressureROSPlugin::OnUpdate(const common::UpdateInfo& _info)
54 {
55  // Publish sensor state
56  this->PublishState();
57 
58  if (!this->EnableMeasurement(_info))
59  return false;
60 
61  // Using the world pose wrt Gazebo's ENU reference frame
62  ignition::math::Vector3d pos;
63 #if GAZEBO_MAJOR_VERSION >= 8
64  pos = this->link->WorldPose().Pos();
65 #else
66  pos = this->link->GetWorldPose().Ign().Pos();
67 #endif
68 
69  double depth = std::abs(pos.Z());
70  double pressure = this->standardPressure;
71  if (depth >= 0)
72  {
73  // Convert depth to pressure
74  pressure += depth * this->kPaPerM;
75  }
76 
77  pressure += this->GetGaussianNoise(this->noiseAmp);
78 
79  double inferredDepth = 0.0;
80  // Estimate depth, if enabled
81  if (this->estimateDepth)
82  {
83  inferredDepth = (pressure - this->standardPressure) / this->kPaPerM;
84  }
85 
86  // Publish Gazebo pressure message, if enabled
87  if (this->gazeboMsgEnabled)
88  {
89  sensor_msgs::msgs::Pressure gazeboMsg;
90 
91  gazeboMsg.set_pressure(pressure);
92  gazeboMsg.set_stddev(this->noiseSigma);
93 
94  if (this->estimateDepth)
95  gazeboMsg.set_depth(inferredDepth);
96  this->gazeboSensorOutputPub->Publish(gazeboMsg);
97  }
98 
99  // Publish ROS pressure message
100  sensor_msgs::FluidPressure rosMsg;
101 
102  rosMsg.header.stamp.sec = _info.simTime.sec;
103  rosMsg.header.stamp.nsec = _info.simTime.nsec;
104  rosMsg.header.frame_id = this->link->GetName();
105 
106  rosMsg.fluid_pressure = pressure;
107  rosMsg.variance = this->noiseSigma * this->noiseSigma;
108 
109  this->rosSensorOutputPub.publish(rosMsg);
110 
111  // Read the current simulation time
112 #if GAZEBO_MAJOR_VERSION >= 8
113  this->lastMeasurementTime = this->world->SimTime();
114 #else
115  this->lastMeasurementTime = this->world->GetSimTime();
116 #endif
117  return true;
118 }
119 
122 }
double kPaPerM
Factor of kPa per meter.
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...
physics::WorldPtr world
Pointer to the world.
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
double noiseAmp
Noise amplitude.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
double saturation
Sensor saturation (max. value for output pressure in Pa)
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
transport::NodePtr gazeboNode
Gazebo&#39;s node handle for transporting measurement messages.
double noiseSigma
Noise standard deviation.
transport::PublisherPtr gazeboSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
physics::LinkPtr link
Pointer to the link.
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
ros::Publisher rosSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
bool estimateDepth
If flag is set to true, estimate depth according to pressure measurement.
std::string robotNamespace
Robot namespace.
double standardPressure
Standard pressure.
bool gazeboMsgEnabled
Flag set to true if the Gazebo sensors messages are supposed to be published as well (it can avoid un...
void PublishState()
Publish the current state of the plugin.


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