sr_diagnosticer.cpp
Go to the documentation of this file.
00001 
00030 //ROS include
00031 #include <ros/ros.h>
00032 
00033 //messages
00034 #include <diagnostic_msgs/DiagnosticStatus.h>
00035 #include <diagnostic_msgs/DiagnosticArray.h>
00036 
00037 //generic C/C++ include
00038 #include <vector>
00039 #include <string>
00040 #include <sstream>
00041 
00042 #include <boost/smart_ptr.hpp>
00043 
00044 #include "sr_hand/sr_diagnosticer.h"
00045 
00046 using namespace ros;
00047 using namespace shadowrobot;
00048 
00049 namespace shadowrobot
00050 {
00051 
00052 // 9 is the number of messages sent on the palm: used in
00053 // converting rate to Hz
00054   const double SRDiagnosticer::palm_numb_msg_const = 9.0;
00055   const double SRDiagnosticer::palm_msg_rate_const = 4000.0;
00056 
00058 //    CONSTRUCTOR/DESTRUCTOR   //
00060 
00061   SRDiagnosticer::SRDiagnosticer( boost::shared_ptr<SRArticulatedRobot> sr_art_robot, hardware_types hw_type ) :
00062     n_tilde("~"), publish_rate(0.0)
00063   {
00064     sr_articulated_robot = sr_art_robot;
00065 
00066     // set publish frequency
00067     double publish_freq;
00068     n_tilde.param("publish_frequency_diagnostics", publish_freq, 1.0);
00069     publish_rate = Rate(publish_freq);
00070 
00071     //publishes /diagnostics messages
00072     sr_diagnostics_pub = node.advertise<diagnostic_msgs::DiagnosticArray> ("diagnostics", 2);
00073 
00074     hardware_type = hw_type;
00075   }
00076 
00077   SRDiagnosticer::~SRDiagnosticer()
00078   {
00079     //if( shadowhand != NULL )
00080     // delete shadowhand;
00081   }
00082 
00084 //       PUBLISH METHOD        //
00086   void SRDiagnosticer::publish()
00087   {
00088     diagnostic_msgs::DiagnosticArray diag_msg;
00089 
00090     std::vector<diagnostic_msgs::DiagnosticStatus> vec_diag_msg;
00091 
00092     std::vector<DiagnosticData> diagnostics = sr_articulated_robot->getDiagnostics();
00093 
00094     std::stringstream ss;
00095 
00096     for( unsigned int i = 0; i < diagnostics.size(); ++i )
00097     {
00098       diagnostic_msgs::DiagnosticStatus diag;
00099 
00100       std::vector<diagnostic_msgs::KeyValue> keyvalues;
00101 
00102       diag.level = diagnostics[i].level;
00103 
00104       switch( hardware_type )
00105       {
00106       case sr_hand_hardware:
00107         diag.name = "srh/" + diagnostics[i].joint_name;
00108         break;
00109       case sr_arm_hardware:
00110         diag.name = "sr_arm/" + diagnostics[i].joint_name;
00111         break;
00112       default:
00113         diag.name = diagnostics[i].joint_name;
00114         break;
00115       }
00116 
00117       diagnostic_msgs::KeyValue keyval;
00118       keyval.key = "Target";
00119 
00120       ss.str("");
00121       ss << diagnostics[i].target;
00122       keyval.value = ss.str();
00123       keyvalues.push_back(keyval);
00124 
00125       keyval.key = "Position";
00126       ss.str("");
00127       ss << diagnostics[i].position;
00128       keyval.value = ss.str();
00129       keyvalues.push_back(keyval);
00130 
00131       keyval.key = "Flags";
00132       ss.str("");
00133       ss << diagnostics[i].flags;
00134       keyval.value = ss.str();
00135       keyvalues.push_back(keyval);
00136 
00137       //get all the debug values
00138       std::map<const std::string, const unsigned int>::const_iterator iter;
00139       for(iter = debug_values::names_and_offsets.begin();
00140           iter !=  debug_values::names_and_offsets.end(); ++iter)
00141       {
00142         keyval.key = iter->first;
00143         ss.str("");
00144         ss << diagnostics[i].debug_values[iter->first];
00145         keyval.value = ss.str();
00146         keyvalues.push_back(keyval);
00147       }
00148       if( diag.level == 0 )
00149         diag.message = "OK";
00150 
00151       diag.values = keyvalues;
00152       vec_diag_msg.push_back(diag);
00153     }
00154 
00155     //set the standard message
00156     diag_msg.status = vec_diag_msg;
00157     //publish the diagnostic data
00158 
00159     diag_msg.header.stamp = ros::Time::now();
00160     sr_diagnostics_pub.publish(diag_msg);
00161 
00162     ros::spinOnce();
00163     publish_rate.sleep();
00164   }
00165 
00166 }// end namespace
00167 
00168 
00169 /* For the emacs weenies in the crowd.
00170 Local Variables:
00171    c-basic-offset: 2
00172 End:
00173 */
00174 


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:08:51