$search
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