gazebo_ros_power_monitor.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "physics/World.hh"
00031 #include "sensors/Sensor.hh"
00032 #include "sdf/interface/SDF.hh"
00033 #include "sdf/interface/Param.hh"
00034 #include "common/Exception.hh"
00035 #include "physics/PhysicsTypes.hh"
00036 #include "physics/Base.hh"
00037 
00038 #include <pr2_gazebo_plugins/gazebo_ros_power_monitor.h>
00039 #include <diagnostic_updater/diagnostic_updater.h>
00040 
00041 using namespace std;
00042 
00043 namespace gazebo {
00044 
00045 GazeboRosPowerMonitor::GazeboRosPowerMonitor()
00046 {
00047     // model_ = dynamic_cast<Model*>(parent);
00048     // if (!model_)
00049     //     gzthrow("GazeboRosPowerMonitor controller requires a Model as its parent");
00050 
00051     // // Initialize parameters
00052     // Param::Begin(&parameters);
00053     // robot_namespace_param_   = new ParamT<string>("robotNamespace",     "/",           0);
00054     // power_state_topic_param_ = new ParamT<string>("powerStateTopic",    "power_state", 0);
00055     // power_state_rate_param_  = new ParamT<double>("powerStateRate",        1.0,        0);
00056     // full_capacity_param_     = new ParamT<double>("fullChargeCapacity",   80.0,        0);
00057     // discharge_rate_param_    = new ParamT<double>("dischargeRate",      -500.0,        0);
00058     // charge_rate_param_       = new ParamT<double>("chargeRate",         1000.0,        0);
00059     // discharge_voltage_param_ = new ParamT<double>("dischargeVoltage",     16.0,        0);
00060     // charge_voltage_param_    = new ParamT<double>("chargeVoltage",        16.0,        0);
00061     // Param::End();
00062 }
00063 
00064 GazeboRosPowerMonitor::~GazeboRosPowerMonitor()
00065 {
00066     // Do nothing
00067     this->rosnode_->shutdown();
00068 
00069     delete rosnode_;
00070 
00071     // delete robot_namespace_param_;
00072     // delete power_state_topic_param_;
00073     // delete power_state_rate_param_;
00074     // delete full_capacity_param_;
00075     // delete discharge_rate_param_;
00076     // delete charge_rate_param_;
00077     // delete discharge_voltage_param_;
00078     // delete charge_voltage_param_;
00079 }
00080 
00081 void GazeboRosPowerMonitor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00082 {
00083   // Get then name of the parent model
00084   std::string modelName = _sdf->GetParent()->GetValueString("name");
00085 
00086   // Get the world name.
00087   this->world = _parent->GetWorld();
00088 
00089   // Get a pointer to the model
00090   this->parent_model_ = _parent;
00091 
00092   // Error message if the model couldn't be found
00093   if (!this->parent_model_)
00094     gzerr << "Unable to get parent model\n";
00095 
00096   // Listen to the update event. This event is broadcast every
00097   // simulation iteration.
00098   this->updateConnection = event::Events::ConnectWorldUpdateStart(
00099       boost::bind(&GazeboRosPowerMonitor::UpdateChild, this));
00100   gzdbg << "plugin model name: " << modelName << "\n";
00101 
00102 
00103     // Load parameters from XML
00104     this->robot_namespace_     = "";
00105     this->power_state_topic_   = "";
00106     this->power_state_rate_    = 0;
00107     this->full_capacity_       = 0;
00108     this->discharge_rate_      = 0;
00109     this->charge_rate_         = 0;
00110     this->discharge_voltage_   = 0;
00111     this->charge_voltage_      = 0;
00112 
00113     if (!ros::isInitialized())
00114     {
00115       int argc = 0;
00116       char** argv = NULL;
00117       ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00118     }
00119 
00120     this->robot_namespace_ = "";
00121     rosnode_        = new ros::NodeHandle(this->robot_namespace_);
00122     power_state_pub_ = rosnode_->advertise<pr2_msgs::PowerState>(this->power_state_topic_, 10);
00123     plugged_in_sub_  = rosnode_->subscribe("plugged_in", 10, &GazeboRosPowerMonitor::SetPlug, this);
00124 }
00125 
00126 void GazeboRosPowerMonitor::InitChild()
00127 {
00128     last_time_ = curr_time_ = this->world->GetSimTime().Double();
00129 
00130     // Initialize battery to full capacity
00131     charge_      = this->full_capacity_;
00132     charge_rate_ = this->discharge_rate_;
00133     voltage_     = this->discharge_voltage_;
00134 }
00135 
00136 void GazeboRosPowerMonitor::UpdateChild()
00137 {
00138     // Update time
00139     curr_time_ = this->world->GetSimTime().Double();
00140     double dt = curr_time_ - last_time_;
00141     last_time_ = curr_time_;
00142 
00143     // Update charge
00144     double current = charge_rate_ / voltage_;
00145     charge_ += (dt / 3600) * current;   // charge is measured in ampere-hours, simulator time is measured in secs
00146 
00147     // Clamp to [0, full_capacity]
00148     if (charge_ < 0)
00149         charge_ = 0;
00150     if (charge_ > this->full_capacity_)
00151         charge_ = this->full_capacity_;
00152 
00153     // Publish power state (simulate the power_monitor node)
00154     power_state_.header.stamp.fromSec(curr_time_);
00155 
00156     power_state_.power_consumption = charge_rate_;
00157 
00158     if (current < 0.0)
00159         power_state_.time_remaining = ros::Duration((charge_ / -current) * 60);      // time remaining reported in hours
00160     else
00161     {
00162         double charge_to_full = this->full_capacity_ - charge_;
00163         if (charge_to_full == 0.0)
00164             power_state_.time_remaining = ros::Duration(0);
00165         else if (current == 0.0)
00166             power_state_.time_remaining = ros::Duration(65535, 65535);               // zero current - time_remaining is undefined
00167         else
00168             power_state_.time_remaining = ros::Duration((charge_to_full / current) * 60);
00169     }
00170 
00171     power_state_.prediction_method = "fuel gauge";
00172     power_state_.relative_capacity = (int) (100.0 * (charge_ / this->full_capacity_));
00173 
00174     lock_.lock();
00175     power_state_pub_.publish(power_state_);
00176     lock_.unlock();
00177 }
00178 
00179 void GazeboRosPowerMonitor::SetPlug(const pr2_gazebo_plugins::PlugCommandConstPtr& plug_msg)
00180 {
00181     lock_.lock();
00182 
00183     if (plug_msg->charge_rate > 0.0)
00184     {
00185       this->charge_rate_ = plug_msg->charge_rate;
00186       ROS_DEBUG("debug: charge rate %f",this->charge_rate_);
00187     }
00188     if (plug_msg->discharge_rate < 0.0)
00189     {
00190       this->discharge_rate_ = plug_msg->discharge_rate;
00191       ROS_DEBUG("debug: discharge rate %f",this->discharge_rate_);
00192     }
00193 
00194     charge_ = plug_msg->charge;
00195     ROS_DEBUG("debug: charge %f",charge_);
00196 
00197     if (plug_msg->ac_present)
00198     {
00199         charge_rate_            = this->charge_rate_ + this->discharge_rate_;
00200         power_state_.AC_present = 4;
00201     }
00202     else
00203     {
00204         charge_rate_            = this->discharge_rate_;
00205         power_state_.AC_present = 0;
00206     }
00207 
00208     lock_.unlock();
00209 }
00210 
00211   // Register this plugin with the simulator
00212   GZ_REGISTER_MODEL_PLUGIN(GazeboRosPowerMonitor)
00213 
00214 }


pr2_gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Thu Jan 2 2014 11:45:00