FinPlugin.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 
18 
19 #include <gazebo/gazebo.hh>
20 #include <gazebo/physics/Link.hh>
21 #include <gazebo/physics/Model.hh>
22 #include <gazebo/physics/World.hh>
23 
24 
26 
27 namespace gazebo {
28 
30 FinPlugin::FinPlugin() : inputCommand(0), angle(0), finID(-1)
31 {
32 }
33 
36 {
37  if (this->updateConnection)
38  {
39 #if GAZEBO_MAJOR_VERSION >= 8
40  this->updateConnection.reset();
41 #else
42  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
43 #endif
44  }
45 }
46 
48 void FinPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
49 {
50  // Initializing the transport node
51  this->node = transport::NodePtr(new transport::Node());
52 #if GAZEBO_MAJOR_VERSION >= 8
53  this->node->Init(_model->GetWorld()->Name());
54 #else
55  this->node->Init(_model->GetWorld()->GetName());
56 #endif
57 
58  // Fin ID
59  GZ_ASSERT(_sdf->HasElement("fin_id"), "Could not find fin_id parameter.");
60  this->finID = _sdf->Get<int>("fin_id");
61  GZ_ASSERT(this->finID >= 0, "Fin ID must be greater or equal than zero");
62 
63  // Root string for topics
64  std::stringstream strs;
65  strs << "/" << _model->GetName() << "/fins/" << this->finID << "/";
66  this->topicPrefix = strs.str();
67 
68  // Input/output topics
69  std::string inputTopic, outputTopic;
70  if (_sdf->HasElement("input_topic"))
71  std::string inputTopic = _sdf->Get<std::string>("input_topic");
72  else
73  inputTopic = this->topicPrefix + "input";
74 
75  if (_sdf->HasElement("output_topic"))
76  outputTopic = _sdf->Get<std::string>("output_topic");
77  else
78  outputTopic = this->topicPrefix + "output";
79 
80  GZ_ASSERT(_sdf->HasElement("link_name"), "Could not find link_name.");
81  std::string link_name = _sdf->Get<std::string>("link_name");
82  this->link = _model->GetLink(link_name);
83  GZ_ASSERT(this->link, "link is invalid");
84 
85  GZ_ASSERT(_sdf->HasElement("joint_name"), "Could not find joint_name.");
86  std::string joint_name = _sdf->Get<std::string>("joint_name");
87  this->joint = _model->GetJoint(joint_name);
88  GZ_ASSERT(this->joint, "joint is invalid");
89 
90  // Dynamic model
91  GZ_ASSERT(_sdf->HasElement("dynamics"), "Could not find dynamics.");
92  this->dynamics.reset(DynamicsFactory::GetInstance().CreateDynamics(
93  _sdf->GetElement("dynamics")));
94 
95  // Lift and drag model
96  GZ_ASSERT(_sdf->HasElement("liftdrag"), "Could not find liftdrag");
97  this->liftdrag.reset(
98  LiftDragFactory::GetInstance().CreateLiftDrag(
99  _sdf->GetElement("liftdrag")));
100 
101  // Subscribe to current velocity topic
102  GZ_ASSERT(_sdf->HasElement("current_velocity_topic"),
103  "Could not find current_velocity_topic.");
104  std::string currentVelocityTopic =
105  _sdf->Get<std::string>("current_velocity_topic");
106 
107  GZ_ASSERT(!currentVelocityTopic.empty(),
108  "Fluid velocity topic tag cannot be empty");
109 
110  gzmsg << "Subscribing to current velocity topic: " << currentVelocityTopic
111  << std::endl;
112  this->currentSubscriber = this->node->Subscribe(currentVelocityTopic,
114 
115  // Advertise the output topic
116  this->anglePublisher = this->node->Advertise<
117  uuv_gazebo_plugins_msgs::msgs::Double>(outputTopic);
118 
119 
120  // Subscribe to the input signal topic
121  this->commandSubscriber = this->node->Subscribe(inputTopic,
123  this);
124 
125  // Connect the update event
126  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
127  boost::bind(&FinPlugin::OnUpdate,
128  this, _1));
129 }
130 
133 {
134 }
135 
137 void FinPlugin::OnUpdate(const common::UpdateInfo &_info)
138 {
139  GZ_ASSERT(!std::isnan(this->inputCommand),
140  "nan in this->inputCommand");
141 
142  double upperLimit, lowerLimit;
143 #if GAZEBO_MAJOR_VERSION >= 8
144  upperLimit = this->joint->UpperLimit(0);
145  lowerLimit = this->joint->LowerLimit(0);
146 #else
147  upperLimit = this->joint->GetUpperLimit(0).Radian();
148  lowerLimit = this->joint->GetLowerLimit(0).Radian();
149 #endif
150  // Limit the input command using the fin joint limits
151  this->inputCommand = std::min(upperLimit, this->inputCommand);
152  this->inputCommand = std::max(lowerLimit, this->inputCommand);
153 
154  // Update dynamics model:
155  this->angle = this->dynamics->update(this->inputCommand,
156  _info.simTime.Double());
157 
158  // Determine velocity in lift/drag plane:
159  ignition::math::Pose3d finPose;
160  ignition::math::Vector3d linVel;
161 #if GAZEBO_MAJOR_VERSION >= 8
162  finPose = this->link->WorldPose();
163  linVel = this->link->WorldLinearVel();
164 #else
165  finPose = this->link->GetWorldPose().Ign();
166  linVel = this->link->GetWorldLinearVel().Ign();
167 #endif
168 
169  ignition::math::Vector3d ldNormalI = finPose.Rot().RotateVector(
170  ignition::math::Vector3d::UnitZ);
171 
172  ignition::math::Vector3d velI = linVel - this->currentVelocity;
173  ignition::math::Vector3d velInLDPlaneI = ldNormalI.Cross(velI.Cross(ldNormalI));
174  ignition::math::Vector3d velInLDPlaneL = finPose.Rot().RotateVectorReverse(velInLDPlaneI);
175 
176  // Compute lift and drag forces:
177  this->finForce = this->liftdrag->compute(velInLDPlaneL);
178 
179  this->link->AddRelativeForce(this->finForce);
180  // Apply forces at cg (with torques for position shift).
181 
182  // Apply new fin angle. Do this last since this sets link's velocity to zero.
183  this->joint->SetPosition(0, this->angle);
184 
185  this->angleStamp = _info.simTime;
186 }
187 
190 {
191  this->inputCommand = _msg->value();
192 }
193 
195 void FinPlugin::UpdateCurrentVelocity(ConstVector3dPtr &_msg)
196 {
197  this->currentVelocity.X() = _msg->x();
198  this->currentVelocity.Y() = _msg->y();
199  this->currentVelocity.Z() = _msg->z();
200 }
201 }
transport::SubscriberPtr currentSubscriber
Subcriber to current message.
Definition: FinPlugin.hh:106
ignition::math::Vector3d finForce
Force component calculated from the lift and drag module.
Definition: FinPlugin.hh:88
double angle
Latest fin angle in [rad].
Definition: FinPlugin.hh:100
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Definition: FinPlugin.cc:48
double inputCommand
Latest input command.
Definition: FinPlugin.hh:91
const boost::shared_ptr< const uuv_gazebo_plugins_msgs::msgs::Double > ConstDoublePtr
Definition of a pointer to the floating point message.
Definition: FinPlugin.hh:36
physics::JointPtr joint
The fin joint.
Definition: FinPlugin.hh:76
GZ_REGISTER_MODEL_PLUGIN(UmbilicalPlugin)
ignition::math::Vector3d currentVelocity
Current velocity vector read from topic.
Definition: FinPlugin.hh:109
void OnUpdate(const common::UpdateInfo &_info)
Update the simulation state.
Definition: FinPlugin.cc:137
virtual ~FinPlugin()
Destructor.
Definition: FinPlugin.cc:35
static DynamicsFactory & GetInstance()
Returns the singleton instance of this factory.
Definition: Dynamics.cc:52
common::Time angleStamp
Time stamp of latest thrust force.
Definition: FinPlugin.hh:103
virtual void Init()
Definition: FinPlugin.cc:132
void UpdateCurrentVelocity(ConstVector3dPtr &_msg)
Reads current velocity topic.
Definition: FinPlugin.cc:195
std::shared_ptr< Dynamics > dynamics
Fin dynamic model.
Definition: FinPlugin.hh:64
static LiftDragFactory & GetInstance()
Returns the singleton instance of this factory.
void UpdateInput(ConstDoublePtr &_msg)
Callback for the input topic subscriber.
Definition: FinPlugin.cc:189
Model plugin for description of a submarine&#39;s fin.
transport::NodePtr node
Gazebo node.
Definition: FinPlugin.hh:73
std::string topicPrefix
Topic prefix.
Definition: FinPlugin.hh:97
physics::LinkPtr link
The fin link.
Definition: FinPlugin.hh:79
int finID
Fin ID.
Definition: FinPlugin.hh:94
transport::SubscriberPtr commandSubscriber
Subscriber to the reference signal topic.
Definition: FinPlugin.hh:82
event::ConnectionPtr updateConnection
Update event.
Definition: FinPlugin.hh:70
transport::PublisherPtr anglePublisher
Publisher to the output thrust topic.
Definition: FinPlugin.hh:85
General definitions.
std::shared_ptr< LiftDrag > liftdrag
Lift&Drag model.
Definition: FinPlugin.hh:67


uuv_gazebo_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:24