pr2_mechanism_diagnostics.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 
00036 #include "pr2_mechanism_diagnostics/pr2_mechanism_diagnostics.h"
00037 #include <exception>
00038 #include <limits>
00039 #include "angles/angles.h"
00040 
00041 using namespace pr2_mechanism_diagnostics;
00042 using namespace std;
00043 
00044 // Diagnostic publisher
00045 CtrlJntDiagnosticPublisher::CtrlJntDiagnosticPublisher() :
00046   pnh_("~"),
00047   use_sim_time_(false),
00048   disable_controller_warnings_(false)
00049 { 
00050   mech_st_sub_ = n_.subscribe("mechanism_statistics", 1000, &CtrlJntDiagnosticPublisher::mechCallback, this);
00051   diag_pub_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00052   
00053   n_.param("/use_sim_time", use_sim_time_, false);
00054 
00055   pnh_.param("disable_controller_warnings", disable_controller_warnings_, false);
00056 }
00057 
00058 
00059 
00060 
00061 
00062 void CtrlJntDiagnosticPublisher::mechCallback(const pr2_mechanism_msgs::MechanismStatistics::ConstPtr& mechMsg)
00063 {
00064   // Update joints
00065   vector<pr2_mechanism_msgs::JointStatistics>::const_iterator it;
00066   for (it = mechMsg->joint_statistics.begin(); it != mechMsg->joint_statistics.end(); ++it)
00067   {
00068     if (joint_stats.count(it->name) == 0)
00069       joint_stats[it->name] = boost::shared_ptr<JointStats>(new JointStats(it->name));
00070     
00071     joint_stats[it->name]->update(*it);
00072   }
00073   
00074   // Update controllers
00075   vector<pr2_mechanism_msgs::ControllerStatistics>::const_iterator c_it;
00076   for (c_it = mechMsg->controller_statistics.begin(); c_it != mechMsg->controller_statistics.end(); ++c_it)
00077   {
00078     if (controller_stats.count(c_it->name) == 0)
00079       controller_stats[c_it->name] = boost::shared_ptr<ControllerStats>(new ControllerStats(c_it->name, use_sim_time_ || disable_controller_warnings_));
00080     
00081     controller_stats[c_it->name]->update(*c_it);
00082   }
00083 }
00084 
00085 void CtrlJntDiagnosticPublisher::publishDiag()
00086 {
00087   diagnostic_msgs::DiagnosticArray array;
00088   
00089   // Update joints
00090   map<string, boost::shared_ptr<JointStats> >::iterator it;
00091   for (it = joint_stats.begin(); it != joint_stats.end(); ++it)
00092     array.status.push_back(*(it->second->toDiagStat()));
00093   
00094   // Update controllers. Note controllers that haven't update are discarded
00095   map<string, boost::shared_ptr<ControllerStats> >::iterator c_it;
00096   unsigned int num_controllers = 0;
00097   vector<string> erase_controllers;
00098   for (c_it = controller_stats.begin(); c_it != controller_stats.end(); ++c_it)
00099   {
00100     if (c_it->second->shouldDiscard())
00101       erase_controllers.push_back(c_it->first);
00102     
00103     array.status.push_back(*(c_it->second->toDiagStat()));
00104     num_controllers++;
00105   }
00106   for (unsigned int i=0; i<erase_controllers.size(); i++)
00107     controller_stats.erase(erase_controllers[i]);
00108   
00109   // Publish even if we have no controllers loaded
00110   if (num_controllers == 0)
00111   {
00112     diagnostic_updater::DiagnosticStatusWrapper stat;
00113     stat.name = "Controller: No controllers loaded in controller manager";
00114     array.status.push_back(stat);
00115   }
00116 
00117   array.header.stamp = ros::Time::now();
00118   diag_pub_.publish(array);
00119 }
00120 
00121 
00122 int main(int argc, char **argv)
00123 {
00124   ros::init(argc, argv, "pr2_mechanism_diagnostics");
00125   
00126   try
00127   {
00128     CtrlJntDiagnosticPublisher ctrlJntPub;
00129     
00130     ros::Rate pub_rate(1.0);
00131     while (ctrlJntPub.ok())
00132     {
00133       pub_rate.sleep();
00134       ros::spinOnce();
00135       ctrlJntPub.publishDiag();
00136     }
00137   }
00138   catch (exception& e)
00139   {
00140     ROS_FATAL("Controllers/Joint to Diagnostics node caught exception. Aborting. %s", e.what());
00141     ROS_BREAK();
00142   }
00143   
00144   exit(0);
00145   return 0;
00146 }


pr2_mechanism_diagnostics
Author(s): Kevin Watts
autogenerated on Thu Dec 12 2013 12:04:01