logSplitter.cpp
Go to the documentation of this file.
00001 #include <limits>
00002 #include <iomanip>
00003 #include <fstream>
00004 #include <iostream>
00005 #include <stdlib.h>
00006 #include "hrpsys/idl/RobotHardwareService.hh"
00007 
00008 int main(int argc, char *argv[])
00009 {
00010     if (argc < 9){
00011         std::cerr << "Usage: " << argv[0] << " [basename of rstate2 log] [dof] [no. of extra servo states] [no. of force sensors] [no. of 3 axes gyro] [no. of 3 axes accelerometers] [no. of batteries] [no. of thermometers]" << std::endl;
00012         return 1;
00013     }
00014 
00015     std::string basename(argv[1]);
00016     std::ifstream ifs((basename+".rstate2").c_str());
00017     if (!ifs.is_open()){
00018         std::cerr << "failed to open " << argv[1] << ".rstate2" << std::endl;
00019         return 2;
00020     }
00021     int dof = atoi(argv[2]);
00022     int nextrass = atoi(argv[3]);
00023     int nfsensor = atoi(argv[4]);
00024     int ngyro = atoi(argv[5]);
00025     int naccel = atoi(argv[6]);
00026     int nbattery = atoi(argv[7]);
00027     int ntemp = atoi(argv[8]);
00028 
00029     std::ofstream ofsq((basename+".q").c_str());
00030     std::ofstream ofsqref((basename+".qRef").c_str());
00031     std::ofstream ofstau((basename+".tau").c_str());
00032     std::ofstream ofsss((basename+".sstate").c_str());
00033     std::ofstream ofsfsensor((basename+".fsensor").c_str());
00034     std::ofstream ofsgyro((basename+".gyro").c_str());
00035     std::ofstream ofsaccel((basename+".accel").c_str());
00036     std::ofstream ofsbat((basename+".bat").c_str());
00037     std::ofstream ofstemp((basename+".temp").c_str());
00038 
00039     ofsq.setf(std::ios::fixed, std::ios::floatfield);
00040     ofsqref.setf(std::ios::fixed, std::ios::floatfield);
00041     ofstau.setf(std::ios::fixed, std::ios::floatfield);
00042     ofsss.setf(std::ios::fixed, std::ios::floatfield);
00043     ofsfsensor.setf(std::ios::fixed, std::ios::floatfield);
00044     ofsgyro.setf(std::ios::fixed, std::ios::floatfield);
00045     ofsaccel.setf(std::ios::fixed, std::ios::floatfield);
00046     ofsbat.setf(std::ios::fixed, std::ios::floatfield);
00047     ofstemp.setf(std::ios::fixed, std::ios::floatfield);
00048 
00049     double time, v;
00050     int ss;
00051     std::string str;
00052     
00053     ifs >> time;
00054     while(!ifs.eof()){
00055         // q
00056         ofsq << time << " ";
00057         for (int i=0; i<dof; i++){
00058             ifs >> v; ofsq << v << " ";
00059         }
00060         ofsq << std::endl;
00061         // qRef
00062         ofsqref << time << " ";
00063         for (int i=0; i<dof; i++){
00064             ifs >> v; ofsqref << v << " ";
00065         }
00066         ofsqref << std::endl;
00067         // tau
00068         ofstau << time << " ";
00069         for (int i=0; i<dof; i++){
00070             ifs >> v; ofstau << v << " ";
00071         }
00072         ofstau << std::endl;
00073         // servo state
00074         ofsss << time << " ";
00075         for (int i=0; i<dof; i++){
00076             ifs >> ss;
00077             ofsss << ((ss&OpenHRP::RobotHardwareService::CALIB_STATE_MASK) >> OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT) << " ";
00078             ofsss << ((ss&OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT) << " ";
00079             ofsss << ((ss&OpenHRP::RobotHardwareService::POWER_STATE_MASK) >> OpenHRP::RobotHardwareService::POWER_STATE_SHIFT) << " ";
00080             ofsss << ((ss&OpenHRP::RobotHardwareService::SERVO_ALARM_MASK) >> OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT) << " ";
00081             ofsss << ((ss&OpenHRP::RobotHardwareService::DRIVER_TEMP_MASK) >> OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT) << " ";
00082             for (int j=0; j<nextrass; j++){
00083                 ifs >> ss;
00084                 ofsss << ss << " ";
00085             }
00086         }
00087         ofsss << std::endl;
00088         // force sensor
00089         ofsfsensor << time << " ";
00090         for (int i=0; i<6*nfsensor; i++){
00091             ifs >> v; ofsfsensor << v << " ";
00092         }
00093         ofsfsensor << std::endl;
00094         // gyro
00095         ofsgyro << time << " ";
00096         for (int i=0; i<3*ngyro; i++){
00097             ifs >> v; ofsgyro << v << " ";
00098         }
00099         ofsgyro << std::endl;
00100         // accelerometer
00101         ofsaccel << time << " ";
00102         for (int i=0; i<3*naccel; i++){
00103             ifs >> v; ofsaccel << v << " ";
00104         }
00105         ofsaccel << std::endl;
00106         // battery
00107         ofsbat << time << " ";
00108         for (int i=0; i<3*nbattery+2; i++){
00109             ifs >> str;
00110             if (str == "nan"){
00111                 v = std::numeric_limits<double>::quiet_NaN();
00112             }else{
00113                 v = atof(str.c_str());
00114             }
00115             ofsbat << v << " ";
00116         }
00117         ofsbat << std::endl;
00118         // thermometer
00119         ofstemp << time << " ";
00120         for (int i=0; i<ntemp; i++){
00121             ifs >> v; ofstemp << v << " ";
00122         }
00123         ofstemp << std::endl;
00124         
00125         ifs >> time;
00126     }
00127     
00128     return 0;
00129 }


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