00001 #include "FRIData.hh" 00002 00003 #include <pthread.h> 00004 00005 #include <yarp/os/all.h> 00006 #include <yarp/os/Bottle.h> 00007 #include <yarp/os/BufferedPort.h> 00008 00009 class FRIThread; 00010 00011 class YARPComm { 00012 public: 00013 YARPComm(const char* prefix); 00014 ~YARPComm(); 00015 bool open(); 00016 void publishData(const RobotData &data, const RobotCommand &cmd_old); 00017 bool receiveCommand(RobotCommand* cmd); 00018 00019 // threading support 00020 void start(FRIThread *fri); 00021 void finish(); 00022 private: 00023 void sendData(yarp::os::BufferedPort<yarp::os::Bottle> &p, const float* data, int n); 00024 bool receiveBottle(yarp::os::BufferedPort<yarp::os::Bottle> &port, float* data, int n); 00025 bool receiveKRLCmd(yarp::os::BufferedPort<yarp::os::Bottle> &port, int* iData, float* rData); 00026 00027 const char* prefix_; 00028 00029 yarp::os::Network *net_; 00030 00031 yarp::os::BufferedPort<yarp::os::Bottle> port_commands; 00032 yarp::os::BufferedPort<yarp::os::Bottle> port_commanded; 00033 yarp::os::BufferedPort<yarp::os::Bottle> port_position; 00034 yarp::os::BufferedPort<yarp::os::Bottle> port_torque; 00035 yarp::os::BufferedPort<yarp::os::Bottle> port_torque_raw; 00036 yarp::os::BufferedPort<yarp::os::Bottle> port_torque_tcp; 00037 yarp::os::BufferedPort<yarp::os::Bottle> port_kukacommand; 00038 00039 yarp::os::BufferedPort<yarp::os::Bottle> port_stiffness; 00040 yarp::os::BufferedPort<yarp::os::Bottle> port_damping; 00041 yarp::os::BufferedPort<yarp::os::Bottle> port_add_torque; 00042 00043 yarp::os::BufferedPort<yarp::os::Bottle> port_krlcommand; 00044 00045 yarp::os::BufferedPort<yarp::os::Bottle> port_cart_stiffness; 00046 yarp::os::BufferedPort<yarp::os::Bottle> port_cart_damping; 00047 yarp::os::BufferedPort<yarp::os::Bottle> port_cart_force_torque; 00048 yarp::os::BufferedPort<yarp::os::Bottle> port_cart_activate; 00049 00050 yarp::os::BufferedPort<yarp::os::Bottle> port_current_cart_stiffness; 00051 yarp::os::BufferedPort<yarp::os::Bottle> port_current_cart_damping; 00052 yarp::os::BufferedPort<yarp::os::Bottle> port_current_cart_force_torque; 00053 00054 // threading support 00055 FRIThread* fri_; 00056 void* run(); 00057 static void* run_s(void *ptr) { return ((YARPComm *) ptr)->run(); } 00058 00059 pthread_t thread_; 00060 bool running_, exitRequested_; 00061 };