ROSBasePlugin.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  this->gazeboMsgEnabled = true;
24  this->referenceFrame = ignition::math::Pose3d::Zero;
25  this->referenceFrameID = "world";
26  this->isReferenceInit = false;
27  this->isOn.data = true;
28  this->world = NULL;
29  this->referenceLink = NULL;
30 
31  // Set seed for the noise generator
32  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
33  this->rndGen = std::default_random_engine(seed);
34 }
35 
38 {
39  if (this->rosNode)
40  this->rosNode->shutdown();
41  if (this->updateConnection)
42  this->updateConnection.reset();
43 }
44 
46 bool ROSBasePlugin::InitBasePlugin(sdf::ElementPtr _sdf)
47 {
48  GZ_ASSERT(this->world != NULL, "World object not available");
49  // Get the robot namespace
50  GetSDFParam<std::string>(_sdf, "robot_namespace", this->robotNamespace, "");
51  GZ_ASSERT(!this->robotNamespace.empty(), "Robot namespace was not provided");
52 
53  // Read separately in case a default topic name is given
54  std::string sensorTopic;
55  GetSDFParam<std::string>(_sdf, "sensor_topic", sensorTopic, "");
56  if (!sensorTopic.empty())
57  this->sensorOutputTopic = sensorTopic;
58  GZ_ASSERT(!this->sensorOutputTopic.empty(),
59  "Sensor output topic has not been provided");
60 
61  // Get the update rate
62  GetSDFParam<double>(_sdf, "update_rate", this->updateRate, 30.0);
63 
64  // Get flag to enable generation of Gazebo messages
65  GetSDFParam<bool>(_sdf, "enable_gazebo_messages", this->gazeboMsgEnabled,
66  true);
67 
68  // If output Gazebo messages have been enabled, create a Gazebo node
69  this->gazeboNode = transport::NodePtr(new transport::Node());
70  this->gazeboNode->Init(this->robotNamespace);
71 
72  // Create ROS node
73  if (!ros::isInitialized())
74  {
75  gzerr << "Not loading sensor plugin since ROS has not been properly "
76  << "initialized." << std::endl;
77  return false;
78  }
79 
80  this->rosNode.reset(new ros::NodeHandle(this->robotNamespace));
81 
82  // Initialize reference frame
83  if (_sdf->HasElement("static_reference_frame"))
84  {
85  GetSDFParam<std::string>(_sdf, "static_reference_frame",
86  this->referenceFrameID, "world");
87  gzmsg << "Static reference frame=" << this->referenceFrameID << std::endl;
88  this->referenceLink = NULL;
89  // In case the reference frame provide is different from world, then
90  // subscribe to the tf_static topic to acquire the pose of the reference
91  // frame.
92  if (this->referenceFrameID.compare("world") != 0)
93  {
94  this->tfStaticSub = this->rosNode->subscribe<tf::tfMessage>(
95  "/tf_static", 1,
96  boost::bind(&ROSBasePlugin::GetTFMessage, this, _1));
97  }
98  else
99  this->isReferenceInit = true;
100  }
101  else if (_sdf->HasElement("reference_link_name"))
102  {
103  GZ_ASSERT(this->referenceLink != NULL,
104  "Reference link has not been initialized");
105  }
106  else
107  {
108  this->referenceFrameID = "world";
109  this->referenceLink = NULL;
110  this->isReferenceInit = true;
111  }
112 
113  // Acquire current simulation time
114 #if GAZEBO_MAJOR_VERSION >= 8
115  this->lastMeasurementTime = this->world->SimTime();
116 #else
117  this->lastMeasurementTime = this->world->GetSimTime();
118 #endif
119 
120  // Initialize the switchable functionality of the sensor
121  bool isSensorOn;
122  GetSDFParam<bool>(_sdf, "is_on", isSensorOn, true);
123  this->isOn.data = isSensorOn;
124 
125  // The ROS node is expected to be initialized under a the namespace of the
126  // plugin running this module
127  this->changeSensorSrv = this->rosNode->advertiseService(
128  this->sensorOutputTopic + "/change_state",
130 
131  this->pluginStatePub = this->rosNode->advertise<std_msgs::Bool>(
132  this->sensorOutputTopic + "/state", 1);
133 
134  GetSDFParam<double>(_sdf, "noise_sigma", this->noiseSigma, 0.0);
135  GZ_ASSERT(this->noiseSigma >= 0.0,
136  "Signal noise sigma must be greater or equal to zero");
137 
138  GetSDFParam<double>(_sdf, "noise_amplitude", this->noiseAmp, 0.0);
139  GZ_ASSERT(this->noiseAmp >= 0.0,
140  "Signal noise amplitude must be greater or equal to zero");
141 
142  // Add a default Gaussian noise model
143  this->AddNoiseModel("default", this->noiseSigma);
144 }
145 
147 void ROSBasePlugin::GetTFMessage(const tf::tfMessage::ConstPtr &_msg)
148 {
149  if (this->isReferenceInit)
150  return;
151 
152  if (_msg->transforms.size() > 0)
153  {
154  for (auto t : _msg->transforms)
155  {
156  if (!t.header.frame_id.compare("world") &&
157  !t.child_frame_id.compare(this->referenceFrameID))
158  {
159  this->referenceFrame.Pos() = ignition::math::Vector3d(
160  t.transform.translation.x,
161  t.transform.translation.y,
162  t.transform.translation.z);
163 
164  this->referenceFrame.Rot() = ignition::math::Quaterniond(
165  t.transform.rotation.w,
166  t.transform.rotation.x,
167  t.transform.rotation.y,
168  t.transform.rotation.z);
169  this->isReferenceInit = true;
170  }
171  }
172  }
173 }
174 
177  uuv_sensor_ros_plugins_msgs::ChangeSensorState::Request& _req,
178  uuv_sensor_ros_plugins_msgs::ChangeSensorState::Response& _res)
179 {
180  this->isOn.data = _req.on;
181  _res.success = true;
182  std::string message = this->sensorOutputTopic + "::";
183 
184  if (_req.on)
185  message += " ON";
186  else
187  message += " OFF";
188  _res.message = message;
189  gzmsg << message << std::endl;
190  return true;
191 }
192 
195 {
196  this->pluginStatePub.publish(this->isOn);
197 }
198 
201 {
202  return _amp * this->noiseModels["default"](this->rndGen);
203 }
204 
206 double ROSBasePlugin::GetGaussianNoise(std::string _name, double _amp)
207 {
208  GZ_ASSERT(this->noiseModels.count(_name),
209  "Gaussian noise model does not exist");
210  return _amp * this->noiseModels[_name](this->rndGen);
211 }
212 
214 bool ROSBasePlugin::AddNoiseModel(std::string _name, double _sigma)
215 {
216  // Check if noise model name already exists
217  if (this->noiseModels.count(_name))
218  return false;
219 
220  this->noiseModels[_name] = std::normal_distribution<double>(0.0, _sigma);
221  return true;
222 }
223 
226 {
227  return this->isOn.data;
228 }
229 
231 bool ROSBasePlugin::EnableMeasurement(const common::UpdateInfo& _info) const
232 {
233  common::Time current_time = _info.simTime;
234  double dt = (current_time - this->lastMeasurementTime).Double();
235  return dt >= 1.0 / this->updateRate && this->isReferenceInit &&
236  this->isOn.data;
237 }
238 
241 {
242  // Read the pose of the reference frame if it was given as a Gazebo link
243  if (this->referenceLink)
244  {
245 #if GAZEBO_MAJOR_VERSION >= 8
246  this->referenceFrame = this->referenceLink->WorldPose();
247 #else
248  this->referenceFrame = this->referenceLink->GetWorldPose().Ign();
249 #endif
250  }
251 }
252 
253 }
physics::LinkPtr referenceLink
Reference link.
double updateRate
Sensor update rate.
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.
physics::WorldPtr world
Pointer to the world.
ROSCPP_DECL bool isInitialized()
ROSBasePlugin()
Class constructor.
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
double noiseAmp
Noise amplitude.
std::map< std::string, std::normal_distribution< double > > noiseModels
Normal distribution describing the noise models.
event::ConnectionPtr updateConnection
Pointer to the update event connection.
bool ChangeSensorState(uuv_sensor_ros_plugins_msgs::ChangeSensorState::Request &_req, uuv_sensor_ros_plugins_msgs::ChangeSensorState::Response &_res)
Change sensor state (ON/OFF)
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.
bool InitBasePlugin(sdf::ElementPtr _sdf)
Initialize base plugin.
double noiseSigma
Noise standard deviation.
std_msgs::Bool isOn
Flag to control the generation of output messages.
std::string referenceFrameID
Frame ID of the reference frame.
virtual ~ROSBasePlugin()
Class destructor.
ros::ServiceServer changeSensorSrv
Service server object.
std::default_random_engine rndGen
Pseudo random number generator.
std::string sensorOutputTopic
Name of the sensor&#39;s output topic.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
ros::Publisher pluginStatePub
ROS publisher for the switchable sensor data.
std::string robotNamespace
Robot namespace.
bool isReferenceInit
Flag set to true if reference frame initialized.
ros::Subscriber tfStaticSub
ROS subscriber for the TF static reference frame.
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.
bool IsOn()
Returns true if the plugin is activated.
void GetTFMessage(const tf::tfMessage::ConstPtr &_msg)
Callback function for the static TF message.


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