Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <gazebo/physics/physics.hh>
00031 #include <gazebo/sensors/sensors.hh>
00032 #include <gazebo/common/common.hh>
00033 #include <sdf/sdf.hh>
00034 #include <sdf/Param.hh>
00035
00036 #include <diagnostic_updater/diagnostic_updater.h>
00037
00038 #include "pr2_gazebo_plugins/gazebo_ros_power_monitor.h"
00039
00040 using namespace std;
00041
00042 namespace gazebo {
00043
00044 GazeboRosPowerMonitor::GazeboRosPowerMonitor()
00045 {
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 }
00062
00063 GazeboRosPowerMonitor::~GazeboRosPowerMonitor()
00064 {
00065
00066 this->rosnode_->shutdown();
00067
00068 delete rosnode_;
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 }
00079
00080 void GazeboRosPowerMonitor::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00081 {
00082
00083 std::string modelName = _sdf->GetParent()->Get<std::string>("name");
00084
00085
00086 this->world = _parent->GetWorld();
00087
00088
00089 this->parent_model_ = _parent;
00090
00091
00092 if (!this->parent_model_)
00093 gzerr << "Unable to get parent model\n";
00094
00095
00096
00097 this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00098 boost::bind(&GazeboRosPowerMonitor::UpdateChild, this));
00099 gzdbg << "plugin model name: " << modelName << "\n";
00100
00101
00102
00103 this->robot_namespace_ = "";
00104 this->power_state_topic_ = "";
00105 this->power_state_rate_ = 0;
00106 this->full_capacity_ = 0;
00107 this->discharge_rate_ = 0;
00108 this->charge_rate_ = 0;
00109 this->discharge_voltage_ = 0;
00110 this->charge_voltage_ = 0;
00111
00112 if (!ros::isInitialized())
00113 {
00114 int argc = 0;
00115 char** argv = NULL;
00116 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00117 }
00118
00119 this->robot_namespace_ = "";
00120 rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00121 power_state_pub_ = rosnode_->advertise<pr2_msgs::PowerState>(this->power_state_topic_, 10);
00122 plugged_in_sub_ = rosnode_->subscribe("plugged_in", 10, &GazeboRosPowerMonitor::SetPlug, this);
00123 }
00124
00125 void GazeboRosPowerMonitor::InitChild()
00126 {
00127 last_time_ = curr_time_ = this->world->GetSimTime().Double();
00128
00129
00130 charge_ = this->full_capacity_;
00131 charge_rate_ = this->discharge_rate_;
00132 voltage_ = this->discharge_voltage_;
00133 }
00134
00135 void GazeboRosPowerMonitor::UpdateChild()
00136 {
00137
00138 curr_time_ = this->world->GetSimTime().Double();
00139 double dt = curr_time_ - last_time_;
00140 last_time_ = curr_time_;
00141
00142
00143 double current = charge_rate_ / voltage_;
00144 charge_ += (dt / 3600) * current;
00145
00146
00147 if (charge_ < 0)
00148 charge_ = 0;
00149 if (charge_ > this->full_capacity_)
00150 charge_ = this->full_capacity_;
00151
00152
00153 power_state_.header.stamp.fromSec(curr_time_);
00154
00155 power_state_.power_consumption = charge_rate_;
00156
00157 if (current < 0.0)
00158 power_state_.time_remaining = ros::Duration((charge_ / -current) * 60);
00159 else
00160 {
00161 double charge_to_full = this->full_capacity_ - charge_;
00162 if (charge_to_full == 0.0)
00163 power_state_.time_remaining = ros::Duration(0);
00164 else if (current == 0.0)
00165 power_state_.time_remaining = ros::Duration(65535, 65535);
00166 else
00167 power_state_.time_remaining = ros::Duration((charge_to_full / current) * 60);
00168 }
00169
00170 power_state_.prediction_method = "fuel gauge";
00171 power_state_.relative_capacity = (int) (100.0 * (charge_ / this->full_capacity_));
00172
00173 lock_.lock();
00174 power_state_pub_.publish(power_state_);
00175 lock_.unlock();
00176 }
00177
00178 void GazeboRosPowerMonitor::SetPlug(const pr2_gazebo_plugins::PlugCommandConstPtr& plug_msg)
00179 {
00180 lock_.lock();
00181
00182 if (plug_msg->charge_rate > 0.0)
00183 {
00184 this->charge_rate_ = plug_msg->charge_rate;
00185 ROS_DEBUG("debug: charge rate %f",this->charge_rate_);
00186 }
00187 if (plug_msg->discharge_rate < 0.0)
00188 {
00189 this->discharge_rate_ = plug_msg->discharge_rate;
00190 ROS_DEBUG("debug: discharge rate %f",this->discharge_rate_);
00191 }
00192
00193 charge_ = plug_msg->charge;
00194 ROS_DEBUG("debug: charge %f",charge_);
00195
00196 if (plug_msg->ac_present)
00197 {
00198 charge_rate_ = this->charge_rate_ + this->discharge_rate_;
00199 power_state_.AC_present = 4;
00200 }
00201 else
00202 {
00203 charge_rate_ = this->discharge_rate_;
00204 power_state_.AC_present = 0;
00205 }
00206
00207 lock_.unlock();
00208 }
00209
00210
00211 GZ_REGISTER_MODEL_PLUGIN(GazeboRosPowerMonitor)
00212
00213 }