SpecializedPort.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 - LASA - EPFL
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 #include "SpecializedPort.h"
00021 
00022 void CartesianDataPort::SendPosition(const float *position, bool strict){
00023   Bottle& b = prepare();
00024   b.clear();
00025   for(int i=0;i<3;i++){
00026     b.addDouble(position[i]);
00027   }
00028   //  cout <<"sending "<<b.toString().c_str()<<endl;
00029   write(strict);
00030 }
00031 
00032 
00033 void CartesianDataPort::SendPositionAndOrientation(const float *position, const float *orientation, bool strict){
00034   Bottle& b = prepare();
00035   b.clear();
00036   for(int i=0;i<3;i++){
00037     b.addDouble(position[i]);
00038   }
00039   for(int i=0;i<3;i++){
00040     b.addDouble(orientation[i]);
00041   }
00042   //  cout <<"sending "<<b.toString().c_str()<<endl;
00043   write(strict);
00044 }
00045 
00046 bool CartesianDataPort::ReadPosition(float *position){
00047   Bottle *b = read(false);
00048   if(b){
00049     for(int i=0;i<3;i++){
00050       position[i] =b->get(i).asDouble();
00051     }
00052     return true;
00053   }
00054   return false;
00055 }
00056 
00057 int CartesianDataPort::ReadPositionAndOrientation(float *position, float *orientation){
00058   Bottle *b = read(false);
00059   if(b){
00060     if(b->size()>=3){
00061       for(int i=0;i<3;i++){
00062         position[i] =b->get(i).asDouble();
00063       }
00064     }
00065     if(b->size()>=6){
00066       for(int i=0;i<3;i++){
00067         orientation[i] =b->get(i+3).asDouble();
00068       }
00069     }
00070     return b->size();
00071   }
00072   return 0;
00073 }
00074 
00075 JointAngleDataPort::JointAngleDataPort(){
00076   mapping_size =0;
00077   mapping =NULL;
00078   data_size = 0;
00079 }
00080 
00081 
00082 bool JointAngleDataPort::SetMapping(const int *map, int m_size){
00083   if(m_size!=mapping_size){
00084     if(mapping) delete [] mapping;
00085     mapping = new int[m_size];
00086   }
00087   mapping_size = m_size;
00088   for(int i=0;i<mapping_size;i++){
00089     mapping[i] = map[i];
00090   }
00091   return true;
00092 }
00093 
00094 
00095 void JointAngleDataPort::SendPosition(const float *position, int size,bool strict){
00096   Bottle& b = prepare();
00097   b.clear();
00098   for(int i=0;i<size;i++){
00099     b.addDouble(position[i]);
00100   }
00101   //  cout <<"sending "<<b.toString().c_str()<<endl;
00102   write(strict);
00103 }
00104 
00105 bool JointAngleDataPort::ReadPosition(float *position, int max_size,bool shouldWait){
00106   Bottle *b = read(shouldWait);
00107   if(b){
00108     int si=data_size=b->size();
00109     if(si>max_size){
00110   //     cout<<"warning: cannot read all data in JointAngleDataPort::ReadPosition() ("
00111 //           <<max_size<<" instead of "<<si<<" )"<<endl;
00112       si =max_size;
00113     }
00114     for(int i=0;i<si;i++){
00115       position[i] =b->get(i).asDouble();
00116     }
00117     return true;
00118   }
00119   return false;
00120 }
00121 
00122 
00123 void JointAngleDataPort::SendWithMapping(const float *position, const int size,bool strict){
00124   Bottle& b = prepare();
00125   b.clear();
00126   for(int i=0;i<size;i++){
00127     b.addInt(mapping[i]);
00128     b.addDouble(position[i]);
00129   }
00130   //  cout <<"sending "<<b.toString().c_str()<<endl;
00131   write(strict);
00132 }
00133 
00134 bool JointAngleDataPort::ReadWithMapping(float *position, int max_size){
00135   Bottle *b = read(false);
00136   if(b){
00137     int si = b->size();
00138     if(si>max_size*2){
00139         cout<<"not enough space allocated to JointAngleDataPort::ReadWithMapping  ("<<si<<" > 2*"<<max_size<<")"<<endl; 
00140       si=max_size*2;
00141     }
00142     for(int i=0;i<si;i=i+2){
00143       position[b->get(i).asInt()]=b->get(i+1).asDouble();
00144     }
00145     return true;
00146   }
00147   return false;
00148 } 
00149 
00150 BodySchemaDataPort::BodySchemaDataPort(){
00151     data_size=0;
00152 }
00153 
00154 void BodySchemaDataPort::SendBodySchema(const float *bodyschema, int size,bool strict){
00155   Bottle& b = prepare();
00156   b.clear();
00157   for(int i=0;i<size;i++){
00158       b.addDouble(bodyschema[i]);
00159   }
00160   //  cout <<"sending "<<b.toString().c_str()<<endl;
00161   write(strict);
00162 }
00163 
00164 bool BodySchemaDataPort::ReadBodySchema(float *bodyschema, int max_size){
00165   Bottle *b = read(false);
00166   if(b){
00167     int si=data_size = b->size();
00168     if(si>max_size){
00169       cout<<"warning: cannot read all data in BodySchemaDataPort::ReadBodySchema()"<<endl;
00170       si =max_size;
00171     }
00172     for(int i=0;i<si;i++){
00173       bodyschema[i] =b->get(i).asDouble();
00174     }
00175     return true;
00176   }
00177   return false;
00178 }
00179 
00180 
00181 void CommandPort::SendQuit(){
00182     Bottle& b = prepare();
00183     b.clear();
00184     b.fromString("quit");
00185     write();
00186 }
00187 
00188 bool CommandPort::ShouldQuit(){
00189     Bottle *b = read(false);
00190     if(b){
00191         if(b->toString()==ConstString("quit")){
00192             return true;
00193         }
00194     }
00195     //    cout<<"no msg"<<endl;
00196     return false;
00197 }
00198 
00199 void VisualRotationPort::SendRotation(const float *rot, double t0, double t1){
00200     Bottle& b = prepare();
00201     b.clear();
00202     for(int i=0;i<3;i++){
00203         b.addDouble(rot[i]);
00204     }
00205     b.addDouble(t0);
00206     b.addDouble(t1);
00207     write();
00208 }
00209 
00210 bool VisualRotationPort::ReadRotation(float *rot, double *t0, double *t1){
00211   Bottle *b = read(false);
00212   if(b){
00213     int si=b->size();
00214     if(si!= 5){
00215         cout<<"warning: wrong format in VisualRotation::ReadPosition expecting 5 doubles"<<endl;
00216         return false;
00217     }
00218     for(int i=0;i<3;i++){
00219       rot[i] =b->get(i).asDouble();
00220     }
00221     *t0 = b->get(3).asDouble();
00222     *t1 = b->get(4).asDouble();
00223     return true;
00224   }
00225   return false;
00226 }
 All Data Structures Functions Variables

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