Monitor.cpp
Go to the documentation of this file.
00001 #include <rtm/CorbaNaming.h>
00002 #include "Monitor.h"
00003 #include "hrpsys/util/OpenRTMUtil.h"
00004 #include "GLscene.h"
00005 
00006 Monitor::Monitor(CORBA::ORB_var orb, const std::string &i_hostname,
00007                  int i_port, int i_interval, LogManager<TimedRobotState> *i_log) :
00008     m_orb(orb),
00009     m_rhCompName("RobotHardware0"),
00010     m_shCompName("StateHolder0"),
00011     m_log(i_log),
00012     m_interval(i_interval)
00013 {
00014     char buf[128];
00015     try {
00016         sprintf(buf, "%s:%d", i_hostname.c_str(), i_port);
00017         RTC::CorbaNaming naming(orb, buf);
00018         m_naming = CosNaming::NamingContext::_duplicate(naming.getRootContext());
00019     }catch (CORBA::SystemException& ex) {
00020         std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << ex._rep_id() << " with " << buf << std::endl;
00021     }catch (const std::string& error){
00022         std::cerr << "[monitor] Failed to initialize CORBA " << std::endl << error << " with " << buf << std::endl;
00023     }catch (...){
00024         std::cerr << "[monitor] Failed to initialize CORBA with " << buf << std::endl;
00025     }
00026 }
00027 
00028 bool Monitor::oneStep()
00029 {
00030     static long long loop = 0;
00031     ThreadedObject::oneStep();
00032 
00033     // RobotHardwareService
00034     if (CORBA::is_nil(m_rhService)){
00035         try{
00036             CosNaming::Name name;
00037             name.length(1);
00038             name[0].id = CORBA::string_dup(m_rhCompName.c_str());
00039             name[0].kind = CORBA::string_dup("rtc");
00040             CORBA::Object_var obj = m_naming->resolve(name);
00041             RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
00042             RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
00043             for(CORBA::ULong i=0; i < eclist->length(); ++i){
00044                 eclist[i]->activate_component(rtc);
00045             }
00046             const char *ior = getServiceIOR(rtc, "RobotHardwareService");
00047             m_rhService = OpenHRP::RobotHardwareService::_narrow(m_orb->string_to_object(ior));
00048         }catch(...){
00049             if ( (loop%(5*(1000/m_interval))) == 0 )
00050                 std::cerr << "[monitor] RobotHardwareService could not connect (" << m_rhCompName << ")" << std::endl;
00051         }
00052     } else {
00053         if ( (loop%(5*(1000/m_interval))) == 0 )
00054             std::cerr << "[monitor] RobotHardwareService is not found (" << m_rhCompName << ")" << std::endl;
00055     }
00056     // StateHolderService
00057     if (CORBA::is_nil(m_shService)){
00058         try{
00059             CosNaming::Name name;
00060             name.length(1);
00061             name[0].id = CORBA::string_dup(m_shCompName.c_str());
00062             name[0].kind = CORBA::string_dup("rtc");
00063             CORBA::Object_var obj = m_naming->resolve(name);
00064             RTC::RTObject_var rtc = RTC::RTObject::_narrow(obj);
00065             const char *ior = getServiceIOR(rtc, "StateHolderService");
00066             m_shService = OpenHRP::StateHolderService::_narrow(m_orb->string_to_object(ior));
00067         }catch(...){
00068             if ( (loop%(5*(1000/m_interval))) == 0 )
00069             std::cerr << "[monitor] StateHolderService could not connect (" << m_shCompName << ")" << std::endl;
00070         }
00071     }else{
00072         if ( (loop%(5*(1000/m_interval))) == 0 )
00073             std::cerr << "[monitor] StateHolderService is not found (" << m_shCompName << ")" << std::endl;
00074     }
00075 
00076     bool stateUpdate = false;
00077     if (!CORBA::is_nil(m_rhService)){
00078         OpenHRP::RobotHardwareService::RobotState_var rs;
00079         try{
00080             m_rhService->getStatus(rs);
00081             m_rstate.state = rs;
00082             stateUpdate = true;
00083         }catch(...){
00084             std::cerr << "[monitor] exception in getStatus()" << std::endl;
00085             m_rhService = NULL;
00086         }
00087     }
00088 
00089     if (!CORBA::is_nil(m_shService)){
00090         OpenHRP::StateHolderService::Command_var com;
00091         try{
00092             m_shService->getCommand(com); 
00093             m_rstate.command = com;
00094             stateUpdate = true;
00095         }catch(...){
00096             std::cerr << "[monitor] exception in getCommand()" << std::endl;
00097             m_shService = NULL;
00098         }
00099     }
00100 
00101 
00102     if (stateUpdate) {
00103         struct timeval tv;
00104         gettimeofday(&tv, NULL);
00105         m_rstate.time = tv.tv_sec + tv.tv_usec/1e6; 
00106         m_log->add(m_rstate);
00107     }
00108     usleep(1000*m_interval);
00109     loop ++;
00110 
00111     return true;
00112 }
00113 
00114 bool Monitor::isConnected()
00115 {
00116     return !CORBA::is_nil(m_rhService);
00117 }
00118 
00119 extern bool isCalibrated(int s);
00120 extern bool isPowerOn(int s);
00121 extern bool isServoOn(int s);
00122 extern int servoAlarm(int s);
00123 extern int temperature(int s);
00124 
00125 void Monitor::showStatus(hrp::BodyPtr &body)
00126 {
00127     if (m_log->index()<0) return;
00128 
00129     LogManager<TimedRobotState> *lm
00130         = (LogManager<TimedRobotState> *)m_log;
00131     OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;
00132 
00133     fprintf(stdout, "\e[1;1H"); // home
00134     fprintf(stdout, "\x1b[42m"); // greep backgroupd
00135 
00136     fprintf(stdout, "\e[2KTimestamp %16.4f, elapsed time %8.4f\n", lm->state().time, lm->currentTime());
00137     double curr_time, prev_time;
00138     std::vector<double> curr_angle, prev_angle, velocity, curr_vel, prev_vel, curr_acc, acceleration;
00139     int n = body->numJoints();
00140     curr_angle.resize(n); prev_angle.resize(n); curr_vel.resize(n); prev_vel.resize(n); curr_acc.resize(n);
00141     velocity.resize(n); acceleration.resize(n);
00142     for(int i = 0; i < n; i++) { prev_angle[i] = lm->state().state.angle[i]; velocity[i] = acceleration[i] = curr_vel[i] = 0; }
00143     prev_time = lm->state().time - 10; // dummy data
00144     while(m_log->index()>0) {
00145         curr_time = lm->state().time;
00146         for(int i = 0; i < n; i++) {
00147             curr_angle[i] = lm->state().state.angle[i];
00148             curr_vel[i] = (curr_angle[i] - prev_angle[i])/(curr_time-prev_time);
00149             curr_acc[i] = (curr_vel[i] - prev_vel[i])/(curr_time-prev_time);
00150             if (fabs(velocity[i]) < fabs(curr_vel[i])) velocity[i] = curr_vel[i];
00151             if (fabs(acceleration[i]) < fabs(curr_acc[i])) acceleration[i] = curr_acc[i];
00152         }
00153         m_log->prev(1);
00154         for(int i = 0; i < n; i++) { prev_angle[i] = curr_angle[i]; prev_vel[i] = curr_vel[i];}
00155         prev_time = curr_time;
00156     }
00157     m_log->tail();
00158     fprintf(stdout, "\e[2KID PW                 NAME    ANGLE  COMMAND    ERROR VELOCITY   ACCEL. TORQUE SERVO TEMP\n"); // greep backgroupd
00159     char buf[256];
00160     for (unsigned int i=0; i<body->numJoints(); i++){
00161         hrp::Link *l = body->joint(i);
00162         if (l){
00163             fprintf(stdout,"\e[2K");
00164             int ss = rstate.servoState[i][0];
00165             // joint ID
00166             if (!isCalibrated(ss)){
00167                 yellow();
00168             }else if(isServoOn(ss)){
00169                 red();
00170             }
00171             fprintf(stdout, "%2d ",i);
00172             black();
00173             // power status
00174             if (isPowerOn(ss)) blue();
00175             fprintf(stdout, " o ");
00176             if (isPowerOn(ss)) black();
00177             // joint name, current angle, command angle and torque
00178             fprintf(stdout, "%20s ", l->name.c_str());
00179             // angle
00180             if( i<rstate.angle.length() )
00181                 fprintf(stdout, "%8.3f ", rstate.angle[i]*180/M_PI);
00182             else
00183                 fprintf(stdout, "-------- ");
00184             // command
00185             if( i<rstate.command.length() )
00186                 fprintf(stdout, "%8.3f ", rstate.command[i]*180/M_PI);
00187             else
00188                 fprintf(stdout, "-------- ");
00189 
00190             // error
00191             if( i<rstate.angle.length() && i<rstate.command.length() ){
00192                 double e = (rstate.angle[i]-rstate.command[i])*180/M_PI;
00193                 if ( abs(e) > 1 ) yellow();
00194                 if ( abs(e) > 2 ) magenta();
00195                 if ( abs(e) > 4 ) red();
00196                 fprintf(stdout, "%8.3f ", e);
00197                 black();
00198             }else{
00199                 fprintf(stdout, "-------- ");
00200             }
00201             // velocity
00202             if( i<velocity.size() ) {
00203                 double e = velocity[i]; //*180/M_PI;
00204                 if ( abs(e) >  2 ) yellow();
00205                 if ( abs(e) > 10 ) magenta();
00206                 if ( abs(e) > 20 ) red();
00207                 fprintf(stdout, "%8.2f ", e);
00208                 black();
00209             }else{
00210                 fprintf(stdout, "-------- ");
00211             }
00212             // accleration
00213             if( i<acceleration.size() ) {
00214                 double e = acceleration[i]; //*180/M_PI;
00215                 if ( abs(e) >  50 ) yellow();
00216                 if ( abs(e) > 100 ) magenta();
00217                 if ( abs(e) > 200 ) red();
00218                 fprintf(stdout, "%8.1f ", e);
00219                 black();
00220             }else{
00221                 fprintf(stdout, "-------- ");
00222             }
00223 
00224             // torque
00225             if( i<rstate.torque.length() )
00226                 fprintf(stdout, "%6.1f ", rstate.torque[i]*180/M_PI);
00227             else
00228                 fprintf(stdout, "------   ");
00229             // servo alarms
00230             fprintf(stdout, "%03x   ", servoAlarm(ss));
00231             // driver temperature
00232             int temp = temperature(ss);
00233             if (!temp){
00234                 fprintf(stdout, "-- ");
00235             }else{
00236                 if (temp >= 60) red();
00237                 fprintf(stdout, "%2d ", temp);
00238                 if (temp >= 60) black();
00239             }
00240             fprintf(stdout, "\n");
00241         }
00242     }
00243     fprintf(stdout, "\e[2K---\n");
00244 
00245     if (rstate.accel.length()){
00246         fprintf(stdout, "\e[2K         acc:");
00247         for (unsigned int i=0; i<rstate.accel.length(); i++){
00248             if(i>0)fprintf(stdout, "\e[2K             ");
00249             fprintf(stdout, " %8.4f %8.4f %8.4f\n",
00250                     rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
00251         }
00252     }
00253     if (rstate.rateGyro.length()){
00254         fprintf(stdout, "\e[2K        rate:");
00255         for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
00256             if(i>0)fprintf(stdout, "\e[2K             ");
00257             fprintf(stdout, " %8.4f %8.4f %8.4f\n",
00258                     rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
00259         }
00260     }
00261     if (rstate.force.length()){
00262         fprintf(stdout, "\e[2Kforce/torque:");
00263         for (unsigned int i=0; i<rstate.force.length(); i++){
00264             if(i>0)fprintf(stdout, "\e[2K             ");
00265             fprintf(stdout, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f\n",
00266                     rstate.force[i][0],
00267                     rstate.force[i][1],
00268                     rstate.force[i][2],
00269                     rstate.force[i][3],
00270                     rstate.force[i][4],
00271                     rstate.force[i][5]);
00272             }
00273     }
00274     fprintf(stdout, "\e[2K\n");
00275 }
00276 
00277 
00278 void Monitor::setRobotHardwareName(const char *i_name)
00279 {
00280     m_rhCompName = i_name;
00281 }
00282 
00283 void Monitor::setStateHolderName(const char *i_name)
00284 {
00285     m_shCompName = i_name;
00286 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55