motor_heating_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "ethercat_hardware/motor_heating_model.h"
00036 
00037 #include <boost/crc.hpp>
00038 #include <boost/static_assert.hpp>
00039 #include <boost/filesystem.hpp>
00040 #include <boost/bind.hpp>
00041 #include <boost/foreach.hpp>
00042 #include <boost/timer.hpp>
00043 
00044 // Use XML format when saving or loading XML file
00045 #include <tinyxml.h>
00046 
00047 #include <stdio.h>
00048 #include <errno.h>
00049 #include <exception>
00050 
00051 namespace ethercat_hardware
00052 {
00053 
00054 
00055 static const int DEBUG_LEVEL = 0; 
00056 
00057 
00058 bool MotorHeatingModelParametersEepromConfig::verifyCRC(void) const
00059 {
00060   BOOST_STATIC_ASSERT(sizeof(ethercat_hardware::MotorHeatingModelParametersEepromConfig) == 256);
00061   BOOST_STATIC_ASSERT( offsetof(ethercat_hardware::MotorHeatingModelParametersEepromConfig, crc32_) == (256-4));
00062   boost::crc_32_type crc32;  
00063   crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
00064   return (this->crc32_ == crc32.checksum());
00065 }
00066 
00067 void MotorHeatingModelParametersEepromConfig::generateCRC(void)
00068 {
00069   boost::crc_32_type crc32;
00070   crc32.process_bytes(this, offsetof(MotorHeatingModelParametersEepromConfig, crc32_));
00071   this->crc32_ = crc32.checksum();
00072 }
00073 
00074 
00075 
00076 MotorHeatingModelCommon::MotorHeatingModelCommon(ros::NodeHandle nh)
00077 {
00078   // There are a couple of rosparams that can be used to control the motor heating motor
00079   //  * <namespace>/motor_heating_model/load_save_files : 
00080   //      boolean : defaults to true
00081   //      Do not load saved motor temperature data at startup .  
00082   //      This might be useful for things like the the burn-in test stands 
00083   //      where different parts are constantly switched in-and-out.
00084   //  * <namespace>/motor_heating_model/save_directory : 
00085   //      string, defaults to '/var/lib'
00086   //      Location where motor heating model save data is loaded from and saved to
00087   //  * <namespace>/motor_heating_model/do_not_halt : 
00088   //      boolean, defaults to false 
00089   //
00090   if (!nh.getParam("load_save_files", load_save_files_))
00091   {
00092     load_save_files_ = true;
00093   }
00094   if (!nh.getParam("update_save_files", update_save_files_))
00095   {
00096     update_save_files_ = true;
00097   }
00098   if (!nh.getParam("do_not_halt", disable_halt_))
00099   {
00100     // TODO : once there has been enough testing of motor heating model on robot in real-world conditions,
00101     //        enable halting by default
00102     disable_halt_ = true;  
00103   }
00104   if (!nh.getParam("save_directory", save_directory_))
00105   {
00106     save_directory_ = "/var/lib/motor_heating_model";
00107   }
00108   if (!nh.getParam("enable_model", enable_model_))
00109   {
00110     enable_model_ = true;
00111   }
00112   if (!nh.getParam("publish_temperature", publish_temperature_))
00113   {
00114     publish_temperature_ = false;
00115   }
00116 }
00117 
00118 
00119 MotorHeatingModelCommon::MotorHeatingModelCommon() :
00120   update_save_files_ ( true ),
00121   save_directory_ ("/var/lib/motor_heating_model" ),
00122   load_save_files_  ( true ),
00123   disable_halt_ ( false ),
00124   enable_model_( true ),
00125   publish_temperature_( false )
00126 {
00127   
00128 }
00129 
00130 
00131 void MotorHeatingModelCommon::attach(boost::shared_ptr<MotorHeatingModel> model)
00132 {
00133   { // LOCK
00134     boost::lock_guard<boost::mutex> lock(mutex_);
00135     models_.push_back(model);
00136   } // UNLOCK
00137 }
00138 
00139 
00140 bool MotorHeatingModelCommon::initialize()
00141 {
00142   if (update_save_files_)
00143   {
00144     // Save thread should not be started until all MotorHeatingModels have been attached()
00145     save_thread_ = boost::thread(boost::bind(&MotorHeatingModelCommon::saveThreadFunc, this));
00146   }
00147   return true;
00148 }
00149 
00150 
00158 void MotorHeatingModelCommon::saveThreadFunc()
00159 {
00160   // DEB install should create directory with proper permissions.
00161   //createSaveDirectory();  
00162 
00163   while (true)
00164   {
00165     sleep(10);
00166     { //LOCK
00167       boost::lock_guard<boost::mutex> lock(mutex_);
00168       BOOST_FOREACH( boost::shared_ptr<MotorHeatingModel> model, models_ )
00169       {
00170         model->saveTemperatureState();
00171       }
00172     } //UNLOCK
00173   }
00174   
00175   { // Throw away ptrs to all motor models
00176     boost::lock_guard<boost::mutex> lock(mutex_);
00177     models_.clear();
00178   } 
00179 }
00180 
00181 
00184 bool MotorHeatingModelCommon::createSaveDirectory()
00185 {
00186   // create save directory if it does not already exist
00187   if (!boost::filesystem::exists(save_directory_))
00188   {
00189     ROS_WARN("Motor heating motor save directory '%s' does not exist, creating it", save_directory_.c_str());
00190     try {
00191       boost::filesystem::create_directory(save_directory_);
00192     } 
00193     catch (const std::exception &e)
00194     {
00195       ROS_ERROR("Error creating save directory '%s' for motor model : %s", 
00196                 save_directory_.c_str(), e.what());
00197       return false;
00198     }
00199   }
00200 
00201   return true;
00202 }
00203 
00204 
00205 
00214 MotorHeatingModel::MotorHeatingModel(const MotorHeatingModelParameters &motor_params,
00215                                      const std::string &actuator_name,
00216                                      const std::string &hwid,
00217                                      const std::string &save_directory
00218                                      ) :  
00219   overheat_(false),
00220   heating_energy_sum_(0.0),
00221   ambient_temperature_sum_(0.0),
00222   duration_since_last_sample_(0.0),
00223   publisher_(NULL),
00224   motor_params_(motor_params),
00225   actuator_name_(actuator_name),
00226   save_filename_(save_directory + "/" + actuator_name_ + ".save"),
00227   hwid_(hwid)
00228 {
00229   
00230 
00231 
00232   // If the guess is too low, the motors may not halt when they get too hot.
00233   // If the guess is too high the motor may halt when they should not.
00234 
00235   // The first time the motor heating model is run on new machine, there will be no saved motor temperature data.
00236   // With the saved temperatures, we have to 'guess' an initial motor temperature. 
00237   // Because the motor have been used for multiple years with only a couple incidents, 
00238   // its probably better to error on the side of false negitives.
00239   static const double default_ambient_temperature = 60.0;
00240   winding_temperature_ = default_ambient_temperature;
00241   housing_temperature_ = default_ambient_temperature;
00242   ambient_temperature_ = default_ambient_temperature;
00243 
00244   // Calculate internal parameters from motor parameters
00245   winding_to_housing_thermal_conductance_ = 1.0 / motor_params_.winding_to_housing_thermal_resistance_;
00246   housing_to_ambient_thermal_conductance_ = 1.0 / motor_params_.housing_to_ambient_thermal_resistance_;  
00247   winding_thermal_mass_inverse_ = 
00248     motor_params_.winding_to_housing_thermal_resistance_ / motor_params_.winding_thermal_time_constant_;
00249   housing_thermal_mass_inverse_ = 
00250     motor_params_.housing_to_ambient_thermal_resistance_ / motor_params_.housing_thermal_time_constant_; 
00251 }
00252 
00253 
00254 bool MotorHeatingModel::startTemperaturePublisher()
00255 {
00256   std::string topic("motor_temperature");
00257   if (!actuator_name_.empty())
00258   {
00259     topic = topic + "/" + actuator_name_;
00260     publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::MotorTemperature>(ros::NodeHandle(), topic, 1, true);
00261     if (publisher_ == NULL)
00262     {
00263       ROS_ERROR("Could not allocate realtime publisher");
00264       return false;
00265     }
00266   }  
00267   return true;
00268 }
00269 
00270 
00271 
00272 double MotorHeatingModel::calculateMotorHeatPower(const ethercat_hardware::MotorTraceSample &s, 
00273                                                   const ethercat_hardware::ActuatorInfo &ai)
00274 {
00275   // Normally the power being put into motor is I^2*R.  
00276   // However, the motor resistance changes with temperature
00277   // Instead this fuction breaks motor voltage into two parts
00278   //   1. back-EMF voltage
00279   //   2. resistance voltage
00280   // Then the power being put into motor is (resistance voltage * I)
00281 
00282   // Output voltage of MCB.  Guesstimate by multipling pwm ratio with supply voltage
00283   double output_voltage   = s.programmed_pwm * s.supply_voltage;
00284   // Back emf voltage of motor
00285   double backemf_constant = 1.0 / (ai.speed_constant * 2.0 * M_PI * 1.0/60.0);
00286   double backemf_voltage = s.velocity * ai.encoder_reduction * backemf_constant;
00287   // What ever voltage is left over must be the voltage used to drive current through the motor
00288   double resistance_voltage  = output_voltage - backemf_voltage;
00289 
00290   // Power going into motor as heat (Watts)
00291   double heating_power = resistance_voltage * s.measured_current;
00292 
00293   if (DEBUG_LEVEL) // Only use for debugging, (prints out debug info for unreasonable heating values)
00294   {
00295     if ((heating_power < -0.5) || (heating_power > 15.0))
00296     {
00297       ROS_DEBUG("heating power = %f, output_voltage=%f, backemf_voltage=%f, resistance_voltage=%f, current=%f",
00298                 heating_power, output_voltage, backemf_voltage, resistance_voltage, s.measured_current);
00299     }
00300   }
00301 
00302   // Heating power can never be less than 0.0
00303   heating_power = std::max(heating_power, 0.0);
00304 
00305   return heating_power;
00306 }
00307 
00308 
00309 bool MotorHeatingModel::update(double heating_power, double ambient_temperature, double duration)
00310 {
00311   // motor winding gains heat power (from motor current)
00312   // motor winding losses heat to motor housing
00313   // motor housing gets heat from winding and looses heat to ambient
00314   double heating_energy = heating_power * duration;
00315   double winding_energy_loss = 
00316     (winding_temperature_ - housing_temperature_) * winding_to_housing_thermal_conductance_ * duration;
00317   double housing_energy_loss = 
00318     (housing_temperature_ - ambient_temperature) * housing_to_ambient_thermal_conductance_ * duration;
00319 
00320   winding_temperature_ += (heating_energy      - winding_energy_loss) * winding_thermal_mass_inverse_;
00321   housing_temperature_ += (winding_energy_loss - housing_energy_loss) * housing_thermal_mass_inverse_;
00322 
00323   { // LOCKED
00324     boost::lock_guard<boost::mutex> lock(mutex_);
00325     heating_energy_sum_ += heating_energy;
00326     ambient_temperature_sum_ += (ambient_temperature * duration);
00327     duration_since_last_sample_ += duration;
00328     if (winding_temperature_ > motor_params_.max_winding_temperature_)
00329       overheat_ = true;
00330   } // LOCKED
00331    
00332   return !overheat_;
00333 }
00334 
00335 
00336 
00337 double MotorHeatingModel::updateFromDowntimeWithInterval(double downtime, 
00338                                                          double saved_ambient_temperature, 
00339                                                          double interval, 
00340                                                          unsigned cycles)
00341 {
00342   static const double heating_power = 0.0; // While motor was off heating power should be zero
00343   for (unsigned i=0; i<cycles; ++i)
00344   {
00345     if (downtime > interval)
00346     {
00347       update(heating_power, saved_ambient_temperature, interval);
00348       downtime -= interval;
00349     }
00350     else
00351     {
00352       // Done
00353       update(heating_power, saved_ambient_temperature, downtime);
00354       return 0.0;
00355     }
00356   }
00357   return downtime;
00358 }
00359 
00360 
00361 void MotorHeatingModel::updateFromDowntime(double downtime, double saved_ambient_temperature)
00362 {
00363   ROS_DEBUG("Initial temperatures : winding  = %f, housing = %f", winding_temperature_, housing_temperature_);
00364   double saved_downtime = downtime;
00365 
00366   boost::timer timer;
00367 
00368   // Simulate motor heating model first with increasing step sizes : 
00369   // first 10 ms, then 100ms, and finally 1 seconds steps
00370   downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.01, 200);
00371   downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 0.10, 200);
00372   downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 1.00, 200);
00373   downtime = updateFromDowntimeWithInterval(downtime, saved_ambient_temperature, 10.0, 2000);
00374 
00375   // If there is any time left over, assume motor has cooled to ambient
00376   if (downtime > 0.0)
00377   {
00378     ROS_DEBUG("Downtime too long, using ambient temperature as final motor temperature");
00379     winding_temperature_ = saved_ambient_temperature;
00380     housing_temperature_ = saved_ambient_temperature;
00381   }
00382 
00383   ROS_DEBUG("Took %f milliseconds to sim %f seconds", timer.elapsed()*1000., saved_downtime);
00384   ROS_DEBUG("Final temperatures : winding  = %f, housing = %f", winding_temperature_, housing_temperature_);
00385 }
00386 
00387 
00388 void MotorHeatingModel::reset()
00389 { 
00390   { // LOCKED
00391     boost::lock_guard<boost::mutex> lock(mutex_);
00392     overheat_ = false;
00393   } // LOCKED
00394 }
00395 
00396 
00397 void MotorHeatingModel::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00398 {
00399   // Sample motor temperature and heating energy every so often, this way the 
00400   // heating of the motor can be published when there is a problem
00401   bool overheat;
00402   double winding_temperature;
00403   double housing_temperature;
00404   double ambient_temperature_sum;
00405   double duration_since_last_sample;
00406   double average_ambient_temperature;
00407   double average_heating_power;
00408 
00409   { // LOCKED
00410     boost::lock_guard<boost::mutex> lock(mutex_);
00411     overheat = overheat_;
00412     winding_temperature = winding_temperature_;
00413     housing_temperature = housing_temperature_;
00414     ambient_temperature_sum = ambient_temperature_sum_;
00415     duration_since_last_sample = duration_since_last_sample_;
00416 
00417     if (duration_since_last_sample > 0.0)
00418     {
00419       average_ambient_temperature = ambient_temperature_sum / duration_since_last_sample;
00420       ambient_temperature_ = average_ambient_temperature;
00421       average_heating_power = heating_energy_sum_ / duration_since_last_sample;
00422     }
00423     else 
00424     {
00425       //ROS_WARN("Duration == 0");
00426       average_ambient_temperature = ambient_temperature_;
00427       average_heating_power = 0.0;
00428     }
00429     
00430     ambient_temperature_sum_ = 0.0;
00431     heating_energy_sum_  = 0.0;
00432     duration_since_last_sample_ = 0.0;
00433   } // LOCKED
00434 
00435 
00436 
00437   const int ERROR = 2;
00438   const int WARN = 1;
00439   //const int GOOD = 0;
00440 
00441   if (overheat)
00442   {
00443     // Once motor overheating flag is set, keep reporting motor has overheated, until the flag is reset.
00444     d.mergeSummary(ERROR, "Motor overheated");
00445   }
00446   else if (winding_temperature > (motor_params_.max_winding_temperature_ * 0.90))
00447   {
00448     // If motor winding temperature reaches 90% of limit, then make a warning
00449     d.mergeSummary(WARN, "Motor hot");
00450   }
00451   
00452   d.addf("Motor winding temp limit (C)", "%f", motor_params_.max_winding_temperature_);
00453   d.addf("Motor winding temp (C)", "%f", winding_temperature);
00454   d.addf("Motor housing temp (C)", "%f", housing_temperature);
00455 
00456   // TODO: removing this, probably not need for purposes other than debugging
00457   d.addf("Heating power (Watts)", "%f", average_heating_power);
00458   d.addf("Ambient temp (C)", "%f", average_ambient_temperature);
00459 
00460   
00461   if ((publisher_ != NULL) && (publisher_->trylock()))
00462   {    
00463     // Fill in sample values
00464     ethercat_hardware::MotorTemperature &msg(publisher_->msg_);
00465     msg.stamp = ros::Time::now();
00466     msg.winding_temperature = winding_temperature;
00467     msg.housing_temperature = housing_temperature;
00468     msg.ambient_temperature = average_ambient_temperature;
00469     msg.heating_power       = average_heating_power;
00470 
00471     // publish sample
00472     publisher_->unlockAndPublish();
00473   }
00474 }
00475 
00476 
00477 
00478 static bool getStringAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, std::string &value)
00479 {
00480   const char *val_str = elt->Attribute(param_name);
00481   if (val_str == NULL)
00482   {
00483     ROS_ERROR("No '%s' attribute for actuator '%s'", param_name, filename.c_str());
00484     return false;
00485   }
00486   value = val_str;
00487   return true;
00488 }
00489 
00490 
00491 static bool getDoubleAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, double &value)
00492 {
00493   const char *val_str = elt->Attribute(param_name);
00494   if (val_str == NULL)
00495   {
00496     ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
00497     return false;
00498   }
00499 
00500   char *endptr=NULL;
00501   value = strtod(val_str, &endptr);
00502   if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00503   {
00504     ROS_ERROR("Couldn't convert '%s' to double for attribute '%s' in '%s'", 
00505               val_str, param_name, filename.c_str());
00506     return false;
00507   }
00508 
00509   return true;
00510 }
00511 
00512 
00513 static bool getIntegerAttribute(TiXmlElement *elt, const std::string& filename, const char* param_name, int &value)
00514 {
00515   const char *val_str = elt->Attribute(param_name);
00516   if (val_str == NULL)
00517   {
00518     ROS_ERROR("No '%s' attribute in '%s'", param_name, filename.c_str());
00519     return false;
00520   }
00521 
00522   char *endptr=NULL;
00523   value = strtol(val_str, &endptr, 0);
00524   if ((endptr == val_str) || (endptr < (val_str+strlen(val_str))))
00525   {
00526     ROS_ERROR("Couldn't convert '%s' to integer for attribute '%s' in '%s'", 
00527               val_str, param_name, filename.c_str());
00528     return false;
00529   }
00530 
00531   return true;
00532 }
00533 
00534 
00535 static void saturateTemperature(double &temperature, const char *name)
00536 {
00537   static const double max_realistic_temperature = 200.0;
00538   static const double min_realistic_temperature = -10.0;
00539 
00540   if (temperature > max_realistic_temperature)
00541   {
00542     ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
00543                 name, temperature, max_realistic_temperature);
00544     temperature = max_realistic_temperature;
00545   }
00546   if (temperature < min_realistic_temperature)
00547   {
00548     ROS_WARN("%s temperature of %f Celcius is unrealisic. Using %f instead",
00549                 name, temperature, min_realistic_temperature);
00550     temperature = min_realistic_temperature;
00551   }
00552 }
00553 
00554 
00555 bool MotorHeatingModel::loadTemperatureState()
00556 {
00557   /*
00558    * The tempature save file will be named after the actuator it was created for
00559    *   <actuactor_name>_motor_heating_model.save
00560    *
00561    * The temperatureState file will have a XML format:
00562    * <motor_heating_model
00563    *   version = "1"
00564    *   actuator_name = "fl_caster_r_wheel_motor"
00565    *   winding_temperature = "100.0"
00566    *   housing_temperature = "100.0" 
00567    *   save_time = "<wall_time when file was lasted saved>"
00568    *   checksum  = "<CRC32 of all previous lines in file>, to allow user editing of files checksum can be "IGNORE-CRC"
00569    * />
00570    *
00571    */
00572 
00573   if (!boost::filesystem::exists(save_filename_))
00574   {
00575     ROS_WARN("Motor heating model saved file '%s' does not exist.  Using defaults", save_filename_.c_str());
00576     return false;
00577   }
00578 
00579   TiXmlDocument xml;
00580   if (!xml.LoadFile(save_filename_))
00581   {
00582     ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
00583     return false;
00584   }
00585 
00586   
00587   TiXmlElement *motor_temp_elt = xml.RootElement();
00588   if (motor_temp_elt == NULL)
00589   {
00590     ROS_ERROR("Unable to parse XML in save file '%s'", save_filename_.c_str());
00591     return false;
00592   }
00593       
00594   std::string version;
00595   std::string actuator_name;
00596   std::string hwid;
00597   double winding_temperature;
00598   double housing_temperature;
00599   double ambient_temperature;
00600   int save_time_sec, save_time_nsec;
00601   if (!getStringAttribute(motor_temp_elt, save_filename_, "version", version))
00602   {
00603     return false;
00604   }
00605   const char* expected_version = "1";
00606   if (version != expected_version)
00607   {
00608     ROS_ERROR("Unknown version '%s', expected '%s'", version.c_str(), expected_version);
00609     return false;
00610   }
00611   
00612   bool success = true;
00613   success &= getStringAttribute(motor_temp_elt, save_filename_, "actuator_name", actuator_name);
00614   success &= getStringAttribute(motor_temp_elt, save_filename_, "hwid", hwid);
00615   success &= getDoubleAttribute(motor_temp_elt, save_filename_, "housing_temperature", housing_temperature);
00616   success &= getDoubleAttribute(motor_temp_elt, save_filename_, "winding_temperature", winding_temperature);
00617   success &= getDoubleAttribute(motor_temp_elt, save_filename_, "ambient_temperature", ambient_temperature);
00618   success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_sec", save_time_sec);
00619   success &= getIntegerAttribute(motor_temp_elt, save_filename_, "save_time_nsec", save_time_nsec);
00620   if (!success)
00621   {
00622     return false;
00623   }
00624 
00625   if (actuator_name != actuator_name_)
00626   {
00627     ROS_ERROR("In save file '%s' : expected actuator name '%s', got '%s'", 
00628               save_filename_.c_str(), actuator_name_.c_str(), actuator_name.c_str());
00629     return false;
00630   }  
00631 
00632   if (hwid != hwid_)
00633   {
00634     ROS_WARN("In save file '%s' : expected HWID '%s', got '%s'", 
00635               save_filename_.c_str(), hwid_.c_str(), hwid.c_str());    
00636   }
00637 
00638   saturateTemperature(housing_temperature, "Housing");
00639   saturateTemperature(winding_temperature, "Winding");
00640   saturateTemperature(ambient_temperature, "Ambient");
00641 
00642   // Update stored temperatures
00643   winding_temperature_ = winding_temperature;
00644   housing_temperature_ = housing_temperature;
00645   ambient_temperature_ = ambient_temperature;
00646 
00647   // Update motor temperature model based on saved time, update time, and saved ambient temperature
00648   double downtime = (ros::Time::now() - ros::Time(save_time_sec, save_time_nsec)).toSec();
00649   if (downtime < 0.0)
00650   {
00651     ROS_WARN("In save file '%s' : save time is %f seconds in future", save_filename_.c_str(), -downtime);
00652   }
00653   else
00654   {
00655     updateFromDowntime(downtime, ambient_temperature);
00656   }
00657 
00658   // Make sure simulation didn't go horribly wrong
00659   saturateTemperature(housing_temperature_, "(2) Housing");
00660   saturateTemperature(winding_temperature_, "(2) Winding");
00661 
00662   return true;
00663 }
00664 
00665 
00666 
00667 
00668 
00669 bool MotorHeatingModel::saveTemperatureState()
00670 {
00671   /*
00672    * This will first generate new XML data into temp file, then rename temp file to final filename
00673    *
00674    */
00675   std::string tmp_filename = save_filename_ + ".tmp";
00676 
00677   double winding_temperature;
00678   double housing_temperature;
00679   double ambient_temperature;
00680   { // LOCKED
00681     boost::lock_guard<boost::mutex> lock(mutex_);
00682     winding_temperature = winding_temperature_;
00683     housing_temperature = housing_temperature_;
00684     ambient_temperature = ambient_temperature_;
00685   } // LOCKED
00686 
00687   TiXmlDocument xml;
00688   TiXmlDeclaration *decl = new TiXmlDeclaration( "1.0", "", "" );
00689   TiXmlElement *elmt = new TiXmlElement("motor_heating_model");
00690   elmt->SetAttribute("version", "1");
00691   elmt->SetAttribute("actuator_name", actuator_name_);
00692   elmt->SetAttribute("hwid", hwid_);
00693   elmt->SetDoubleAttribute("winding_temperature", winding_temperature);
00694   elmt->SetDoubleAttribute("housing_temperature", housing_temperature);
00695   elmt->SetDoubleAttribute("ambient_temperature", ambient_temperature);
00696   ros::Time now = ros::Time::now();
00697   elmt->SetAttribute("save_time_sec", now.sec);
00698   elmt->SetAttribute("save_time_nsec", now.nsec);
00699 
00700   xml.LinkEndChild(decl);
00701   xml.LinkEndChild( elmt );
00702 
00703   // save xml with temporary filename
00704   if (!xml.SaveFile(tmp_filename))
00705   {
00706     ROS_WARN("Could not save motor heating model file '%s'", tmp_filename.c_str());
00707     return false;
00708   }
00709 
00710   // now rename file
00711   if (rename(tmp_filename.c_str(), save_filename_.c_str()) != 0)
00712   {
00713     int error = errno;
00714     char errbuf[100];
00715     strerror_r(error, errbuf, sizeof(errbuf));
00716     errbuf[sizeof(errbuf)-1] = '\0';
00717     ROS_WARN("Problem renaming '%s' to '%s' : (%d) '%s'", 
00718              tmp_filename.c_str(), save_filename_.c_str(), error, errbuf);
00719     return false;
00720   }
00721 
00722   return true;
00723 }
00724 
00725 
00726 
00727 };  // end namespace ethercat_hardware


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:45