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
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 }
