BodySchemaLogger.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * author Micha Hersch
00006  * email:   micha.hersch@robotcub.org
00007  * website: www.robotcub.org
00008  * Permission is granted to copy, distribute, and/or modify this program
00009  * under the terms of the GNU General Public License, version 2 or any
00010  * later version published by the Free Software Foundation.
00011  *
00012  * A copy of the license can be found at
00013  * http://www.robotcub.org/icub/license/gpl.txt
00014  *
00015  * This program is distributed in the hope that it will be useful, but
00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00018  * Public License for more details
00019  */
00020 
00021 #include <ace/OS_NS_stdio.h>
00022 #include "BodySchemaLogger.h"
00023 
00024 
00025 bool BodySchemaLogger::open(Searchable& s){
00026 
00027     char fname[80];
00028     ios_base::openmode opt;
00029   
00030 
00031 //     //loading tree
00032 //     Value& struc = s.find("structure");
00033 //     if(struc.isNull()){
00034 //         ACE_OS::printf("no robot structure file given\n");
00035 //         return false;
00036 //   }
00037     
00038 //   body = new KinematicTree(struc.asString().c_str());
00039 
00040     body_data_size = 200;
00041 
00042     Value& head_s = s.find("head_size");
00043     if(head_s.isNull()){
00044          ACE_OS::printf("No size for head proprioception specified\n");
00045          return false;
00046     }
00047     head_size = head_s.asInt();
00048     
00049     Value& arm_s = s.find("arm_size");
00050     if(arm_s.isNull()){
00051         ACE_OS::printf("No size for arm proprioception specified\n");
00052         return false;
00053     }
00054     arm_size = arm_s.asInt();
00055  
00056     
00057     body_data = new float[body_data_size];    
00058     head_position = new float[head_size];
00059     arm_position = new float[arm_size];
00060 
00061 
00062     Value& dir = s.find("directory");
00063     if(dir.isNull()){
00064           ACE_OS::printf("No directory specified\n");
00065           return false;
00066     }
00067 
00068     if(s.check("binary")){
00069         binary = true;
00070         opt = ios_base::out | ios_base::binary;
00071     }
00072     else{
00073         binary = false;
00074         opt = ios_base::out;
00075     }
00076 
00077     cout<<"opening files "<<endl;
00078     //opening log files
00079     sprintf(fname,"%s/%s",dir.asString().c_str(),"body_schema_log");
00080     body_schema_file.open(fname,opt);
00081     if(body_schema_file.fail()){    
00082         ACE_OS::printf("Cannot open file %s\n",fname);
00083           return false;
00084     }
00085 
00086     sprintf(fname,"%s/%s",dir.asString().c_str(),"head_log");
00087     head_file.open(fname,opt);
00088     if(head_file.fail()){    
00089         ACE_OS::printf("Cannot open file %s\n",fname);
00090           return false;
00091     }
00092 
00093     sprintf(fname,"%s/%s",dir.asString().c_str(),"arm_log");
00094     arm_file.open(fname,opt);
00095     if(arm_file.fail()){    
00096         ACE_OS::printf("Cannot open file %s\n",fname);
00097           return false;
00098     }
00099 
00100     sprintf(fname,"%s/%s",dir.asString().c_str(),"vision_log");
00101     vision_file.open(fname,opt);
00102     if(vision_file.fail()){    
00103         ACE_OS::printf("Cannot open file %s\n",fname);
00104           return false;
00105     }
00106 
00107     sprintf(fname,"%s/%s",dir.asString().c_str(),"visual_rotation_log");
00108     visual_rotation_file.open(fname,opt);
00109     if(visual_rotation_file.fail()){    
00110         ACE_OS::printf("Cannot open file %s\n",fname);
00111           return false;
00112     }
00113 
00114     cout<<"opening ports "<<endl;
00115     //opening ports
00116 
00117     if(! body_schema_port.open(getName("body_schema:i").c_str())){
00118         ACE_OS::printf("Cannot open port %s\n",getName("body_schema:i").c_str());
00119       return false;
00120   }
00121     if(! head_port.open(getName("proprio_head:i").c_str())){
00122        ACE_OS::printf("Cannot open port %s\n",getName("proprio_head:i").c_str());
00123       return false;
00124   }
00125 
00126     if(! arm_port.open(getName("proprio_arm:i").c_str())){
00127         ACE_OS::printf("Cannot open port %s\n",getName("proprio_arm:i").c_str());
00128         return false;
00129     }
00130 
00131    if(! vision_port.open(getName("vision:i").c_str())){
00132        ACE_OS::printf("Cannot open port %s\n",getName("vision:i").c_str());
00133       return false;
00134    }
00135 
00136    if(! visual_rotation_port.open(getName("visual_rotation:i").c_str())){
00137        ACE_OS::printf("Cannot open port %s\n",getName("visual_rotation:i").c_str());
00138       return false;
00139    }
00140 
00141   //Opening the communication ports
00142    if(!cmd.open(getName("cmd:i").c_str())){
00143        ACE_OS::printf("Cannot open port %s\n",getName("cmd:i").c_str());
00144        return false;
00145    }
00146 
00147    return true;
00148 }
00149 
00150 bool BodySchemaLogger::close(){
00151     //closing files
00152     body_schema_file.close();
00153     head_file.close();
00154     arm_file.close();
00155     vision_file.close();
00156     visual_rotation_file.close();
00157 
00158     //closing ports
00159     body_schema_port.close();
00160     head_port.close();
00161     arm_port.close();
00162     vision_port.close();
00163     visual_rotation_port.close();
00164 
00165     //releasing memory
00166     delete[] body_data;
00167     delete[] head_position;
00168     delete[] arm_position;
00169     
00170     return true;
00171 }
00172 
00173 
00174 void BodySchemaLogger::putInFile(ofstream& out,const float *data, int size){
00175     if(binary){
00176         out.write((char *)data, size*sizeof(float));
00177     }
00178     else{
00179         for(int i=0;i<size;i++){
00180             out<<data[i]<<" ";
00181         }
00182         out<<endl;
00183     }
00184 }
00185 
00186 
00187 bool BodySchemaLogger::updateModule(){    
00188     Stamp stamp;
00189     if(cmd.ShouldQuit()){return false;}
00190     if(body_schema_port.ReadBodySchema(body_data,body_data_size)){
00191         if(body_schema_port.GetSize()<=body_data_size){
00192             putInFile(body_schema_file,body_data,body_schema_port.GetSize());
00193         }
00194         else{
00195             body_data_size =body_schema_port.GetSize();
00196             delete [] body_data;
00197             body_data = new float[body_data_size];
00198         } 
00199     }
00200     if(head_port.ReadPosition(head_position+1,head_size-1)){
00201         head_port.getEnvelope(stamp);
00202         head_position[0] = (float) stamp.getTime();
00203         putInFile(head_file,head_position,head_size);
00204     }
00205 
00206     if(arm_port.ReadPosition(arm_position+1,arm_size-1)){
00207         arm_port.getEnvelope(stamp);
00208         arm_position[0] = (float) stamp.getTime();
00209         putInFile(arm_file,arm_position,arm_size);
00210     }
00211     
00212     if(vision_port.ReadPosition(vision+1)){
00213         vision_port.getEnvelope(stamp);
00214         vision[0] = (float) stamp.getTime();
00215         putInFile(vision_file,vision,4);
00216     }
00217 
00218     double t0,t1;
00219     if(visual_rotation_port.ReadRotation(visual_rotation+2,&t0,&t1 )){
00220         visual_rotation[0] = (float)t0;
00221         visual_rotation[1] = (float)t1;        
00222         putInFile(visual_rotation_file,visual_rotation,5);
00223     }
00224 
00225     
00226     return true;
00227 }
00228 
00229 int main(int argc, char *argv[]){
00230     BodySchemaLogger module;
00231     module.runModule(argc,argv);
00232     return 0;
00233 }
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:25 2010 for Body_Schema_Learning by  doxygen 1.6.1