fri_dummy_arm.cc
Go to the documentation of this file.
00001 #include <strings.h>
00002 #include <string.h>
00003 #include <unistd.h>
00004 #include <stdio.h>
00005 
00006 #include <sys/types.h> 
00007 #include <sys/socket.h>
00008 #include <netinet/in.h>
00009 #include <netinet/udp.h>
00010 #include <arpa/inet.h>
00011 
00012 #include <time.h>
00013 #include <sys/time.h>
00014 #include <errno.h>
00015 
00016 #include <math.h>
00017 
00018 #include "friComm.h"
00019 
00020 
00021 class FRIServer
00022 {
00023 public:
00024   int sock_;
00025   struct sockaddr_in recv_addr_, peer_addr_;
00026   socklen_t recv_addr_len_;
00027 
00028 
00029   tFriMsrData msr_msg_;
00030   tFriCmdData cmd_msg_;
00031 
00032   int counter_;
00033 
00034   void init_msr_packet(tFriMsrData &msr);
00035   void init_network(const char *peer_address, int peer_port,
00036                     const char *my_address,   int my_port);
00037 
00038   void communicate();
00039 };
00040 
00041 void FRIServer::init_msr_packet(tFriMsrData &msr)
00042 {
00043   msr.head.packetSize = sizeof(tFriMsrData);
00044   msr.head.datagramId = FRI_DATAGRAM_ID_MSR;
00045 
00046   msr.intf.state = FRI_STATE_CMD;
00047   msr.intf.quality = FRI_QUALITY_PERFECT;
00048   msr.intf.desiredMsrSampleTime = 0.0;
00049   msr.intf.desiredCmdSampleTime = 0.0;
00050   msr.intf.safetyLimits = 0.0;
00051 
00052   msr.intf.stat.answerRate = 1.0;
00053   msr.intf.stat.latency = 0.0;
00054   msr.intf.stat.jitter = 0.0;
00055   msr.intf.stat.missRate = 0.0;
00056   msr.intf.stat.missCounter = 0;
00057 
00058   msr.robot.power = 0;
00059   msr.robot.control = 0;
00060   msr.robot.error = 0;
00061   msr.robot.warning = 0;
00062 
00063   for(int i=0; i < 7; i++)
00064   {
00065     msr.data.msrJntPos[i] = 0.0;
00066     msr.data.cmdJntPosFriOffset[i] = 0.0;
00067     msr.data.msrJntTrq[i] = 0.0;
00068     msr.data.estExtJntTrq[i] = 0.0;
00069 
00070     msr.data.cmdJntPos[i] = 0.0;
00071     msr.data.gravity[i] = 0.0;
00072     msr.robot.temperature[i] = 0;
00073   }
00074 
00075   counter_ = 0;
00076 }
00077 
00078 void FRIServer::init_network(const char *peer_address, int peer_port,
00079                              const char *my_address,   int my_port)
00080 {
00081   sock_ = socket(AF_INET, SOCK_DGRAM, 0);
00082 
00083   bzero((char *) &peer_addr_, sizeof(peer_addr_));
00084   peer_addr_.sin_family = AF_INET;
00085   peer_addr_.sin_addr.s_addr = inet_addr(peer_address);
00086   peer_addr_.sin_port = htons(peer_port);
00087 
00088   struct sockaddr_in my_addr;
00089   bzero((char *) &my_addr, sizeof(my_addr));
00090   my_addr.sin_family = AF_INET;
00091   my_addr.sin_addr.s_addr = inet_addr(my_address);
00092   my_addr.sin_port = htons(my_port);
00093 
00094   bind(sock_, (sockaddr*) &my_addr, sizeof(sockaddr_in));
00095 
00096   bzero((char *) &recv_addr_, sizeof(recv_addr_));
00097   recv_addr_len_ = sizeof(recv_addr_);
00098 }
00099 
00100 void FRIServer::communicate()
00101 {
00102   while(1)
00103   {
00104     sendto(sock_, (void*) &msr_msg_, sizeof(msr_msg_), 0,
00105            (sockaddr*) &peer_addr_, sizeof(peer_addr_));
00106     recvfrom(sock_, (tFriCmdData*) &cmd_msg_, sizeof(cmd_msg_), 0,
00107              (sockaddr*) &recv_addr_, &recv_addr_len_);
00108 
00109     msr_msg_.head.sendSeqCount = counter_;
00110     msr_msg_.head.reflSeqCount = counter_ - 1;
00111 
00112     for(int i=0; i < 7; i++)
00113     {
00114       //msr_data.data.msrJntPos[i] = sin(0.01*counter)*(i+1)*0.3;
00115       msr_msg_.data.msrJntPos[i] = cmd_msg_.cmd.jntPos[i];
00116       msr_msg_.data.cmdJntPos[i] = cmd_msg_.cmd.jntPos[i];
00117       msr_msg_.data.msrJntTrq[i] = 0.0;
00118       msr_msg_.data.estExtJntTrq[i] = 0.0;
00119       msr_msg_.intf.timestamp=double(time(NULL));
00120     }
00121 
00122     usleep(990);
00123     counter_++;
00124   }
00125 }
00126 
00127 
00128 int main(int argc, char** argv)
00129 {
00130   const char *localhost="127.0.0.1";
00131 
00132   printf("server address: 127.0.0.1:7010\n");
00133   printf("peer   address: 127.0.0.1:7001\n");
00134 
00135   FRIServer server;
00136   server.init_network(localhost, 7001, localhost, 7010);
00137   server.init_msr_packet(server.msr_msg_);
00138 
00139   server.communicate();
00140 }


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:27:39