YARPComm.cc
Go to the documentation of this file.
00001 
00002 #include <yarp/os/all.h>
00003 #include <yarp/os/impl/Logger.h>
00004 #include <yarp/os/Bottle.h>
00005 #include <yarp/os/BufferedPort.h>
00006 
00007 #include "YARPComm.hh"
00008 #include "FRIComm.hh"
00009 
00010 #include <signal.h>
00011 
00012 #include <string>
00013 
00014 using namespace yarp::os;
00015 
00016 
00017 YARPComm::YARPComm(const char* prefix) :
00018   prefix_(prefix), net_(0),
00019   running_(false), exitRequested_(false)
00020 {
00021   // making checkNetwork() quiet
00022   yarp::os::impl::Logger::get().setVerbosity(-1);
00023 }
00024 
00025 
00026 YARPComm::~YARPComm()
00027 {
00028   if(net_) {
00029     net_->fini();
00030     delete net_;
00031   }
00032 }
00033 
00034 
00035 bool YARPComm::open()
00036 {
00037   if(!yarp::os::Network::checkNetwork())
00038     return false;
00039 
00040   // returning to normal verbosity
00041   yarp::os::impl::Logger::get().setVerbosity(0);
00042 
00043   if(net_ == 0) {
00044     net_ = new Network();
00045     net_->init();
00046   }
00047 
00048   std::string tprefix(prefix_);
00049   port_commands.open((tprefix+"/cmd").c_str());
00050   port_commanded.open((tprefix+"/cmded").c_str());
00051   port_position.open((tprefix+"/pos").c_str());
00052   port_torque.open((tprefix+"/torque").c_str());
00053   port_torque_raw.open((tprefix+"/torque_raw").c_str());
00054   port_torque_tcp.open((tprefix+"/torque_tcp").c_str());
00055   port_kukacommand.open((tprefix+"/sent_to_rsi").c_str());
00056 
00057   port_stiffness.open((tprefix+"/stiffness").c_str());
00058   port_damping.open((tprefix+"/damping").c_str());
00059   port_add_torque.open((tprefix+"/add_torque").c_str());
00060 
00061   port_krlcommand.open((tprefix+"/krl_command").c_str());
00062 
00063   port_cart_stiffness.open((tprefix+"/cart_stiffness").c_str());
00064   port_cart_damping.open((tprefix+"/cart_damping").c_str());
00065   port_cart_force_torque.open((tprefix+"/cart_force_torque").c_str());
00066   port_cart_activate.open((tprefix+"/cart_activate").c_str());
00067 
00068   port_current_cart_stiffness.open((tprefix+"/current_cart_stiffness").c_str());
00069   port_current_cart_damping.open((tprefix+"/current_cart_damping").c_str());
00070   port_current_cart_force_torque.open((tprefix+"/current_cart_force_torque").c_str());
00071 
00072   return true;
00073 }
00074 
00075 
00076 bool YARPComm::receiveBottle(BufferedPort<Bottle> &port, float* data, int n)
00077 {
00078   if(port.getPendingReads() <= 0) {
00079     return false;
00080   } else {
00081     // read commands, emptying the queue
00082     while(port.getPendingReads() > 0) {
00083       Bottle *b = port.read();
00084       for(int i=0; i < n; i++)
00085         data[i] = b->get(i).asDouble();
00086     }
00087     return true;
00088   }
00089 }
00090 
00091 
00092 bool YARPComm::receiveKRLCmd(BufferedPort<Bottle> &port, int* iData, float* rData)
00093 {
00094   if(port.getPendingReads() <= 0) {
00095     return false;
00096   } else {
00097     // read command
00098     Bottle *b = port.read();
00099     int i=0, n_i=0, n_f=0;
00100     for(; i < b->size() && b->get(i).isInt() && i < 16; i++)
00101       iData[i] = b->get(i).asInt();
00102     n_i=i;
00103     
00104     for(; i < b->size() && b->get(i).isDouble() && i < 32; i++)
00105       rData[i-n_i] = b->get(i).asDouble();
00106     n_f=i-n_i;
00107 
00108     printf("YARP: got command [");
00109     if(n_i > 0) printf("%d", iData[0]);
00110     for(i=1; i < n_i; i++) printf(", %d", iData[i]);
00111     printf("] [");
00112     if(n_f > 0) printf("%2.2f", rData[0]);
00113     for(i=1; i < n_f; i++) printf(", %2.2f", rData[i]);
00114     printf("]\n");
00115   }
00116   return true;
00117 }
00118 
00119 
00120 bool YARPComm::receiveCommand(RobotCommand* cmd)
00121 {
00122   bool got_data = false;
00123 
00124   got_data |= receiveBottle(port_commands, cmd->command, 7);
00125   got_data |= receiveBottle(port_stiffness, cmd->stiffness, 7);
00126   got_data |= receiveBottle(port_damping, cmd->damping, 7);
00127   got_data |= receiveBottle(port_add_torque, cmd->addTorque, 7);
00128 
00129   got_data |= receiveBottle(port_cart_stiffness, cmd->cartImpedance.K, 6*6);
00130   got_data |= receiveBottle(port_cart_damping, cmd->cartImpedance.D, 6*6);
00131   got_data |= receiveBottle(port_cart_force_torque, cmd->cartImpedance.ft, 6);
00132 
00133   float cart_activate;
00134   if(receiveBottle(port_cart_activate, &cart_activate, 1))
00135   {
00136     cmd->useCartesianImpedance = (cart_activate > 0.5);
00137     got_data = true;
00138   }
00139  
00140   return got_data;
00141 }
00142 
00143 
00144 void YARPComm::publishData(const RobotData &data, const RobotCommand &cmd_old)
00145 {
00146   sendData(port_position, data.position, 7);
00147   sendData(port_commanded, data.commanded, 7);
00148   sendData(port_torque, data.torque, 7);
00149   sendData(port_torque_raw, data.torque_raw, 7);
00150   sendData(port_torque_tcp, data.torqueTCP, 12);
00151   sendData(port_kukacommand, cmd_old.command, 7);
00152 
00153   sendData(port_current_cart_stiffness, cmd_old.cartImpedance.K, 6*6);
00154   sendData(port_current_cart_damping,   cmd_old.cartImpedance.D, 6*6);
00155   sendData(port_current_cart_force_torque,   cmd_old.cartImpedance.ft, 6);
00156 }
00157 
00158 
00159 void YARPComm::sendData(BufferedPort<Bottle> &p, const float* data, int n)
00160 {
00161   Bottle &b=p.prepare();
00162   b.clear();
00163 
00164   for(int i=0; i < n; i++)
00165     b.addDouble(data[i]);
00166 
00167   p.write();
00168 }
00169 
00170 
00171 void YARPComm::start(FRIThread *fri)
00172 {
00173   fri_ = fri;
00174   pthread_create(&thread_, 0, &YARPComm::run_s, (void *) this);
00175 }
00176 
00177 
00178 void* YARPComm::run()
00179 {
00180   // this may take > 10s if YARP is down.
00181   while(!open() && !exitRequested_)
00182     usleep(500000);
00183 
00184   running_ = true;  // YARP is up, we are running
00185 
00186   // some process data
00187   RobotData data;
00188   RobotCommand cmd;
00189 
00190   int iData[16];
00191   float rData[16];
00192 
00193   int seq = 0;
00194 
00195   while(!exitRequested_) {
00196     cmd = fri_->cmd();
00197     if(receiveCommand(&cmd))
00198       fri_->setCmd(cmd);
00199 
00200     if(receiveKRLCmd(port_krlcommand, iData, rData))
00201       fri_->postCommand(iData, rData);
00202 
00203     data = fri_->data();
00204     if(seq != data.seq)
00205       publishData(data, cmd);  // publish arm state
00206 
00207     Time::delay(0.002);
00208   }
00209   running_ = false;
00210 
00211   return 0;
00212 }
00213 
00214 
00215 void YARPComm::finish()
00216 {
00217   exitRequested_ = true;
00218   pthread_join(thread_, 0);
00219 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Thu May 23 2013 05:28:18