PoseGTROSPlugin.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 gazebo_ros_pkgs
17 // (https://github.com/ros-simulation/gazebo_ros_pkgs)
18 // * Copyright 2012 Open Source Robotics Foundation,
19 // licensed under the Apache-2.0 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  this->offset.Pos() = ignition::math::Vector3d::Zero;
34  this->offset.Rot() = ignition::math::Quaterniond(
35  ignition::math::Vector3d(0, 0, 0));
36 
37  // Initialize the reference's velocity and acceleration vectors
38  this->refLinAcc = ignition::math::Vector3d::Zero;
39  this->refAngAcc = ignition::math::Vector3d::Zero;
40 
41  this->nedTransform = ignition::math::Pose3d::Zero;
42  this->nedTransformIsInit = true;
43 }
44 
47 { }
48 
50 void PoseGTROSPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
51 {
52  ROSBaseModelPlugin::Load(_model, _sdf);
53 
54  ignition::math::Vector3d vec;
55  GetSDFParam<ignition::math::Vector3d>(_sdf, "position_offset",
56  vec, ignition::math::Vector3d::Zero);
57  this->offset.Pos() = vec;
58  GetSDFParam<ignition::math::Vector3d>(_sdf, "orientation_offset", vec,
59  ignition::math::Vector3d::Zero);
60  this->offset.Rot() = ignition::math::Quaterniond(vec);
61 
62  GetSDFParam<bool>(_sdf, "publish_ned_odom", this->publishNEDOdom, false);
63 
64  if (this->publishNEDOdom)
65  {
66  this->nedFrameID = this->link->GetName() + "_ned";
67  this->nedOdomPub = this->rosNode->advertise<nav_msgs::Odometry>(
68  this->sensorOutputTopic + "_ned", 1);
69  this->nedTransformIsInit = false;
70  }
71 
72  // Initialize the reference's velocity and acceleration vectors
73  if (!this->referenceLink)
74  {
75  this->lastRefLinVel = ignition::math::Vector3d::Zero;
76  this->lastRefAngVel = ignition::math::Vector3d::Zero;
77  }
78  else
79  {
80 #if GAZEBO_MAJOR_VERSION >= 8
81  this->lastRefLinVel = this->referenceLink->WorldLinearVel();
82  this->lastRefAngVel = this->referenceLink->WorldAngularVel();
83 #else
84  this->lastRefLinVel = this->referenceLink->GetWorldLinearVel().Ign();
85  this->lastRefAngVel = this->referenceLink->GetWorldAngularVel().Ign();
86 #endif
87  }
88 
89  this->tfListener.reset(new tf2_ros::TransformListener(this->tfBuffer));
90 
91  this->rosSensorOutputPub = this->rosNode->advertise<nav_msgs::Odometry>(
92  this->sensorOutputTopic, 1);
93 }
94 
96 bool PoseGTROSPlugin::OnUpdate(const common::UpdateInfo& _info)
97 {
98  if (!this->EnableMeasurement(_info))
99  return false;
100 
101  // Read the current simulation time
102 #if GAZEBO_MAJOR_VERSION >= 8
103  common::Time curTime = this->world->SimTime();
104 #else
105  common::Time curTime = this->world->GetSimTime();
106 #endif
107 
108  double dt = curTime.Double() - this->lastMeasurementTime.Double();
109 
110  if (dt <= 0)
111  return false;
112 
113  ignition::math::Pose3d linkPose, refLinkPose;
114  ignition::math::Vector3d refLinVel, refAngVel;
115  ignition::math::Vector3d linkLinVel, linkAngVel;
116 
117  this->UpdateNEDTransform();
118  // Read sensor link's current pose and velocity
119 #if GAZEBO_MAJOR_VERSION >= 8
120  linkLinVel = this->link->WorldLinearVel();
121  linkAngVel = this->link->WorldAngularVel();
122 
123  linkPose = this->link->WorldPose();
124 #else
125  linkLinVel = this->link->GetWorldLinearVel().Ign();
126  linkAngVel = this->link->GetWorldAngularVel().Ign();
127 
128  linkPose = this->link->GetWorldPose().Ign();
129 #endif
130 
131  this->UpdateReferenceFramePose();
132 
133  // Update the reference frame in case it is given as a Gazebo link and
134  // read the reference link's linear and angular velocity vectors
135  if (this->referenceLink)
136  {
137 #if GAZEBO_MAJOR_VERSION >= 8
138  refLinVel = this->referenceLink->WorldLinearVel();
139  refAngVel = this->referenceLink->WorldAngularVel();
140 
141  this->referenceFrame = this->referenceLink->WorldPose();
142 #else
143  refLinVel = this->referenceLink->GetWorldLinearVel().Ign();
144  refAngVel = this->referenceLink->GetWorldAngularVel().Ign();
145 
146  this->referenceFrame = this->referenceLink->GetWorldPose().Ign();
147 #endif
148  }
149  else
150  {
151  // If no Gazebo link is given as a reference, the linear and angular
152  // velocity vectors are set to zero
153  refLinVel = ignition::math::Vector3d::Zero;
154  refAngVel = ignition::math::Vector3d::Zero;
155  }
156 
157  // Transform pose and velocity vectors to be represented wrt the
158  // reference link provided
159  linkLinVel -= refLinVel;
160  linkAngVel -= refAngVel;
161 
162  // Add noise to the link's linear velocity
163  linkLinVel += ignition::math::Vector3d(
164  this->GetGaussianNoise(this->noiseAmp),
165  this->GetGaussianNoise(this->noiseAmp),
166  this->GetGaussianNoise(this->noiseAmp));
167 
168  // Add noise to the link's angular velocity
169  linkAngVel += ignition::math::Vector3d(
170  this->GetGaussianNoise(this->noiseAmp),
171  this->GetGaussianNoise(this->noiseAmp),
172  this->GetGaussianNoise(this->noiseAmp));
173 
174  // Publish the odometry message of the base_link wrt Gazebo's ENU
175  // inertial reference frame
176  this->PublishOdomMessage(curTime, linkPose, linkLinVel, linkAngVel);
177  // If the world_ned frame exists (North-East-Down reference frame),
178  // the odometry is also published from the robot's base_link_ned wrt
179  // world_ned
180  this->PublishNEDOdomMessage(curTime, linkPose, linkLinVel, linkAngVel);
181 
182  // Store the time stamp for this measurement
183  this->lastMeasurementTime = curTime;
184  return true;
185 }
186 
187 void PoseGTROSPlugin::PublishOdomMessage(common::Time _time,
188  ignition::math::Pose3d _pose, ignition::math::Vector3d _linVel,
189  ignition::math::Vector3d _angVel)
190 {
191  // Generates the odometry message of the robot's base_link frame wrt
192  // Gazebo's default ENU inertial reference frame
193  nav_msgs::Odometry odomMsg;
194 
195  // Initialize header of the odometry message
196  odomMsg.header.frame_id = "world";
197  odomMsg.header.stamp.sec = _time.sec;
198  odomMsg.header.stamp.nsec = _time.nsec;
199  odomMsg.child_frame_id = this->link->GetName();
200 
201  // Apply pose offset
202  _pose += this->offset;
203 
204  // Fill out the messages
205  odomMsg.pose.pose.position.x = _pose.Pos().X();
206  odomMsg.pose.pose.position.y = _pose.Pos().Y();
207  odomMsg.pose.pose.position.z = _pose.Pos().Z();
208 
209  odomMsg.pose.pose.orientation.x = _pose.Rot().X();
210  odomMsg.pose.pose.orientation.y = _pose.Rot().Y();
211  odomMsg.pose.pose.orientation.z = _pose.Rot().Z();
212  odomMsg.pose.pose.orientation.w = _pose.Rot().W();
213 
214  odomMsg.twist.twist.linear.x = _linVel.X();
215  odomMsg.twist.twist.linear.y = _linVel.Y();
216  odomMsg.twist.twist.linear.z = _linVel.Z();
217 
218  odomMsg.twist.twist.angular.x = _angVel.X();
219  odomMsg.twist.twist.angular.y = _angVel.Y();
220  odomMsg.twist.twist.angular.z = _angVel.Z();
221 
222  // Fill in the covariance matrix
223  double gn2 = this->noiseSigma * this->noiseSigma;
224  odomMsg.pose.covariance[0] = gn2;
225  odomMsg.pose.covariance[7] = gn2;
226  odomMsg.pose.covariance[14] = gn2;
227  odomMsg.pose.covariance[21] = gn2;
228  odomMsg.pose.covariance[28] = gn2;
229  odomMsg.pose.covariance[35] = gn2;
230 
231  odomMsg.twist.covariance[0] = gn2;
232  odomMsg.twist.covariance[7] = gn2;
233  odomMsg.twist.covariance[14] = gn2;
234  odomMsg.twist.covariance[21] = gn2;
235  odomMsg.twist.covariance[28] = gn2;
236  odomMsg.twist.covariance[35] = gn2;
237 
238  this->rosSensorOutputPub.publish(odomMsg);
239 }
240 
242  ignition::math::Pose3d _pose, ignition::math::Vector3d _linVel,
243  ignition::math::Vector3d _angVel)
244 {
245  // Generates the odometry message of the robot's base_link_ned frame
246  // wrt generated NED inertial reference frame
247  if (!this->publishNEDOdom)
248  return;
249 
250  if (!this->nedTransformIsInit)
251  return;
252 
253  nav_msgs::Odometry odomMsg;
254 
255  // Initialize header of the odometry message
256  odomMsg.header.frame_id = this->referenceFrameID;
257  odomMsg.header.stamp.sec = _time.sec;
258  odomMsg.header.stamp.nsec = _time.nsec;
259  odomMsg.child_frame_id = this->nedFrameID;
260 
261  _pose.Pos() = _pose.Pos() - this->referenceFrame.Pos();
262  _pose.Pos() = this->referenceFrame.Rot().RotateVectorReverse(_pose.Pos());
263 
264  ignition::math::Quaterniond q = this->nedTransform.Rot();
265  q = _pose.Rot() * q;
266  q = this->referenceFrame.Rot() * q;
267  _pose.Rot() = q;
268 
269  _linVel = this->referenceFrame.Rot().RotateVector(_linVel);
270  _angVel = this->referenceFrame.Rot().RotateVector(_angVel);
271 
272  // Apply pose offset
273  _pose += this->offset;
274 
275  // Fill out the messages
276  odomMsg.pose.pose.position.x = _pose.Pos().X();
277  odomMsg.pose.pose.position.y = _pose.Pos().Y();
278  odomMsg.pose.pose.position.z = _pose.Pos().Z();
279 
280  odomMsg.pose.pose.orientation.x = _pose.Rot().X();
281  odomMsg.pose.pose.orientation.y = _pose.Rot().Y();
282  odomMsg.pose.pose.orientation.z = _pose.Rot().Z();
283  odomMsg.pose.pose.orientation.w = _pose.Rot().W();
284 
285  odomMsg.twist.twist.linear.x = _linVel.X();
286  odomMsg.twist.twist.linear.y = _linVel.Y();
287  odomMsg.twist.twist.linear.z = _linVel.Z();
288 
289  odomMsg.twist.twist.angular.x = _angVel.X();
290  odomMsg.twist.twist.angular.y = _angVel.Y();
291  odomMsg.twist.twist.angular.z = _angVel.Z();
292 
293  double gn2 = this->noiseSigma * this->noiseSigma;
294  odomMsg.pose.covariance[0] = gn2;
295  odomMsg.pose.covariance[7] = gn2;
296  odomMsg.pose.covariance[14] = gn2;
297  odomMsg.pose.covariance[21] = gn2;
298  odomMsg.pose.covariance[28] = gn2;
299  odomMsg.pose.covariance[35] = gn2;
300 
301  odomMsg.twist.covariance[0] = gn2;
302  odomMsg.twist.covariance[7] = gn2;
303  odomMsg.twist.covariance[14] = gn2;
304  odomMsg.twist.covariance[21] = gn2;
305  odomMsg.twist.covariance[28] = gn2;
306  odomMsg.twist.covariance[35] = gn2;
307 
308  this->nedOdomPub.publish(odomMsg);
309 }
310 
313 {
314  if (!this->publishNEDOdom)
315  return;
316  if (this->nedTransformIsInit)
317  return;
318 
319  geometry_msgs::TransformStamped childTransform;
320  std::string targetFrame = this->nedFrameID;
321  std::string sourceFrame = this->link->GetName();
322  try
323  {
324  childTransform = this->tfBuffer.lookupTransform(
325  targetFrame, sourceFrame, ros::Time(0));
326  }
327  catch(tf2::TransformException &ex)
328  {
329  gzmsg << "Transform between " << targetFrame << " and " << sourceFrame
330  << std::endl;
331  gzmsg << ex.what() << std::endl;
332  return;
333  }
334 
335  this->nedTransform.Pos() = ignition::math::Vector3d(
336  childTransform.transform.translation.x,
337  childTransform.transform.translation.y,
338  childTransform.transform.translation.z);
339  this->nedTransform.Rot() = ignition::math::Quaterniond(
340  childTransform.transform.rotation.w,
341  childTransform.transform.rotation.x,
342  childTransform.transform.rotation.y,
343  childTransform.transform.rotation.z);
344 
345  this->nedTransformIsInit = true;
346 }
347 
350 
351 }
ignition::math::Vector3d lastRefAngVel
ignition::math::Vector3d lastRefLinVel
physics::LinkPtr referenceLink
Reference link.
ignition::math::Pose3d nedTransform
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.
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ignition::math::Pose3d offset
Pose offset.
double noiseAmp
Noise amplitude.
~PoseGTROSPlugin()
Class destructor.
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
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.
tf2_ros::Buffer tfBuffer
double noiseSigma
Noise standard deviation.
std::string referenceFrameID
Frame ID of the reference frame.
void PublishOdomMessage(common::Time _time, ignition::math::Pose3d _pose, ignition::math::Vector3d _linVel, ignition::math::Vector3d _angVel)
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.
ignition::math::Vector3d refLinAcc
boost::shared_ptr< tf2_ros::TransformListener > tfListener
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
PoseGTROSPlugin()
Class constructor.
void PublishNEDOdomMessage(common::Time _time, ignition::math::Pose3d _pose, ignition::math::Vector3d _linVel, ignition::math::Vector3d _angVel)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
ignition::math::Vector3d refAngAcc


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