30 #include <gazebo/physics/physics.hh> 31 #include <gazebo/sensors/sensors.hh> 32 #include <gazebo/common/common.hh> 34 #include <sdf/Param.hh> 44 GazeboRosPowerMonitor::GazeboRosPowerMonitor()
47 GazeboRosPowerMonitor::~GazeboRosPowerMonitor()
49 this->update_connection_.reset();
51 this->queue_.disable();
52 this->rosnode_->shutdown();
53 this->callback_queue_thread_.join();
57 void GazeboRosPowerMonitor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
60 std::string modelName = _sdf->GetParent()->Get<std::string>(
"name");
63 this->world_ = _parent->GetWorld();
65 this->deferred_load_thread_ = boost::thread(
66 boost::bind(&GazeboRosPowerMonitor::LoadThread,
this));
69 void GazeboRosPowerMonitor::LoadThread()
71 if (this->sdf_->HasElement(
"robotNamespace"))
72 this->robot_namespace_ = this->sdf_->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
74 this->robot_namespace_ =
"";
76 if (!this->sdf_->HasElement(
"powerStateTopic"))
78 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <powerStateTopic>, defaults to \"power_state\"");
79 this->power_state_topic_ =
"power_state";
82 this->power_state_topic_ = this->sdf_->GetElement(
"powerStateTopic")->Get<std::string>();
84 if (!this->sdf_->HasElement(
"powerStateRate"))
86 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <powerStateRate>, defaults to 1.0");
87 this->power_state_rate_ = 1.0;
90 this->power_state_rate_ = this->sdf_->GetElement(
"powerStateRate")->Get<
double>();
92 if (!this->sdf_->HasElement(
"fullChargeCapacity"))
94 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <fullChargeCapacity>, defaults to 80.0");
95 this->full_capacity_ = 80.0;
97 this->full_capacity_ = this->sdf_->GetElement(
"fullChargeCapacity")->Get<
double>();
99 if (!this->sdf_->HasElement(
"dischargeRate"))
101 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <dischargeRate>, defaults to -500.0");
102 this->discharge_rate_ = -500.0;
105 this->discharge_rate_ = this->sdf_->GetElement(
"dischargeRate")->Get<
double>();
107 if (!this->sdf_->HasElement(
"chargeRate"))
109 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <chargeRate>, defaults to 1000.0");
110 this->charge_rate_ = 1000.0;
113 this->charge_rate_ = this->sdf_->GetElement(
"chargeRate")->Get<
double>();
115 if (!this->sdf_->HasElement(
"dischargeVoltage"))
117 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <dischargeVoltage>, defaults to 16.0");
118 this->discharge_voltage_ = 16.0;
121 this->discharge_voltage_ = this->sdf_->GetElement(
"dischargeVoltage")->Get<
double>();
123 if (!this->sdf_->HasElement(
"chargeVoltage"))
125 ROS_INFO_NAMED(
"power_monitor",
"power_monitor plugin missing <chargeVoltage>, defaults to 16.0");
126 this->charge_voltage_ = 16.0;
129 this->charge_voltage_ = this->sdf_->GetElement(
"chargeVoltage")->Get<
double>();
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)");
142 this->power_state_topic_, 10,
143 boost::bind(&GazeboRosPowerMonitor::ConnectCb,
this),
144 boost::bind(&GazeboRosPowerMonitor::DisconnectCb,
this),
146 this->power_state_pub_ = this->rosnode_->advertise(opts);
148 this->plugged_in_sub_ = this->rosnode_->subscribe(
149 "plugged_in", 10, &GazeboRosPowerMonitor::SetPlug,
this);
151 this->callback_queue_thread_ = boost::thread(
152 boost::bind(&GazeboRosPowerMonitor::QueueThread,
this));
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();
161 last_time_ = this->world_->GetSimTime().Double();
166 this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
167 boost::bind(&GazeboRosPowerMonitor::UpdateChild,
this));
170 void GazeboRosPowerMonitor::ConnectCb()
172 this->connect_count_++;
175 void GazeboRosPowerMonitor::DisconnectCb()
177 this->connect_count_--;
180 void GazeboRosPowerMonitor::QueueThread()
182 static const double timeout = 0.01;
183 while (this->rosnode_->ok())
189 void GazeboRosPowerMonitor::UpdateChild()
192 #if GAZEBO_MAJOR_VERSION >= 8 193 double curr_time_ = this->world_->SimTime().Double();
195 double curr_time_ = this->world_->GetSimTime().Double();
197 double dt = curr_time_ - last_time_;
199 if (dt < this->power_state_rate_)
202 last_time_ = curr_time_;
204 if (connect_count_ == 0)
208 double current = 0.0;
210 current = charge_rate_ / voltage_;
212 charge_ += (dt / 3600) * current;
218 if (charge_ > this->full_capacity_)
219 charge_ = this->full_capacity_;
222 power_state_.header.stamp.fromSec(curr_time_);
224 power_state_.power_consumption = charge_rate_;
227 power_state_.time_remaining =
ros::Duration((charge_ / -current) * 60);
230 double charge_to_full = this->full_capacity_ - charge_;
231 if (charge_to_full == 0.0)
233 else if (current == 0.0)
236 power_state_.time_remaining =
ros::Duration((charge_to_full / current) * 60);
239 power_state_.prediction_method =
"fuel gauge";
240 power_state_.relative_capacity = (int) (100.0 * (charge_ / this->full_capacity_));
243 power_state_pub_.publish(power_state_);
247 void GazeboRosPowerMonitor::SetPlug(
const pr2_gazebo_plugins::PlugCommandConstPtr& plug_msg)
251 if (plug_msg->charge_rate > 0.0)
253 this->charge_rate_ = plug_msg->charge_rate;
254 ROS_DEBUG(
"debug: charge rate %f",this->charge_rate_);
256 if (plug_msg->discharge_rate < 0.0)
258 this->discharge_rate_ = plug_msg->discharge_rate;
259 ROS_DEBUG(
"debug: discharge rate %f",this->discharge_rate_);
262 charge_ = plug_msg->charge;
265 if (plug_msg->ac_present)
267 charge_rate_ = this->charge_rate_ + this->discharge_rate_;
268 power_state_.AC_present = 4;
272 charge_rate_ = this->discharge_rate_;
273 power_state_.AC_present = 0;
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