ROSBaseModelPlugin.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 {
20 
23 {
24  // Initialize local NED frame
25  this->localNEDFrame = ignition::math::Pose3d::Zero;
26  this->localNEDFrame.Rot() = ignition::math::Quaterniond(
27  ignition::math::Vector3d(M_PI, 0, 0));
28  // Initialize the local NED frame
29  this->tfLocalNEDFrame.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
31  tf::createQuaternionFromRPY(M_PI, 0.0, 0.0));
32  // Initialize TF broadcaster
34 }
35 
38 {
39  if (this->tfBroadcaster)
40  delete this->tfBroadcaster;
41 }
42 
44 void ROSBaseModelPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
45 {
46  // Initialize model pointer
47  this->model = _model;
48 
49  // Store world pointer
50  this->world = this->model->GetWorld();
51 
52  std::string linkName;
53  GZ_ASSERT(_sdf->HasElement("link_name"), "No link name provided");
54  GetSDFParam<std::string>(_sdf, "link_name", linkName, "");
55  GZ_ASSERT(!linkName.empty(), "Link name string is empty");
56 
57  // Get flag to enable generation of Gazebo messages
58  GetSDFParam<bool>(_sdf, "enable_local_ned_frame", this->enableLocalNEDFrame,
59  true);
60 
61  if (_sdf->HasElement("reference_link_name"))
62  {
63  std::string refLinkName;
64  GetSDFParam<std::string>(_sdf, "reference_link_name", refLinkName, "");
65  if (!refLinkName.empty())
66  {
67  this->referenceLink = this->model->GetLink(refLinkName);
68  GZ_ASSERT(this->referenceLink != NULL, "Invalid reference link");
69  this->referenceFrameID = refLinkName;
70  }
71  }
72 
73  // Get sensor link
74  this->link = this->model->GetLink(linkName);
75  GZ_ASSERT(this->link != NULL, "Invalid link pointer");
76 
77  // Set the frame IDs for the local NED frame
78  this->tfLocalNEDFrame.frame_id_ = this->link->GetName();
79  this->tfLocalNEDFrame.child_frame_id_ = this->link->GetName() + "_ned";
80 
81  this->InitBasePlugin(_sdf);
82 
83  // Bind the sensor update callback function to the world update event
84  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
85  boost::bind(&ROSBasePlugin::OnUpdate, this, _1));
86 }
87 
89 bool ROSBaseModelPlugin::OnUpdate(const common::UpdateInfo&)
90 {
91  return true;
92 }
93 
96 {
97  geometry_msgs::TransformStamped msg;
100  this->tfBroadcaster->sendTransform(msg);
101 }
102 
103 }
msg
physics::LinkPtr referenceLink
Reference link.
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf::StampedTransform tfLocalNEDFrame
Local NED TF frame.
physics::WorldPtr world
Pointer to the world.
bool enableLocalNEDFrame
True if a the local NED frame needs to be broadcasted.
void SendLocalNEDTransform()
Returns true if the base_link_ned frame exists.
event::ConnectionPtr updateConnection
Pointer to the update event connection.
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf,.
std::string child_frame_id_
virtual bool OnUpdate(const common::UpdateInfo &)=0
Update callback from simulation.
bool InitBasePlugin(sdf::ElementPtr _sdf)
Initialize base plugin.
void sendTransform(const StampedTransform &transform)
std::string referenceFrameID
Frame ID of the reference frame.
ignition::math::Pose3d localNEDFrame
Pose of the local NED frame wrt link frame.
physics::LinkPtr link
Pointer to the link.
ROSBaseModelPlugin()
Class constructor.
physics::ModelPtr model
Pointer to the model.
tf::TransformBroadcaster * tfBroadcaster
TF broadcaster for the local NED frame.
static Time now()
virtual bool OnUpdate(const common::UpdateInfo &)
Update callback from simulation.
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
virtual ~ROSBaseModelPlugin()
Class destructor.
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
std::string frame_id_


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