ReachingModuleThread.h

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  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 
00020 
00185 #ifndef __REACHING_MODULE_H__
00186 #define __REACHING_MODULE_H__
00187 
00188 #define WITH_KIN_TREE
00189 
00190 
00191 #include "KinematicTree.h"
00192 #include "KChainBodySchema.h"
00193 
00194 
00195 #include "ReachingGen.h"
00196 #include "SpecializedPort.h" 
00197 #include "yarp/os/all.h"
00198 #include <yarp/os/RateThread.h>
00199 #include <yarp/dev/Drivers.h>
00200 #include <yarp/dev/PolyDriver.h>
00201 #include <yarp/dev/ControlBoardInterfaces.h>
00202 
00203 using namespace yarp::os;
00204 using namespace yarp::dev;
00205 
00212 class ReachingThread: public RateThread {
00213 
00214 protected:
00215     
00216 
00217     KinematicTree *body;
00218     KChainBodySchema *arm;        
00220     Reaching *reach;         
00221     //  char body_fname[80]; not used anymore
00222     
00223     bool active_mode;        
00225     bool pointing;           
00226     double time_last;        
00227     double time_now;         
00228     double original_time;    
00230     float time_granularity;  
00231     int nbDOFs;
00232     char partname[80];       
00234     int *jointMapping;       
00237     int mappingSize;         
00239     Module *module;          
00241     float *bodydata;         
00242     int body_data_size;      
00243     int new_schema;
00244     int listen_cnt;
00245     int simulation;          
00247     // for recieving the input
00248     CartesianDataPort targetPort;             
00249     BodySchemaDataPort bodyPort;              
00251     JointAngleDataPort robotJointPort;
00252 
00253     // for sending the output
00254     BufferedPort<Bottle> vcControl_port;      
00255     BufferedPort<Bottle> vcFastCommand_port;  
00257     CommandPort cmdPort;
00258 
00259 
00260 
00261 
00262     // int cnt;// for debugging
00263  protected:
00264   bool listen();
00265   bool sendOutput();
00266   void sendAnglesFormatVC(joint_vec_t& angles);
00267   ConstString getName(const char *sub = 0);
00268     bool matchBodySchemaToRobot();//
00269     bool initMapping(Searchable& s);
00270     void setZeroGains();
00271 
00272 
00273  public:
00274 
00275 
00280   ReachingThread(int thread_period=50);
00284   ~ReachingThread();
00285   
00289   bool threadInit();
00290 
00294   void threadRelease();
00295 
00300   bool init(Searchable &s);
00301 
00307   void run();
00308 
00312   void setModule(Module *mod){module=mod;};
00313 };
00314 
00315 
00316 
00317 class ReachingModule : public Module{
00318  private: 
00319   ReachingThread m_thread;
00320 
00321  public:  
00322   ReachingModule();
00323   virtual ~ReachingModule(){cout<<"module deleted"<<endl;};
00324   virtual bool close();
00325   virtual bool open(Searchable &s);
00326   virtual bool updateModule(){return true;};
00327 };
00328 
00329 
00330 #endif
 All Data Structures Functions Variables

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