naoqi_joints_analyzer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright 2011 Stefan Osswald, University of Freiburg
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 University of Freiburg 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 "naoqi_joints_analyzer.h"
00036 
00037 #include <sstream>
00038 #include <fstream>
00039 
00040 PLUGINLIB_DECLARE_CLASS(nao_dashboard, NaoqiJointsAnalyzer,
00041                 diagnostic_aggregator::NaoqiJointsAnalyzer,
00042                 diagnostic_aggregator::Analyzer)
00043 
00044 namespace diagnostic_aggregator {
00045 
00049 NaoqiJointsAnalyzer::NaoqiJointsAnalyzer() :
00050         m_path(""), m_niceName("Joints"), m_lastSeen(0) {
00051 }
00052 
00056 NaoqiJointsAnalyzer::~NaoqiJointsAnalyzer() {
00057 }
00058 
00059 bool NaoqiJointsAnalyzer::init(const std::string base_name, const ros::NodeHandle &n) {
00060         if (!n.getParam("path", m_niceName))
00061         {
00062                 ROS_ERROR("NaoJointsAnalyzer was not given parameter \"path\". Namespace: %s",
00063                           n.getNamespace().c_str());
00064                 return false;
00065         }
00066 
00067         m_path = base_name;
00068 
00069         boost::shared_ptr<StatusItem> item(new StatusItem(m_niceName));
00070         m_jointsMasterItem = item;
00071 
00072         return true;
00073 }
00074 
00075 bool NaoqiJointsAnalyzer::match(const std::string name) {
00076         return name.find("nao_joint") == 0;
00077 }
00078 
00079 bool NaoqiJointsAnalyzer::analyze(const boost::shared_ptr<StatusItem> item) {
00080         if(item->getName().find("nao_joint") != 0)
00081                 return false;
00082 
00083         JointData data;
00084 
00085         std::stringstream ssStiffness(item->getValue("Stiffness"));
00086         ssStiffness >> data.stiffness;
00087 
00088         std::stringstream ssTemperature(item->getValue("Temperature"));
00089         ssTemperature >> data.temperature;
00090 
00091         data.status = item;
00092 
00093         if(m_joints.find(item->getName()) == m_joints.end())
00094                 m_joints.insert(make_pair(item->getName(), data));
00095         else
00096                 m_joints.at(item->getName()) = data;
00097 
00098         m_lastSeen = ros::Time::now();
00099 
00100         return true;
00101 }
00102 
00103 bool NaoqiJointsAnalyzer::compareByTemperature(const NaoqiJointsAnalyzer::JointData& a, const NaoqiJointsAnalyzer::JointData& b) {
00104     return (a.temperature > b.temperature);
00105 }
00106 
00114 template<typename T>
00115 void NaoqiJointsAnalyzer::addValue(boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> joint_stat, const std::string& key, const T& value) const {
00116         std::stringstream ss;
00117         ss << value;
00118         diagnostic_msgs::KeyValue kv;
00119         kv.key = key;
00120         kv.value = ss.str();
00121         joint_stat->values.push_back(kv);
00122 }
00123 
00124 std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > NaoqiJointsAnalyzer::report() {
00125     bool stale = (ros::Time::now()-m_lastSeen).toSec() > 5.0;
00126         boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> joint_stat =
00127                         m_jointsMasterItem->toStatusMsg(m_path, stale);
00128 
00129         std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
00130         if(stale) {
00131                 output.push_back(joint_stat);
00132                 return output;
00133         }
00134 
00135         double maxTemperature = 0.0;
00136         double maxStiffness = 0.0;
00137         double minStiffness = 1.0;
00138         double minStiffnessWoHands = 1.0;
00139 
00140         std::vector<JointData> hotJoints;
00141         joint_stat->level = diagnostic_msgs::DiagnosticStatus::OK;
00142 
00143         for(JointsMapType::const_iterator it = m_joints.begin(); it != m_joints.end(); it++) {
00144                 if(it->first.find("RHipYawPitch") == std::string::npos) {
00145                         maxTemperature = std::max(maxTemperature, it->second.temperature);
00146                         maxStiffness = std::max(maxStiffness, it->second.stiffness);
00147                         minStiffness = std::min(minStiffness, it->second.stiffness);
00148                         if(it->first.find("Hand") == std::string::npos)
00149                                 minStiffnessWoHands = std::min(minStiffnessWoHands, it->second.stiffness);
00150                         if((int) it->second.status->getLevel() >= (int) diagnostic_msgs::DiagnosticStatus::WARN) {
00151                             hotJoints.push_back(it->second);
00152                         }
00153                         if(it->second.status->getLevel() > joint_stat->level)
00154                             joint_stat->level = it->second.status->getLevel();
00155                 }
00156         }
00157 
00158         addValue(joint_stat, "Highest Temperature", maxTemperature);
00159         addValue(joint_stat, "Highest Stiffness", maxStiffness);
00160         addValue(joint_stat, "Lowest Stiffness", minStiffness);
00161         addValue(joint_stat, "Lowest Stiffness without Hands", minStiffnessWoHands);
00162 
00163     std::sort(hotJoints.begin(), hotJoints.end(), NaoqiJointsAnalyzer::compareByTemperature);
00164     std::stringstream hotJointsSS;
00165     for(size_t i = 0; i < hotJoints.size(); i++) {
00166         hotJointsSS << std::endl << removeLeadingNameChaff(hotJoints[i].status->getName(), "nao_joint") << ": " << hotJoints[i].temperature << "°C";
00167     }
00168     addValue(joint_stat, "Hot Joints", hotJointsSS.str());
00169 
00170         if (joint_stat->level == diagnostic_msgs::DiagnosticStatus::OK) {
00171                 joint_stat->message = "OK";
00172         } else if (joint_stat->level == diagnostic_msgs::DiagnosticStatus::WARN) {
00173                 joint_stat->message = "WARN";
00174         } else {
00175                 joint_stat->message = "ERROR";
00176         }
00177 
00178         output.push_back(joint_stat);
00179 
00180         for(JointsMapType::const_iterator it = m_joints.begin(); it != m_joints.end(); it++) {
00181                 boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> msg(it->second.status->toStatusMsg(m_path + "/" + m_niceName,
00182                                 (ros::Time::now() - it->second.status->getLastUpdateTime()).toSec() > 3.0));
00183                 msg->name = m_path + "/" + m_niceName + "/" + removeLeadingNameChaff(it->second.status->getName(), "nao_joint: ");
00184                 output.push_back(msg);
00185         }
00186 
00187         return output;
00188 }
00189 }


naoqi_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Severin Lemaignan
autogenerated on Fri Jul 3 2015 12:51:45