CPCROSPlugin.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 CPCROSPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
30 {
31  ROSBaseModelPlugin::Load(_model, _sdf);
32 
33  std::string inputTopic;
34  GetSDFParam<std::string>(_sdf, "plume_topic", inputTopic, "");
35  GZ_ASSERT(!inputTopic.empty(), "Plume topic for the point cloud is empty!");
36 
37  GetSDFParam<double>(_sdf, "gamma", this->gamma, 0.001);
38  GZ_ASSERT(this->gamma > 0, "Gamma value must be greater than zero");
39 
40  GetSDFParam<double>(_sdf, "gain", this->gain, 1.0);
41  GZ_ASSERT(this->gamma > 0, "Gain value must be greater than zero");
42 
43  GetSDFParam<double>(_sdf, "radius", this->smoothingLength, 3.0);
44  GZ_ASSERT(this->smoothingLength > 0,
45  "Radius of the sensor must be greater than zero");
46 
47  // Reading the name of the output salinity topic
48  std::string salinityTopic;
49  GetSDFParam<std::string>(_sdf, "salinity_output_topic", salinityTopic, "salinity");
50  GZ_ASSERT(!salinityTopic.empty(), "Salinity topic name is empty!");
51 
52  // Reading the salinity unit to be used, options are
53  // - PPT (parts per thousand)
54  // - PPM (parts per million)
55  // - PSU (practical salinity unit)
56  std::string salinityUnit;
57  GetSDFParam<std::string>(_sdf, "salinity_unit", salinityUnit, "ppt");
58 
59  GZ_ASSERT(salinityUnit.compare("ppt") == 0 ||
60  salinityUnit.compare("ppm") == 0 ||
61  salinityUnit.compare("psu") == 0,
62  "Invalid salinity unit, it can be ppt, ppm or psu");
63 
64  this->salinityMsg.unit = salinityUnit;
65 
66  if (_sdf->HasElement("water_salinity_value"))
67  {
68  GetSDFParam<double>(_sdf, "water_salinity_value", this->waterSalinityValue, 0.0);
69  GZ_ASSERT(this->waterSalinityValue > 0, "Water salinity reference must be a positive value");
70  }
71  else
72  {
73  if (salinityUnit.compare(uuv_sensor_ros_plugins_msgs::Salinity::PPT) == 0)
74  this->waterSalinityValue = 35.0;
75  else if (salinityUnit.compare(uuv_sensor_ros_plugins_msgs::Salinity::PPM) == 0)
76  this->waterSalinityValue = 35000.0;
77  else
78  this->waterSalinityValue = 35.0;
79  }
80 
81  GetSDFParam<double>(_sdf, "plume_salinity_value", this->plumeSalinityValue, 0.0);
82  GZ_ASSERT(this->plumeSalinityValue >= 0.0,
83  "Plume salinity value must be greater or equal to zero");
84  GZ_ASSERT(this->plumeSalinityValue < this->waterSalinityValue,
85  "Plume salinity value must be lower than the water salinity value");
86 
87  this->particlesSub = this->rosNode->subscribe<sensor_msgs::PointCloud>(
88  inputTopic, 1,
90  this, _1));
91 
92  // Set initial particle concentration value to zero
93  this->outputMsg.concentration = 0.0;
94 
95  // Set the is_measuring flag initially to false
96  this->outputMsg.is_measuring = false;
97 
98  this->salinityMsg.variance = this->noiseSigma * this->noiseSigma;
99 
100  this->rosSensorOutputPub = this->rosNode->advertise<
101  uuv_sensor_ros_plugins_msgs::ChemicalParticleConcentration>(
102  this->sensorOutputTopic, 1);
103 
104  this->salinityPub = this->rosNode->advertise<
105  uuv_sensor_ros_plugins_msgs::Salinity>(salinityTopic, 1);
106 
108 
109  gzmsg << "CPCROSPlugin[" << this->link->GetName()
110  << "] initialized!" << std::endl
111  << "\t- Input topic= " << inputTopic << std::endl
112  << "\t- Output topic= " << this->sensorOutputTopic << std::endl
113  << "\t- Salinity output topic= " << salinityTopic << std::endl;
114 }
115 
117 bool CPCROSPlugin::OnUpdate(const common::UpdateInfo& _info)
118 {
119  // Publish sensor state
120  this->PublishState();
121 
122  if (!this->EnableMeasurement(_info) || this->updatingCloud)
123  return false;
124 
125  // Set particle concentration to zero if the point cloud message has not
126  // been received for a long time
127  if (_info.simTime.Double() - this->lastUpdateTimestamp.toSec() > 5.0)
128  {
129  this->outputMsg.is_measuring = false;
130  this->outputMsg.concentration = 0.0;
131  }
132 
133  this->outputMsg.header.frame_id = this->referenceFrameID;
134  this->outputMsg.concentration += this->GetGaussianNoise(
135  this->noiseAmp);
136  this->outputMsg.header.stamp.sec = _info.simTime.sec;
137  this->outputMsg.header.stamp.nsec = _info.simTime.nsec;
138  this->rosSensorOutputPub.publish(this->outputMsg);
139 
140  this->salinityMsg.header.frame_id = this->referenceFrameID;
141  this->salinityMsg.header.stamp.sec = _info.simTime.sec;
142  this->salinityMsg.header.stamp.nsec = _info.simTime.nsec;
143 
144  this->salinityMsg.salinity =
145  this->waterSalinityValue * (1 - std::min(1.0, this->outputMsg.concentration)) +
146  std::min(1.0, this->outputMsg.concentration) * this->plumeSalinityValue;
147 
148  this->salinityPub.publish(this->salinityMsg);
149 
150  // Read the current simulation time
151 #if GAZEBO_MAJOR_VERSION >= 8
152  this->lastMeasurementTime = this->world->SimTime();
153 #else
154  this->lastMeasurementTime = this->world->GetSimTime();
155 #endif
156  return true;
157 }
158 
161  const sensor_msgs::PointCloud::ConstPtr &_msg)
162 {
163  if (this->rosSensorOutputPub.getNumSubscribers() > 0)
164  {
165  this->updatingCloud = true;
166 
167  double totalParticleConc = 0.0;
168  double smoothingParam;
169  double particleConc;
170  double distToParticle;
171 
172  ignition::math::Vector3d linkPos, linkPosRef;
173 #if GAZEBO_MAJOR_VERSION >= 8
174  linkPos = this->link->WorldPose().Pos();
175 #else
176  linkPos = this->link->GetWorldPose().Ign().Pos();
177 #endif
178 
179  // Apply transformation wrt to the reference frame
180  linkPosRef = linkPos - this->referenceFrame.Pos();
181  linkPosRef = this->referenceFrame.Rot().RotateVectorReverse(linkPosRef);
182 
183  this->outputMsg.is_measuring = true;
184  // Store the current position wrt the reference frame where this
185  // measurement was taken
186  this->outputMsg.position.x = linkPosRef.X();
187  this->outputMsg.position.y = linkPosRef.Y();
188  this->outputMsg.position.z = linkPosRef.Z();
189 
190  // Calculate the current position in WGS84 spherical coordinates
191  ignition::math::Vector3d cartVec = linkPos;
192 #if GAZEBO_MAJOR_VERSION >= 8
193  ignition::math::Vector3d scVec =
194  this->link->GetWorld()->SphericalCoords()->SphericalFromLocal(
195  cartVec);
196 #else
197  ignition::math::Vector3d scVec =
198  this->link->GetWorld()->GetSphericalCoordinates()->SphericalFromLocal(
199  cartVec);
200 #endif
201  this->outputMsg.latitude = scVec.X();
202  this->outputMsg.longitude = scVec.Y();
203  this->outputMsg.depth = -1 * scVec.Z();
204 
205  // Store this measurement's time stamp
206  this->lastUpdateTimestamp = _msg->header.stamp;
207 
208  double currentTime = _msg->header.stamp.toSec();
209 
210  double initSmoothingLength = std::pow(this->smoothingLength, 2.0 / 3);
211  ignition::math::Vector3d particle;
212  for (int i = 0; i < _msg->points.size(); i++)
213  {
214  particle = ignition::math::Vector3d(_msg->points[i].x, _msg->points[i].y,
215  _msg->points[i].z);
216 
217  smoothingParam = std::pow(initSmoothingLength +
218  this->gamma * (currentTime - _msg->channels[0].values[i]), 1.5);
219  distToParticle = linkPos.Distance(particle);
220 
221  // Compute particle concentration
222  if (distToParticle >= 0 && distToParticle < smoothingParam)
223  particleConc = 4.0 -
224  6.0 * std::pow(distToParticle / smoothingParam, 2) +
225  3.0 * std::pow(distToParticle / smoothingParam, 3);
226  else if (distToParticle >= smoothingParam && distToParticle < 2 * smoothingParam)
227  particleConc = std::pow(2 - distToParticle / smoothingParam, 3);
228  else
229  particleConc = 0.0;
230 
231  particleConc *= 1 / (4 * M_PI * std::pow(smoothingParam, 3));
232  totalParticleConc += particleConc;
233  }
234 
235  this->outputMsg.concentration = this->gain * totalParticleConc;
236  this->updatingCloud = false;
237  }
238 }
239 
242 }
bool updatingCloud
Flag to ensure the cloud and measurement update don&#39;t coincide.
Definition: CPCROSPlugin.hh:54
virtual ~CPCROSPlugin()
Class destructor.
Definition: CPCROSPlugin.cc:25
uuv_sensor_ros_plugins_msgs::ChemicalParticleConcentration outputMsg
Output measurement topic.
Definition: CPCROSPlugin.hh:71
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...
ignition::math::Pose3d referenceFrame
Pose of the reference frame wrt world frame.
CPCROSPlugin()
Class constructor.
Definition: CPCROSPlugin.cc:21
physics::WorldPtr world
Pointer to the world.
ros::Subscriber particlesSub
Input topic for the plume particle point cloud.
Definition: CPCROSPlugin.hh:48
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
ros::Time lastUpdateTimestamp
Last update from the point cloud callback.
Definition: CPCROSPlugin.hh:67
double noiseAmp
Noise amplitude.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
Definition: CPCROSPlugin.cc:29
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
virtual void OnPlumeParticlesUpdate(const sensor_msgs::PointCloud::ConstPtr &_msg)
Update callback from simulator.
double noiseSigma
Noise standard deviation.
std::string referenceFrameID
Frame ID of the reference frame.
uuv_sensor_ros_plugins_msgs::Salinity salinityMsg
Output salinity measurement message.
Definition: CPCROSPlugin.hh:74
physics::LinkPtr link
Pointer to the link.
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
uint32_t getNumSubscribers() const
ros::Publisher rosSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
ros::Publisher salinityPub
Output topic for salinity measurements based on the particle concentration.
Definition: CPCROSPlugin.hh:51
static Time now()
double gain
Sensor gain.
Definition: CPCROSPlugin.hh:60
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
void PublishState()
Publish the current state of the plugin.
double gamma
Gamma velocity parameter for the smoothing function.
Definition: CPCROSPlugin.hh:57


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