FRIDiagnostics.cpp
Go to the documentation of this file.
00001 
00002 #include <rtt/TaskContext.hpp>
00003 #include <rtt/Port.hpp>
00004 #include <rtt/Component.hpp>
00005 
00006 // Start of user code includes
00007 #include <diagnostic_msgs/typekit/Types.h>
00008 #include <kuka_lwr_fri/friComm.h>
00009 #include <boost/lexical_cast.hpp>
00010 #include <string>
00011 #include <bitset>
00012 // End of user code
00013 
00014 class FRIDiagnostics : public RTT::TaskContext {
00015 public:
00016   FRIDiagnostics(const std::string & name) : TaskContext(name) {
00017 
00018     prop_prefix = "lwr";
00019         
00020     this->addProperty("prefix", prop_prefix);
00021 
00022     this->ports()->addPort("FRIState", port_FRIState).doc("");
00023     this->ports()->addPort("RobotState", port_RobotState).doc("");
00024 
00025     this->ports()->addPort("Diagnostics", port_Diagnostics).doc("");
00026   }
00027 
00028   ~FRIDiagnostics(){
00029   }
00030 
00031   bool configureHook() {
00032     // Start of user code configureHook
00033     diagnostic_.status.resize(2);
00034     diagnostic_.status[0].values.resize(11);
00035     diagnostic_.status[1].values.resize(4);
00036 
00037     diagnostic_.status[0].name = prop_prefix + " FRI state";
00038     diagnostic_.status[0].values[0].key = "Kuka System Time";
00039     diagnostic_.status[0].values[1].key = "State";
00040     diagnostic_.status[0].values[2].key = "Quality";
00041     diagnostic_.status[0].values[3].key = "Desired Send Sample Time";
00042     diagnostic_.status[0].values[4].key = "Desired Command Sample Time";
00043     diagnostic_.status[0].values[5].key = "Safety Limits";
00044     diagnostic_.status[0].values[6].key = "Answer Rate";
00045     diagnostic_.status[0].values[7].key = "Latency";
00046     diagnostic_.status[0].values[8].key = "Jitter";
00047     diagnostic_.status[0].values[9].key = "Average Missed Answer Packages";
00048     diagnostic_.status[0].values[10].key = "Total Missed Packages";
00049 
00050     diagnostic_.status[1].name = prop_prefix + " robot state";
00051     diagnostic_.status[1].values[0].key = "Power";
00052     diagnostic_.status[1].values[1].key = "Control Strategy";
00053     diagnostic_.status[1].values[2].key = "Error";
00054     diagnostic_.status[1].values[3].key = "Warning";
00055     // End of user code
00056     return true;
00057   }
00058 
00059   bool startHook() {
00060     // Start of user code startHook
00061     // TODO Put implementation of startHook here !!!
00062     // End of user code
00063     return true;
00064   }
00065 
00066   void stopHook() {
00067     // Start of user code stopHook
00068           // TODO Put implementation of stopHook here !!!
00069           // End of user code
00070   }
00071 
00072   void updateHook() {
00073         if( true) {
00074       doDiagnostics();
00075     }
00076   }
00077 
00078 private:
00079 
00080   void doDiagnostics() {
00081     // Start of user code Diagnostics
00082     tFriIntfState fristate;
00083     tFriRobotState robotstate;
00084 
00085     port_FRIState.read(fristate);
00086     port_RobotState.read(robotstate);
00087 
00088     fri_comm_diagnostics(fristate);
00089     fri_robot_diagnostics(robotstate);
00090 
00091     port_Diagnostics.write(diagnostic_);
00092           // End of user code
00093   }
00094 
00095 
00096   RTT::InputPort<tFriIntfState > port_FRIState;
00097   RTT::InputPort<tFriRobotState > port_RobotState;
00098 
00099   RTT::OutputPort<diagnostic_msgs::DiagnosticArray > port_Diagnostics;
00100 
00101   std::string prop_prefix;
00102 
00103 
00104   // Start of user code userData
00105   diagnostic_msgs::DiagnosticArray diagnostic_;
00106 
00107   void fri_comm_diagnostics(const tFriIntfState &fristate){
00108 
00109     if(fristate.quality==FRI_QUALITY_PERFECT){
00110       diagnostic_.status[0].level = diagnostic_msgs::DiagnosticStatus::OK;
00111       diagnostic_.status[0].message = "Communication quality PERFECT";
00112       diagnostic_.status[0].values[2].value = "PERFECT";
00113     }else if(fristate.quality==FRI_QUALITY_OK){
00114       diagnostic_.status[0].level = diagnostic_msgs::DiagnosticStatus::OK;
00115       diagnostic_.status[0].message = "Communication quality OK";
00116       diagnostic_.status[0].values[2].value = "OK";
00117     }else if(fristate.quality==FRI_QUALITY_BAD){
00118       diagnostic_.status[0].level = diagnostic_msgs::DiagnosticStatus::WARN;
00119       diagnostic_.status[0].message = "Communication quality BAD";
00120       diagnostic_.status[0].values[2].value = "BAD";
00121     }else{
00122       diagnostic_.status[0].level = diagnostic_msgs::DiagnosticStatus::ERROR;
00123       diagnostic_.status[0].message = "Communication quality UNACCEPTABLE";
00124       diagnostic_.status[0].values[2].value = "UNACCEPTABLE";
00125     }
00126 
00127     diagnostic_.status[0].values[0].value = boost::lexical_cast<std::string>(fristate.timestamp);
00128 
00129     if(fristate.state == FRI_STATE_MON){
00130       diagnostic_.status[0].values[1].value = "monitor";
00131     }else if(fristate.state == FRI_STATE_CMD){
00132       diagnostic_.status[0].values[1].value = "command";
00133     }else{
00134       diagnostic_.status[0].values[1].value = "invalid";
00135     }
00136 
00137     diagnostic_.status[0].values[3].value = boost::lexical_cast<std::string>(fristate.desiredMsrSampleTime);
00138     diagnostic_.status[0].values[4].value = boost::lexical_cast<std::string>(fristate.desiredCmdSampleTime);
00139     diagnostic_.status[0].values[5].value = boost::lexical_cast<std::string>(fristate.safetyLimits);
00140     diagnostic_.status[0].values[6].value = boost::lexical_cast<std::string>(fristate.stat.answerRate);
00141     diagnostic_.status[0].values[7].value = boost::lexical_cast<std::string>(fristate.stat.latency);
00142     diagnostic_.status[0].values[8].value = boost::lexical_cast<std::string>(fristate.stat.jitter);
00143     diagnostic_.status[0].values[9].value = boost::lexical_cast<std::string>(fristate.stat.missRate);
00144     diagnostic_.status[0].values[10].value = boost::lexical_cast<std::string>(fristate.stat.missCounter);
00145 
00146 //    stat.add("Kuka System Time",fristate.timestamp);
00147 //    stat.add("State",fristate.state);
00148 //    stat.add("Quality",fristate.quality);
00149 //    stat.add("Desired Send Sample Time",fristate.desiredMsrSampleTime);
00150 //    stat.add("Desired Command Sample Time",fristate.desiredCmdSampleTime);
00151 //    stat.add("Safety Limits",fristate.safetyLimits);
00152 //    stat.add("Answer Rate",fristate.stat.answerRate);
00153 //    stat.add("Latency",fristate.stat.latency);
00154 //    stat.add("Jitter",fristate.stat.jitter);
00155 //    stat.add("Average Missed Answer Packages",fristate.stat.missRate);
00156 //    stat.add("Total Missed Packages",fristate.stat.missCounter);
00157   }
00158 
00159   void fri_robot_diagnostics(const tFriRobotState &robotstate){
00160 
00161     std::bitset<7> power(robotstate.power);
00162     std::bitset<7> error(robotstate.error);
00163     std::bitset<7> warning(robotstate.warning);
00164 
00165     std::bitset<7> clear;
00166     if(clear==error){
00167       if(clear==warning){
00168         diagnostic_.status[1].level = diagnostic_msgs::DiagnosticStatus::OK;
00169         diagnostic_.status[1].message = "All drives OK";
00170       }else{
00171         diagnostic_.status[1].level = diagnostic_msgs::DiagnosticStatus::WARN;
00172         diagnostic_.status[1].message = "Drives warning";
00173       }
00174     }else{
00175       diagnostic_.status[1].level = diagnostic_msgs::DiagnosticStatus::ERROR;
00176       diagnostic_.status[1].message = "Drives Error";
00177     }
00178 
00179     diagnostic_.status[1].values[0].value = power.to_string();
00180 
00181     if(robotstate.control == FRI_CTRL_POSITION){
00182       diagnostic_.status[1].values[1].value = "Position";
00183     } else if(robotstate.control == FRI_CTRL_CART_IMP){
00184       diagnostic_.status[1].values[1].value = "Cartesian impedance";
00185     } else if(robotstate.control == FRI_CTRL_JNT_IMP){
00186       diagnostic_.status[1].values[1].value = "Joint impedance";
00187     } else {
00188       diagnostic_.status[1].values[1].value = "Invalid";
00189     }
00190 
00191     diagnostic_.status[1].values[2].value = error.to_string();
00192     diagnostic_.status[1].values[3].value = error.to_string();
00193 
00194 //    stat.add("Power",power.to_string());
00195 //    stat.add("Control Strategy",robotstate.control);
00196 //    stat.add("Error",error.to_string());
00197 //    stat.add("Warning",warning.to_string());
00198 //    stat.add("Temperature Joint 1",robotstate.temperature[0]);
00199 //    stat.add("Temperature Joint 2",robotstate.temperature[1]);
00200 //    stat.add("Temperature Joint 3",robotstate.temperature[2]);
00201 //    stat.add("Temperature Joint 4",robotstate.temperature[3]);
00202 //    stat.add("Temperature Joint 5",robotstate.temperature[4]);
00203 //    stat.add("Temperature Joint 6",robotstate.temperature[5]);
00204 //    stat.add("Temperature Joint 7",robotstate.temperature[6]);
00205 
00206   }
00207 
00208   // End of user code
00209 
00210 };
00211 
00212 ORO_CREATE_COMPONENT(FRIDiagnostics)
00213 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


lwr_fri
Author(s): Ruben Smits
autogenerated on Thu Nov 14 2013 11:55:34