RPTROSPlugin.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 RPTROSPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
30 {
31  ROSBaseModelPlugin::Load(_model, _sdf);
32 
33  double variance = this->noiseSigma * this->noiseSigma;
34  for (int i = 0; i < 9; i++)
35  this->rosMessage.pos.covariance[i] = 0;
36 
37  this->rosMessage.pos.covariance[0] = this->rosMessage.pos.covariance[4] =
38  this->rosMessage.pos.covariance[8] = variance;
39 
40  // Initialize the default RPT output
41  this->rosSensorOutputPub =
42  this->rosNode->advertise<
43  uuv_sensor_ros_plugins_msgs::PositionWithCovarianceStamped>(
44  this->sensorOutputTopic, 1);
45 
46  if (this->gazeboMsgEnabled)
47  {
48  this->gazeboSensorOutputPub =
49  this->gazeboNode->Advertise<sensor_msgs::msgs::Rpt>(
50  this->robotNamespace + "/" + this->sensorOutputTopic, 1);
51  }
52 }
53 
55 bool RPTROSPlugin::OnUpdate(const common::UpdateInfo& _info)
56 {
57  // Publish sensor state
58  this->PublishState();
59 
60  if (!this->EnableMeasurement(_info))
61  return false;
62 
63  // True position
64  // TODO This is a temporary implementation, next step includes making
65  // plugins for acoustic channels and beacons
66 #if GAZEBO_MAJOR_VERSION >= 8
67  this->position = this->link->WorldPose().Pos();
68 #else
69  this->position = this->link->GetWorldPose().Ign().Pos();
70 #endif
71 
73  if (this->referenceLink)
74  {
75 #if GAZEBO_MAJOR_VERSION >= 8
76  this->referenceFrame = this->referenceLink->WorldPose();
77 #else
78  this->referenceFrame = this->referenceLink->GetWorldPose().Ign();
79 #endif
80  }
81 
82  this->position = this->position - this->referenceFrame.Pos();
83  this->position = this->referenceFrame.Rot().RotateVectorReverse(
84  this->position);
85 
86  this->position.X() += this->GetGaussianNoise(this->noiseAmp);
87  this->position.Y() += this->GetGaussianNoise(this->noiseAmp);
88  this->position.Z() += this->GetGaussianNoise(this->noiseAmp);
89 
90  this->rosMessage.header.stamp = ros::Time::now();
91  this->rosMessage.header.frame_id = this->referenceFrameID;
92  this->rosMessage.pos.pos.x = this->position.X();
93  this->rosMessage.pos.pos.y = this->position.Y();
94  this->rosMessage.pos.pos.z = this->position.Z();
95 
97 
98  if (this->gazeboMsgEnabled)
99  {
100  sensor_msgs::msgs::Rpt gazeboMessage;
101  double variance = this->noiseSigma * this->noiseSigma;
102  // Prepare constant covariance part of message
103  for (int i = 0 ; i < 9; i++)
104  {
105  if (i == 0 || i == 4 || i == 8)
106  gazeboMessage.add_position_covariance(variance);
107  else
108  gazeboMessage.add_position_covariance(0.0);
109  }
110  // Publish simulated measurement
111  gazebo::msgs::Vector3d * p = new gazebo::msgs::Vector3d();
112  p->set_x(this->position.X());
113  p->set_y(this->position.Y());
114  p->set_z(this->position.Z());
115 
116  gazeboMessage.set_allocated_position(p);
117  this->gazeboSensorOutputPub->Publish(gazeboMessage);
118  }
119 }
120 
123 }
RPTROSPlugin()
Class constructor.
Definition: RPTROSPlugin.cc:21
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
Definition: RPTROSPlugin.cc:55
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...
ignition::math::Pose3d referenceFrame
Pose of the reference frame wrt world frame.
virtual ~RPTROSPlugin()
Class destructor.
Definition: RPTROSPlugin.cc:25
double noiseAmp
Noise amplitude.
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.
transport::NodePtr gazeboNode
Gazebo&#39;s node handle for transporting measurement messages.
double noiseSigma
Noise standard deviation.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
Definition: RPTROSPlugin.cc:29
uuv_sensor_ros_plugins_msgs::PositionWithCovarianceStamped rosMessage
Store message since many attributes do not change (cov.).
Definition: RPTROSPlugin.hh:46
transport::PublisherPtr gazeboSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
std::string referenceFrameID
Frame ID of the reference frame.
physics::LinkPtr link
Pointer to the link.
ignition::math::Vector3d position
Latest measured position.
Definition: RPTROSPlugin.hh:43
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
ros::Publisher rosSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
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...
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