DVLROSPlugin.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->beamTransformsInitialized = false;
24 }
25 
28 { }
29 
31 void DVLROSPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
32 {
33  ROSBaseModelPlugin::Load(_model, _sdf);
34 
35  // Load the link names for all the beams
36  std::string beamLinkName;
37  GetSDFParam<std::string>(_sdf, "beam_link_name_0", beamLinkName, "");
38  GZ_ASSERT(!beamLinkName.empty(), "Beam 0 link name empty");
39  this->beamsLinkNames.push_back(beamLinkName);
40 
41  GetSDFParam<std::string>(_sdf, "beam_link_name_1", beamLinkName, "");
42  GZ_ASSERT(!beamLinkName.empty(), "Beam 1 link name empty");
43  this->beamsLinkNames.push_back(beamLinkName);
44 
45  GetSDFParam<std::string>(_sdf, "beam_link_name_2", beamLinkName, "");
46  GZ_ASSERT(!beamLinkName.empty(), "Beam 2 link name empty");
47  this->beamsLinkNames.push_back(beamLinkName);
48 
49  GetSDFParam<std::string>(_sdf, "beam_link_name_3", beamLinkName, "");
50  GZ_ASSERT(!beamLinkName.empty(), "Beam 3 link name empty");
51  this->beamsLinkNames.push_back(beamLinkName);
52 
53  // Load the beam output topic names
54  std::string beamTopic;
55  GetSDFParam<std::string>(_sdf, "beam_topic_0", beamTopic, "");
56  GZ_ASSERT(!beamTopic.empty(), "Beam 0 topic name empty");
57  this->beamTopics.push_back(beamTopic);
58 
59  GetSDFParam<std::string>(_sdf, "beam_topic_1", beamTopic, "");
60  GZ_ASSERT(!beamTopic.empty(), "Beam 1 topic name empty");
61  this->beamTopics.push_back(beamTopic);
62 
63  GetSDFParam<std::string>(_sdf, "beam_topic_2", beamTopic, "");
64  GZ_ASSERT(!beamTopic.empty(), "Beam 2 topic name empty");
65  this->beamTopics.push_back(beamTopic);
66 
67  GetSDFParam<std::string>(_sdf, "beam_topic_3", beamTopic, "");
68  GZ_ASSERT(!beamTopic.empty(), "Beam 3 topic name empty");
69  this->beamTopics.push_back(beamTopic);
70 
71  // Create beam subscribers
73  *this->rosNode.get(), this->beamTopics[0], 1));
75  *this->rosNode.get(), this->beamTopics[1], 1));
77  *this->rosNode.get(), this->beamTopics[2], 1));
79  *this->rosNode.get(), this->beamTopics[3], 1));
80 
81  for (int i = 0; i < 4; i++)
82  this->dvlBeamMsgs.push_back(uuv_sensor_ros_plugins_msgs::DVLBeam());
83 
84  // Synchronize the beam topics
86  sensor_msgs::Range, sensor_msgs::Range,
87  sensor_msgs::Range, sensor_msgs::Range>(
88  *this->beamSub0.get(), *this->beamSub1.get(), *this->beamSub2.get(),
89  *this->beamSub3.get(), 10));
90 
91  // Set synchronized callback function for the DVL beams
92  this->syncBeamMessages->registerCallback(
93  boost::bind(&DVLROSPlugin::OnBeamCallback, this, _1, _2, _3, _4));
94 
95  // Initialize the default DVL output
96  this->rosSensorOutputPub =
97  this->rosNode->advertise<uuv_sensor_ros_plugins_msgs::DVL>(
98  this->sensorOutputTopic, 1);
99 
100  this->twistPub =
101  this->rosNode->advertise<geometry_msgs::TwistWithCovarianceStamped>(
102  this->sensorOutputTopic + "_twist", 1);
103 
104  // Initialize ROS messages headers
105  if (this->enableLocalNEDFrame)
106  {
107  // Use the local NED frame format
108  this->dvlROSMsg.header.frame_id = this->tfLocalNEDFrame.child_frame_id_;
109  this->twistROSMsg.header.frame_id = this->tfLocalNEDFrame.child_frame_id_;
110  }
111  else
112  {
113  // Use the link's frame ID
114  this->dvlROSMsg.header.frame_id = this->link->GetName();
115  this->twistROSMsg.header.frame_id = this->link->GetName();
116  }
117 
118  double variance = this->noiseSigma * this->noiseSigma;
119 
120  // Set covariance
121  for (int i = 0; i < 9; i++)
122  this->dvlROSMsg.velocity_covariance[i] = 0.0;
123 
124  this->dvlROSMsg.velocity_covariance[0] = variance;
125  this->dvlROSMsg.velocity_covariance[4] = variance;
126  this->dvlROSMsg.velocity_covariance[8] = variance;
127 
128  for (int i = 0; i < 36; i++)
129  this->twistROSMsg.twist.covariance[i] = 0.0;
130 
131  this->twistROSMsg.twist.covariance[0] = variance;
132  this->twistROSMsg.twist.covariance[7] = variance;
133  this->twistROSMsg.twist.covariance[14] = variance;
134  this->twistROSMsg.twist.covariance[21] = -1; // not available
135  this->twistROSMsg.twist.covariance[28] = -1; // not available
136  this->twistROSMsg.twist.covariance[35] = -1; // not available
137 
138  if (this->gazeboMsgEnabled)
139  {
140  this->gazeboSensorOutputPub =
141  this->gazeboNode->Advertise<sensor_msgs::msgs::Dvl>(
142  this->robotNamespace + "/" + this->sensorOutputTopic, 1);
143  }
144 }
145 
147 bool DVLROSPlugin::OnUpdate(const common::UpdateInfo& _info)
148 {
149  // Publish sensor state
150  this->PublishState();
151 
152  if (!this->EnableMeasurement(_info))
153  return false;
154 
155  if (this->enableLocalNEDFrame)
156  this->SendLocalNEDTransform();
157 
158  ignition::math::Vector3d bodyVel;
159 
160  if (!this->UpdateBeamTransforms())
161  return false;
162 
163  // Read true body velocity
164  // TODO Temporary solution to generate DVL message, use beams in the future
165  // instead
166 #if GAZEBO_MAJOR_VERSION >= 8
167  bodyVel = this->link->RelativeLinearVel();
168 #else
169  bodyVel = this->link->GetRelativeLinearVel().Ign();
170 #endif
171 
172  bodyVel.X() += this->GetGaussianNoise(this->noiseAmp);
173  bodyVel.Y() += this->GetGaussianNoise(this->noiseAmp);
174  bodyVel.Z() += this->GetGaussianNoise(this->noiseAmp);
175 
176  if (this->enableLocalNEDFrame)
177  bodyVel = this->localNEDFrame.Rot().RotateVector(bodyVel);
178 
179  if (this->gazeboMsgEnabled)
180  {
181  sensor_msgs::msgs::Dvl dvlGazeboMsg;
182  double variance = this->noiseSigma * this->noiseSigma;
183 
184  for (int i = 0; i < 9; i++)
185  {
186  if (i == 0 || i == 4 || i == 8)
187  dvlGazeboMsg.add_linear_velocity_covariance(variance);
188  else
189  dvlGazeboMsg.add_linear_velocity_covariance(0.0);
190  }
191 
192  // Publish simulated measurement
193  gazebo::msgs::Vector3d* v = new gazebo::msgs::Vector3d();
194  v->set_x(bodyVel.X());
195  v->set_y(bodyVel.Y());
196  v->set_z(bodyVel.Z());
197  dvlGazeboMsg.set_allocated_linear_velocity(v);
198  this->gazeboSensorOutputPub->Publish(dvlGazeboMsg);
199  }
200 
201  // Publish ROS DVL message
202  this->dvlROSMsg.header.stamp.sec = _info.simTime.sec;
203  this->dvlROSMsg.header.stamp.nsec = _info.simTime.nsec;
204 
205  this->dvlROSMsg.altitude = this->altitude;
206 
207  this->dvlROSMsg.beams = this->dvlBeamMsgs;
208 
209  this->dvlROSMsg.velocity.x = bodyVel.X();
210  this->dvlROSMsg.velocity.y = bodyVel.Y();
211  this->dvlROSMsg.velocity.z = bodyVel.Z();
212  this->rosSensorOutputPub.publish(this->dvlROSMsg);
213 
214  this->twistROSMsg.header.stamp = this->dvlROSMsg.header.stamp;
215 
216  this->twistROSMsg.twist.twist.linear.x = bodyVel.X();
217  this->twistROSMsg.twist.twist.linear.y = bodyVel.Y();
218  this->twistROSMsg.twist.twist.linear.z = bodyVel.Z();
219 
220  this->twistPub.publish(this->twistROSMsg);
221 
222  // Read the current simulation time
223  #if GAZEBO_MAJOR_VERSION >= 8
224  this->lastMeasurementTime = this->world->SimTime();
225  #else
226  this->lastMeasurementTime = this->world->GetSimTime();
227  #endif
228  return true;
229 }
230 
232 void DVLROSPlugin::OnBeamCallback(const sensor_msgs::RangeConstPtr& _range0,
233  const sensor_msgs::RangeConstPtr& _range1,
234  const sensor_msgs::RangeConstPtr& _range2,
235  const sensor_msgs::RangeConstPtr& _range3)
236 {
237  if (_range0->range == _range0->min_range &&
238  _range1->range == _range1->min_range &&
239  _range2->range == _range2->min_range &&
240  _range3->range == _range3->min_range)
241  {
243  return;
244  }
245 
246 
247  if (_range0->range == _range0->max_range &&
248  _range1->range == _range1->max_range &&
249  _range2->range == _range2->max_range &&
250  _range3->range == _range3->max_range)
251  {
253  return;
254  }
255 
256  // TODO Compute the altitude taking into account the vehicle's orientation
257  this->altitude =
258  0.25 * (_range0->range + _range1->range + _range2->range + _range3->range);
259 
260  this->dvlBeamMsgs[0].range = _range0->range;
261  this->dvlBeamMsgs[1].range = _range1->range;
262  this->dvlBeamMsgs[2].range = _range2->range;
263  this->dvlBeamMsgs[3].range = _range3->range;
264 }
265 
268 {
269  if (this->beamPoses.size() == 4)
270  return true;
271 
272  tf::StampedTransform beamTransform;
273  std::string targetFrame, sourceFrame;
274  bool success = true;
275 
276  for (int i = 0; i < this->beamsLinkNames.size(); i++)
277  {
278  sourceFrame = this->beamsLinkNames[i];
279  if (!this->enableLocalNEDFrame)
280  targetFrame = this->link->GetName();
281  else
282  targetFrame = tfLocalNEDFrame.child_frame_id_;
283  try
284  {
285  ros::Time now = ros::Time::now();
287  targetFrame, sourceFrame, ros::Time(0),
288  beamTransform);
289  }
290  catch(tf::TransformException &ex)
291  {
292  success = false;
293  break;
294  }
295 
296  ignition::math::Pose3d pose;
297  pose.Pos() = ignition::math::Vector3d(
298  beamTransform.getOrigin().x(),
299  beamTransform.getOrigin().y(),
300  beamTransform.getOrigin().z());
301  pose.Rot() = ignition::math::Quaterniond(
302  beamTransform.getRotation().getW(),
303  beamTransform.getRotation().getAxis().x(),
304  beamTransform.getRotation().getAxis().y(),
305  beamTransform.getRotation().getAxis().z());
306 
307  this->dvlBeamMsgs[i].pose = geometry_msgs::PoseStamped();
308  this->dvlBeamMsgs[i].pose.header.stamp = ros::Time::now();
309  this->dvlBeamMsgs[i].pose.header.frame_id = sourceFrame;
310 
311  this->dvlBeamMsgs[i].pose.pose.position.x = beamTransform.getOrigin().x();
312  this->dvlBeamMsgs[i].pose.pose.position.y = beamTransform.getOrigin().y();
313  this->dvlBeamMsgs[i].pose.pose.position.z = beamTransform.getOrigin().z();
314 
315  this->dvlBeamMsgs[i].pose.pose.orientation.x = beamTransform.getRotation().getAxis().x();
316  this->dvlBeamMsgs[i].pose.pose.orientation.y = beamTransform.getRotation().getAxis().y();
317  this->dvlBeamMsgs[i].pose.pose.orientation.z = beamTransform.getRotation().getAxis().z();
318  this->dvlBeamMsgs[i].pose.pose.orientation.w = beamTransform.getRotation().getW();
319 
320  this->beamPoses.push_back(pose);
321  }
322  return success;
323 }
324 
327 }
uuv_sensor_ros_plugins_msgs::DVL dvlROSMsg
ROS DVL message.
Definition: DVLROSPlugin.hh:68
std::vector< ignition::math::Pose3d > beamPoses
List of poses of each beam wrt to the DVL frame.
Definition: DVLROSPlugin.hh:85
std::vector< std::string > beamTopics
List of beam topics.
Definition: DVLROSPlugin.hh:82
bool UpdateBeamTransforms()
Updates the poses of each beam wrt the DVL frame.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
Definition: DVLROSPlugin.cc:31
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
tf::StampedTransform tfLocalNEDFrame
Local NED TF frame.
#define ALTITUDE_OUT_OF_RANGE
Definition: DVLROSPlugin.hh:35
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.
ros::Publisher twistPub
ROS publisher for twist data.
Definition: DVLROSPlugin.hh:73
common::Time lastMeasurementTime
(Simulation) time when the last sensor measurement was generated.
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::Range > > beamSub3
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::Range > > beamSub0
Definition: DVLROSPlugin.hh:92
std::vector< uuv_sensor_ros_plugins_msgs::DVLBeam > dvlBeamMsgs
Definition: DVLROSPlugin.hh:70
TFSIMD_FORCE_INLINE const tfScalar & getW() const
bool enableLocalNEDFrame
True if a the local NED frame needs to be broadcasted.
DVLROSPlugin()
Class constructor.
Definition: DVLROSPlugin.cc:21
void SendLocalNEDTransform()
Returns true if the base_link_ned frame exists.
double noiseAmp
Noise amplitude.
boost::shared_ptr< message_filters::TimeSynchronizer< sensor_msgs::Range, sensor_msgs::Range, sensor_msgs::Range, sensor_msgs::Range > > syncBeamMessages
Definition: DVLROSPlugin.hh:89
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
void OnBeamCallback(const sensor_msgs::RangeConstPtr &_range0, const sensor_msgs::RangeConstPtr &_range1, const sensor_msgs::RangeConstPtr &_range2, const sensor_msgs::RangeConstPtr &_range3)
Get beam Range message update.
TFSIMD_FORCE_INLINE const tfScalar & x() const
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::Range > > beamSub1
Definition: DVLROSPlugin.hh:95
std::string child_frame_id_
transport::NodePtr gazeboNode
Gazebo&#39;s node handle for transporting measurement messages.
TFSIMD_FORCE_INLINE const tfScalar & z() const
double noiseSigma
Noise standard deviation.
Vector3 getAxis() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
transport::PublisherPtr gazeboSensorOutputPub
Gazebo&#39;s publisher for transporting measurement messages.
ignition::math::Pose3d localNEDFrame
Pose of the local NED frame wrt link frame.
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
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.
Quaternion getRotation() const
tf::TransformListener transformListener
boost::shared_ptr< ros::NodeHandle > rosNode
ROS node handle for communication with ROS.
geometry_msgs::TwistWithCovarianceStamped twistROSMsg
Store pose message since many attributes do not change (cov.).
Definition: DVLROSPlugin.hh:76
virtual ~DVLROSPlugin()
Class destructor.
Definition: DVLROSPlugin.cc:27
boost::shared_ptr< message_filters::Subscriber< sensor_msgs::Range > > beamSub2
Definition: DVLROSPlugin.hh:98
static Time now()
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
std::string robotNamespace
Robot namespace.
virtual bool OnUpdate(const common::UpdateInfo &_info)
Update sensor measurement.
bool gazeboMsgEnabled
Flag set to true if the Gazebo sensors messages are supposed to be published as well (it can avoid un...
TODO: Modify computation of velocity using the beams.
Definition: DVLROSPlugin.hh:39
void PublishState()
Publish the current state of the plugin.
std::vector< std::string > beamsLinkNames
List of beam links.
Definition: DVLROSPlugin.hh:79
double altitude
Measured altitude in meters.
Definition: DVLROSPlugin.hh:65


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