ThermoPlugin.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <stdio.h>
00003 
00004 #include <boost/bind.hpp>
00005 #include <gazebo/gazebo.hh>
00006 #include <gazebo/physics/physics.hh>
00007 #include <gazebo/common/common.hh>
00008 #include <gazebo/common/Time.hh>
00009 
00010 #include <ros/ros.h>
00011 #include <ros/callback_queue.h>
00012 #include <ros/advertise_options.h>
00013 #include <ros/subscribe_options.h>
00014 
00015 #include <std_msgs/Float32.h>
00016 #include <std_msgs/String.h>
00017 #include <geometry_msgs/Vector3.h>
00018 
00019 #include "PubQueue.h"
00020 
00021 namespace gazebo {
00022 class ThermoPlugin: public ModelPlugin {
00023 
00024 public:
00025         // Initialize
00026         void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
00027                 // Store the pointer to the model
00028                 this->model = _parent;
00029 
00030                 // read option args in sdf tags
00031                 this->robot_name = "robot";
00032                 if (_sdf->HasElement("robotname")) {
00033                         this->link_name = _sdf->Get<std::string>("robotname");
00034                 }
00035                 this->link_name = "root";
00036                 if (_sdf->HasElement("linkname")) {
00037                         this->link_name = _sdf->Get<std::string>("linkname");
00038                 }
00039                 this->joint_name = "";
00040                 if (_sdf->HasElement("jointname")) {
00041                         this->joint_name = _sdf->Get<std::string>("jointname");
00042                 }
00043                 this->electric_resistance = this->parseThermoParam(_sdf, "electric_resistance", 1.16);
00044                 this->inner_thermo_resistance = this->parseThermoParam(_sdf, "inner_thermo_resistance", 1.93);
00045                 this->coil_thermo_conductance = this->parseThermoParam(_sdf, "coil_thermo_conductance", 21.55);
00046                 this->outer_thermo_resistance = this->parseThermoParam(_sdf, "outer_thermo_resistance", 4.65);
00047                 this->case_thermo_conductance = this->parseThermoParam(_sdf, "case_thermo_conductance", 240.86);
00048                 this->atomosphere_thermo_conductance = this->parseThermoParam(_sdf, "atomosphere_thermo_conductance", 1000.0);
00049                 this->A_vs_Nm = this->parseThermoParam(_sdf, "A_vs_Nm", 0.1);
00050                 this->atomosphere_temperature = this->parseThermoParam(_sdf, "atomosphere_temperature", 300);
00051                 this->case_temperature = this->parseThermoParam(_sdf, "case_temperature", 300);
00052                 this->coil_temperature = this->parseThermoParam(_sdf, "coil_temperature", 300);
00053                 this->thermal_calcuration_step = this->parseThermoParam(_sdf, "thermal_calcuration_step", 10);
00054                 this->thermal_calcuration_cnt = this->thermal_calcuration_step ;
00055                 this->tau = 0;
00056 
00057                 // find root link
00058                 this->joint = this->model->GetJoint(this->joint_name);
00059                 this->link = this->model->GetLink(this->link_name);
00060                 if (!this->link) { this->link = this->joint->GetChild(); }
00061                 this->world = this->model->GetWorld();
00062                 this->lastUpdateTime = this->world->GetSimTime() ;
00063 
00064                 // Make sure the ROS node for Gazebo has already been initialized
00065                 if (!ros::isInitialized()) {
00066                         gzerr
00067                                         << "A ROS node for Gazebo has not been initialized, unable to load plugin. "
00068                                         << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)";
00069                         return;
00070                 }
00071                 // ros node
00072                 this->rosNode = new ros::NodeHandle("");
00073                 // ros callback queue for processing subscription
00074                 this->deferredLoadThread = boost::thread(
00075                                 boost::bind(&ThermoPlugin::DeferredLoad, this));
00076         }
00077 
00078         void DeferredLoad() {
00079                 // publish multi queue
00080                 this->pmq.startServiceThread();
00081 
00082                 // ros topic publications
00083                 this->pubTorqueQueue = this->pmq.addPub<std_msgs::Float32>();
00084                 this->pubTorque = this->rosNode->advertise<std_msgs::Float32>(
00085                                  "/" + this->robot_name + "/thermo_plugin/" + this->joint_name + "/torque", 100, true);
00086                 this->pubCoilThermoQueue = this->pmq.addPub<std_msgs::Float32>();
00087                 this->pubCoilThermo = this->rosNode->advertise<std_msgs::Float32>(
00088                                 "/" + this->robot_name + "/thermo_plugin/" + this->joint_name + "/thermo/coil", 100, true);
00089                 this->pubCaseThermoQueue = this->pmq.addPub<std_msgs::Float32>();
00090                         this->pubCaseThermo = this->rosNode->advertise<std_msgs::Float32>(
00091                                         "/" + this->robot_name + "/thermo_plugin/" + this->joint_name + "/thermo/case", 100, true);
00092 
00093 
00094                 // Listen to the update event.
00095                 this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00096                                 boost::bind(&ThermoPlugin::OnUpdate, this, _1));
00097 
00098                 gzmsg << "ThermoPlugin was loaded !" << std::endl;
00099         }
00100 
00101         // Called by the world update start event
00102         void OnUpdate(const common::UpdateInfo & /*_info*/) {
00103                 physics::JointPtr j = this->joint ;
00104                 physics::JointWrench w = j->GetForceTorque(0u);
00105                 math::Vector3 a = j->GetGlobalAxis(0u);
00106                 math::Vector3 m = this->link->GetWorldPose().rot * w.body2Torque;
00107                 this->tau += (float)a.Dot(m);
00108 
00109                 if ( --this->thermal_calcuration_cnt > 0 ) return ;
00110 
00111                 this->tau /= this->thermal_calcuration_step;
00112                 this->thermal_calcuration_cnt = this->thermal_calcuration_step;
00113                 common::Time curTime = this->world->GetSimTime();
00114                 this->PublishThermo(curTime, this->lastUpdateTime);
00115                 this->lastUpdateTime = curTime ;
00116                 this->tau = 0;
00117         }
00118 
00119         // Link has only 1 joint, and the joint has only 1 axis
00120         void PublishThermo(const common::Time &_curTime, const common::Time &_lastTime) {
00121                 std_msgs::Float32 tor, the1, the2 ;
00122                 float abs_tau  ;
00123                 if ( this->tau > 0 ){
00124                         abs_tau = this->tau ;
00125                 } else {
00126                         abs_tau = -this->tau ;
00127                 }
00128 
00129                 float dt = (_curTime - _lastTime).Float();
00130                 float dT = (this->atomosphere_temperature - this->case_temperature)/this->outer_thermo_resistance + (this->coil_temperature - this->case_temperature)/this->inner_thermo_resistance;
00131                 float dTin = (this->case_temperature - this->coil_temperature)/this->inner_thermo_resistance + abs_tau * this->A_vs_Nm * this->electric_resistance;
00132                 float dTout = (this->case_temperature - this->atomosphere_temperature)/this->outer_thermo_resistance;
00133                 this->case_temperature += dT / this->case_thermo_conductance * dt;
00134                 this->coil_temperature += dTin / this->coil_thermo_conductance * dt;
00135                 this->atomosphere_temperature += dTout / this->atomosphere_thermo_conductance * dt;
00136 
00137                 tor.data = this->tau ;
00138                 this->pubTorqueQueue->push(tor, this->pubTorque);
00139                 the1.data = this->case_temperature;
00140                 this->pubCaseThermoQueue->push(the1, this->pubCaseThermo);
00141                 the2.data = this->coil_temperature;
00142                 this->pubCoilThermoQueue->push(the2, this->pubCoilThermo);
00143 
00144                 //std::cout << "(" << this->coil_temperature << "," << this->case_temperature << "," << this->atomosphere_temperature << ") in " << dt << "sec" << std::endl;
00145         }
00146 
00147 private:
00148         physics::ModelPtr model;
00149         physics::WorldPtr world;
00150         std::string robot_name;
00151         std::string joint_name;
00152         std::string link_name;
00153         physics::LinkPtr link;
00154         physics::JointPtr joint;
00155         event::ConnectionPtr updateConnection;
00156         common::Time lastUpdateTime;
00157         int thermal_calcuration_step, thermal_calcuration_cnt;
00158         float tau;
00159 
00160         float electric_resistance, inner_thermo_resistance, outer_thermo_resistance;
00161         float coil_thermo_conductance, case_thermo_conductance, atomosphere_thermo_conductance;
00162         float A_vs_Nm, atomosphere_temperature, case_temperature, coil_temperature;
00163 
00164         ros::NodeHandle* rosNode;
00165         PubMultiQueue pmq;
00166         boost::mutex mutex;
00167         boost::thread deferredLoadThread;
00168 
00169         ros::Publisher pubTorque;
00170         PubQueue<std_msgs::Float32>::Ptr pubTorqueQueue;
00171         ros::Publisher pubCoilThermo;
00172         PubQueue<std_msgs::Float32>::Ptr pubCoilThermoQueue;
00173         ros::Publisher pubCaseThermo;
00174         PubQueue<std_msgs::Float32>::Ptr pubCaseThermoQueue;
00175 
00176 //      ros::Publisher pubMoment;
00177 //      PubQueue<geometry_msgs::Vector3>::Ptr pubMomentQueue;
00178 
00179         float parseThermoParam(sdf::ElementPtr _sdf, std::string name, float defo){
00180                 float ret ;
00181                 if (_sdf->HasElement(name)) {
00182                         ret = _sdf->Get<float>(name);
00183                 } else {
00184                         ret = defo;
00185                 }
00186                 std::cout << " [thermo plugin] " << name << " = " << ret << " for " << this->joint_name << std::endl;
00187                 return ret ;
00188         }
00189 };
00190 
00191 GZ_REGISTER_MODEL_PLUGIN(ThermoPlugin)
00192 }


hrpsys_gazebo_general
Author(s): Yohei Kakiuchi , Kei Okada , Masaki Murooka
autogenerated on Thu Jun 6 2019 20:56:53