CustomBatteryConsumerROSPlugin.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->isDeviceOn = true;
24 }
25 
28 {
29  this->rosNode->shutdown();
30 }
31 
33 void CustomBatteryConsumerROSPlugin::Load(physics::ModelPtr _parent,
34  sdf::ElementPtr _sdf)
35 {
36  if (!ros::isInitialized())
37  {
38  gzerr << "Not loading plugin since ROS has not been "
39  << "properly initialized. Try starting gazebo with ros plugin:\n"
40  << " gazebo -s libgazebo_ros_api_plugin.so\n";
41  return;
42  }
43 
44  this->rosNode.reset(new ros::NodeHandle(""));
45 
46  GZ_ASSERT(_sdf->HasElement("link_name"), "Consumer link name is missing");
47  this->linkName = _sdf->Get<std::string>("link_name");
48  physics::LinkPtr link = _parent->GetLink(this->linkName);
49  GZ_ASSERT(link, "Link was NULL");
50 
51  GZ_ASSERT(_sdf->HasElement("battery_name"), "Battery name is missing");
52  this->batteryName = _sdf->Get<std::string>("battery_name");
53  this->battery = link->Battery(this->batteryName);
54  GZ_ASSERT(this->battery, "Battery was NULL");
55 
56  GZ_ASSERT(_sdf->HasElement("power_load"), "Power load is missing");
57  this->powerLoad = _sdf->Get<double>("power_load");
58 
59  GZ_ASSERT(this->powerLoad > 0, "Power load must be greater than zero");
60 
61  // Adding consumer
62  this->consumerID = this->battery->AddConsumer();
63 
64  if (_sdf->HasElement("topic_device_state"))
65  {
66  std::string topicName = _sdf->Get<std::string>("topic_device_state");
67  if (!topicName.empty())
68  this->deviceStateSub = this->rosNode->subscribe<std_msgs::Bool>(
69  topicName, 1,
71  this, _1));
72  }
73  else
74  {
75  // In the case the device is always on, then set the power load only once
76  this->UpdatePowerLoad(this->powerLoad);
77  }
78 
79  gzmsg << "CustomBatteryConsumerROSPlugin::Device <"
80  << this->linkName << "> added as battery consumer" << std::endl
81  << "\t- ID=" << this->consumerID << std::endl
82  << "\t- Power load [W]=" << this->powerLoad
83  << std::endl;
84 }
85 
88  const std_msgs::Bool::ConstPtr &_msg)
89 {
90  this->isDeviceOn = _msg->data;
91  if (this->isDeviceOn)
92  this->UpdatePowerLoad(this->powerLoad);
93  else
94  this->UpdatePowerLoad(0.0);
95 }
96 
99 {
100  if (!this->battery->SetPowerLoad(this->consumerID, _powerLoad))
101  gzerr << "Error setting the consumer power load" << std::endl;
102 }
103 
105 }
boost::scoped_ptr< ros::NodeHandle > rosNode
Pointer to this ROS node&#39;s handle.
ROSCPP_DECL bool isInitialized()
GZ_REGISTER_MODEL_PLUGIN(UmbilicalPlugin)
bool isDeviceOn
Flag to signal whether a specific device is running.
common::BatteryPtr battery
Pointer to battery.
void UpdatePowerLoad(double _powerLoad=0.0)
Update power load.
ros::Subscriber deviceStateSub
Subscriber to the device state flag.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load module and read parameters from SDF.
void UpdateDeviceState(const std_msgs::Bool::ConstPtr &_msg)
Callback for the device state topic subscriber.


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