gazebo_ros_power_monitor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gazebo/physics/physics.hh>
31 #include <gazebo/sensors/sensors.hh>
32 #include <gazebo/common/common.hh>
33 #include <sdf/sdf.hh>
34 #include <sdf/Param.hh>
35 
37 
39 
40 using namespace std;
41 
42 namespace gazebo {
43 
44 GazeboRosPowerMonitor::GazeboRosPowerMonitor()
45 {}
46 
47 GazeboRosPowerMonitor::~GazeboRosPowerMonitor()
48 {
49  this->update_connection_.reset();
50  this->queue_.clear();
51  this->queue_.disable();
52  this->rosnode_->shutdown();
53  this->callback_queue_thread_.join();
54  delete rosnode_;
55 }
56 
57 void GazeboRosPowerMonitor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
58 {
59  // Get then name of the parent model
60  std::string modelName = _sdf->GetParent()->Get<std::string>("name");
61 
62  // Get the world name.
63  this->world_ = _parent->GetWorld();
64  this->sdf_ = _sdf;
65  this->deferred_load_thread_ = boost::thread(
66  boost::bind(&GazeboRosPowerMonitor::LoadThread, this));
67 }
68 
69 void GazeboRosPowerMonitor::LoadThread()
70 {
71  if (this->sdf_->HasElement("robotNamespace"))
72  this->robot_namespace_ = this->sdf_->GetElement("robotNamespace")->Get<std::string>() + "/";
73  else
74  this->robot_namespace_ = "";
75 
76  if (!this->sdf_->HasElement("powerStateTopic"))
77  {
78  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <powerStateTopic>, defaults to \"power_state\"");
79  this->power_state_topic_ = "power_state";
80  }
81  else
82  this->power_state_topic_ = this->sdf_->GetElement("powerStateTopic")->Get<std::string>();
83 
84  if (!this->sdf_->HasElement("powerStateRate"))
85  {
86  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <powerStateRate>, defaults to 1.0");
87  this->power_state_rate_ = 1.0;
88  }
89  else
90  this->power_state_rate_ = this->sdf_->GetElement("powerStateRate")->Get<double>();
91 
92  if (!this->sdf_->HasElement("fullChargeCapacity"))
93  {
94  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <fullChargeCapacity>, defaults to 80.0");
95  this->full_capacity_ = 80.0;
96  }
97  this->full_capacity_ = this->sdf_->GetElement("fullChargeCapacity")->Get<double>();
98 
99  if (!this->sdf_->HasElement("dischargeRate"))
100  {
101  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <dischargeRate>, defaults to -500.0");
102  this->discharge_rate_ = -500.0;
103  }
104  else
105  this->discharge_rate_ = this->sdf_->GetElement("dischargeRate")->Get<double>();
106 
107  if (!this->sdf_->HasElement("chargeRate"))
108  {
109  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <chargeRate>, defaults to 1000.0");
110  this->charge_rate_ = 1000.0;
111  }
112  else
113  this->charge_rate_ = this->sdf_->GetElement("chargeRate")->Get<double>();
114 
115  if (!this->sdf_->HasElement("dischargeVoltage"))
116  {
117  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <dischargeVoltage>, defaults to 16.0");
118  this->discharge_voltage_ = 16.0;
119  }
120  else
121  this->discharge_voltage_ = this->sdf_->GetElement("dischargeVoltage")->Get<double>();
122 
123  if (!this->sdf_->HasElement("chargeVoltage"))
124  {
125  ROS_INFO_NAMED("power_monitor", "power_monitor plugin missing <chargeVoltage>, defaults to 16.0");
126  this->charge_voltage_ = 16.0;
127  }
128  else
129  this->charge_voltage_ = this->sdf_->GetElement("chargeVoltage")->Get<double>();
130 
131  if (!ros::isInitialized())
132  {
133  ROS_FATAL_STREAM_NAMED("power_monitor", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
134  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
135  return;
136  }
137 
138  // Initialize ROS functions
139  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
140 
141  ros::AdvertiseOptions opts = ros::AdvertiseOptions::create<pr2_msgs::PowerState>(
142  this->power_state_topic_, 10,
143  boost::bind(&GazeboRosPowerMonitor::ConnectCb, this),
144  boost::bind(&GazeboRosPowerMonitor::DisconnectCb, this),
145  ros::VoidPtr(), &this->queue_);
146  this->power_state_pub_ = this->rosnode_->advertise(opts);
147 
148  this->plugged_in_sub_ = this->rosnode_->subscribe(
149  "plugged_in", 10, &GazeboRosPowerMonitor::SetPlug, this);
150 
151  this->callback_queue_thread_ = boost::thread(
152  boost::bind(&GazeboRosPowerMonitor::QueueThread, this));
153 
154  // Initialize internal variables
155  charge_ = this->full_capacity_;
156  charge_rate_ = this->discharge_rate_;
157  voltage_ = this->discharge_voltage_;
158 #if GAZEBO_MAJOR_VERSION >= 8
159  last_time_ = this->world_->SimTime().Double();
160 #else
161  last_time_ = this->world_->GetSimTime().Double();
162 #endif
163 
164  // Listen to the update event. This event is broadcast every
165  // simulation iteration.
166  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
167  boost::bind(&GazeboRosPowerMonitor::UpdateChild, this));
168 }
169 
170 void GazeboRosPowerMonitor::ConnectCb()
171 {
172  this->connect_count_++;
173 }
174 
175 void GazeboRosPowerMonitor::DisconnectCb()
176 {
177  this->connect_count_--;
178 }
179 
180 void GazeboRosPowerMonitor::QueueThread()
181 {
182  static const double timeout = 0.01;
183  while (this->rosnode_->ok())
184  {
185  this->queue_.callAvailable(ros::WallDuration(timeout));
186  }
187 }
188 
189 void GazeboRosPowerMonitor::UpdateChild()
190 {
191  // Update time
192 #if GAZEBO_MAJOR_VERSION >= 8
193  double curr_time_ = this->world_->SimTime().Double();
194 #else
195  double curr_time_ = this->world_->GetSimTime().Double();
196 #endif
197  double dt = curr_time_ - last_time_;
198 
199  if (dt < this->power_state_rate_)
200  return;
201 
202  last_time_ = curr_time_;
203 
204  if (connect_count_ == 0)
205  return;
206 
207  // Update charge
208  double current = 0.0;
209  if (voltage_ > 0) {
210  current = charge_rate_ / voltage_;
211  // charge is measured in ampere-hours, simulator time is measured in secs
212  charge_ += (dt / 3600) * current;
213  }
214 
215  // Clamp to [0, full_capacity]
216  if (charge_ < 0)
217  charge_ = 0;
218  if (charge_ > this->full_capacity_)
219  charge_ = this->full_capacity_;
220 
221  // Publish power state (simulate the power_monitor node)
222  power_state_.header.stamp.fromSec(curr_time_);
223 
224  power_state_.power_consumption = charge_rate_;
225 
226  if (current < 0.0)
227  power_state_.time_remaining = ros::Duration((charge_ / -current) * 60); // time remaining reported in hours
228  else
229  {
230  double charge_to_full = this->full_capacity_ - charge_;
231  if (charge_to_full == 0.0)
232  power_state_.time_remaining = ros::Duration(0);
233  else if (current == 0.0)
234  power_state_.time_remaining = ros::Duration(65535, 65535); // zero current - time_remaining is undefined
235  else
236  power_state_.time_remaining = ros::Duration((charge_to_full / current) * 60);
237  }
238 
239  power_state_.prediction_method = "fuel gauge";
240  power_state_.relative_capacity = (int) (100.0 * (charge_ / this->full_capacity_));
241 
242  lock_.lock();
243  power_state_pub_.publish(power_state_);
244  lock_.unlock();
245 }
246 
247 void GazeboRosPowerMonitor::SetPlug(const pr2_gazebo_plugins::PlugCommandConstPtr& plug_msg)
248 {
249  lock_.lock();
250 
251  if (plug_msg->charge_rate > 0.0)
252  {
253  this->charge_rate_ = plug_msg->charge_rate;
254  ROS_DEBUG("debug: charge rate %f",this->charge_rate_);
255  }
256  if (plug_msg->discharge_rate < 0.0)
257  {
258  this->discharge_rate_ = plug_msg->discharge_rate;
259  ROS_DEBUG("debug: discharge rate %f",this->discharge_rate_);
260  }
261 
262  charge_ = plug_msg->charge;
263  ROS_DEBUG("debug: charge %f",charge_);
264 
265  if (plug_msg->ac_present)
266  {
267  charge_rate_ = this->charge_rate_ + this->discharge_rate_;
268  power_state_.AC_present = 4;
269  }
270  else
271  {
272  charge_rate_ = this->discharge_rate_;
273  power_state_.AC_present = 0;
274  }
275 
276  lock_.unlock();
277 }
278 
279  // Register this plugin with the simulator
281 
282 }
GZ_REGISTER_MODEL_PLUGIN(GazeboRosElevator)
#define ROS_INFO_NAMED(name,...)
ROSCPP_DECL bool isInitialized()
#define ROS_FATAL_STREAM_NAMED(name, args)
boost::shared_ptr< void > VoidPtr
#define ROS_DEBUG(...)


pr2_gazebo_plugins
Author(s): John Hsu
autogenerated on Mon Jun 10 2019 14:28:45