Changeset 363 in flair-src for trunk

Timestamp:

Sep 3, 2020, 8:54:16 AM (4 years ago)

Author:

Sanahuja Guillaume

Message:

allow mutliple connection on udp (only nrt for the moment)

Location:

trunk

Files:

• lib/FlairCore/src/UdpSocket.cpp (modified) (2 diffs)
• lib/FlairCore/src/UdpSocket.h (modified) (4 diffs)
• lib/FlairCore/src/UdpSocket_impl.cpp (modified) (10 diffs)
• lib/FlairCore/src/unexported/UdpSocket_impl.h (modified) (2 diffs)
• tools/VrpnLite/src/VrpnLite.cpp (modified) (4 diffs)
• tools/VrpnLite/src/VrpnLite.h (modified) (1 diff)

trunk/lib/FlairCore/src/UdpSocket.cpp

@@ -39 +39 @@
 UdpSocket::~UdpSocket() { delete pimpl_; }
 
-void UdpSocket::SendMessage(const char *message, size_t message_len) {
-  pimpl_->SendMessage(message, message_len);
+void UdpSocket::SendMessage(const char *message, size_t message_len,int dst_id) {
+  pimpl_->SendMessage(message, message_len,dst_id);
 }
 
@@ -46 +46 @@
 
 ssize_t UdpSocket::RecvMessage(char *buf, size_t buf_len, Time timeout, char *src,
-                            size_t *src_len) {
-  return pimpl_->RecvMessage(buf, buf_len, timeout, src, src_len);
+                            size_t *src_len,int *src_id) {
+  return pimpl_->RecvMessage(buf, buf_len, timeout, src, src_len,src_id);
 }
 

trunk/lib/FlairCore/src/UdpSocket.h

@@ -14 +14 @@
 #define UDPSOCKET_H
 
-#include <unistd.h>
-#include <stdint.h>
 #include <Object.h>
 
@@ -76 +74 @@
   * \param message message
   * \param message_len message length
+  * \param dst_id id of the dst if multiple connection (quick and dirty hack for sido, only works on NRT)
   */
-  void SendMessage(const char *message, size_t message_len);
+  void SendMessage(const char *message, size_t message_len,int dst_id=0);
 
   /*!
@@ -84 +83 @@
   * Receive a message and wait up to timeout. \n
   * If src and src_len are specified, the source of the message will be
-  * copied in the src buffer. \n
+  * copied in the src buffer. Source is the Flair name of sender\n
   * Note that in case of a broadcast socket, own messages are filtered and
   * are not received.
@@ -93 +92 @@
   * \param src buffer to put source name
   * \param src_len buffer length
+  * \param src_id id of the src, to be used for sending if multiple connection (quick and dirty hack for sido, only works on NRT)
   *
   * \return size of the received message
   */
   ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout, char *src = NULL,
-                      size_t *src_len = NULL);
+                      size_t *src_len = NULL,int *src_id=NULL);
 
   void NetworkToHost(char *data, size_t dataSize);

trunk/lib/FlairCore/src/UdpSocket_impl.cpp

@@ -53 +53 @@
 void UdpSocket_impl::Init(void) {
   int yes = 1;
+  sock_in_size=0;
+  sock_in=NULL;
 
   fd = socket(AF_INET, SOCK_DGRAM, 0); // UDP
@@ -136 +138 @@
       self->Err("gethostbyname\n");
     }
-
-    sock_in.sin_addr = *(in_addr *)hostinfo->h_addr;
-    sock_in.sin_port = htons(port);
-    sock_in.sin_family = AF_INET;
+    sock_in=(struct sockaddr_in*)malloc(sizeof(struct sockaddr_in));
+    sock_in[0].sin_addr = *(in_addr *)hostinfo->h_addr;
+    sock_in[0].sin_port = htons(port);
+    sock_in[0].sin_family = AF_INET;
+    sock_in_size++;
   }
 }
@@ -155 +158 @@
 #endif
   close(fd);
-}
-
-void UdpSocket_impl::SendMessage(const char *src, size_t src_len) {
+  free(sock_in);
+}
+
+void UdpSocket_impl::SendMessage(const char *msg, size_t msg_len,int dst_id) {
   ssize_t written;
   string to_send;
         
   if (broadcast == true) {
-    to_send = getFrameworkManager()->ObjectName() + ":" + string(src, src_len);
-    src_len = to_send.size();
-    src = (char *)to_send.c_str();
+    to_send = getFrameworkManager()->ObjectName() + ":" + string(msg, msg_len);
+    msg_len = to_send.size();
+    msg = (char *)to_send.c_str();
   }
 
 #ifdef __XENO__
   // Printf("send pipe %s\n",src);
-  written = rt_pipe_write(&pipe, src, src_len, P_NORMAL);
+  written = rt_pipe_write(&pipe, msg, msg_len, P_NORMAL);
 
   if (written < 0) {
     char errorMsg[256];
     self->Err("rt_pipe_write error (%s)\n", strerror_r(-written, errorMsg, sizeof(errorMsg)));
-  } else if (written != (ssize_t)src_len) {
+  } else if (written != (ssize_t)msg_len) {
     self->Err("rt_pipe_write error %i/%i\n", written, to_send.size());
   }
 #else
-  written =
-      sendto(fd, src, src_len, 0, (struct sockaddr *)&sock_in, sizeof(sock_in));
+  written = sendto(fd, msg, msg_len, 0, (struct sockaddr *)&(sock_in[dst_id]), sizeof(struct sockaddr_in));
   if(written==-1) {
                 char errorMsg[256];
     self->Err("sendto error (%s)\n",strerror_r(errno, errorMsg, sizeof(errorMsg)));
-  } else if (written != (ssize_t)src_len) {
-    self->Err("sendto error %i/%i\n",written,src_len);
+  } else if (written != (ssize_t)msg_len) {
+    self->Err("sendto error %i/%i\n",written,msg_len);
   }
 #endif
@@ -205 +208 @@
   }
 #else
-  written = sendto(fd, message.c_str(), message.size(), 0,
-                   (struct sockaddr *)&sock_in, sizeof(sock_in));
+  written = sendto(fd, message.c_str(), message.size(), 0, (struct sockaddr *)&sock_in, sizeof(struct sockaddr_in));
   if (written != (ssize_t)message.size()) {
     self->Err("sendto error\n");
@@ -215 +217 @@
 
 ssize_t UdpSocket_impl::RecvMessage(char *msg, size_t msg_len, Time timeout,
-                                 char *src, size_t *src_len) {
+                                 char *src, size_t *src_len,int* src_id) {
   ssize_t nb_read;
   char buffer[128];
@@ -221 +223 @@
   nb_read = rt_pipe_read(&pipe, &buffer, sizeof(buffer), timeout);
 #else
-  socklen_t sinsize = sizeof(sock_in);
-  struct timeval tv;
 
   if (timeout != TIME_NONBLOCK) {
+    struct timeval tv;
     int attr = fcntl(fd, F_GETFL, 0);
     fcntl(fd, F_SETFL, attr & (~O_NONBLOCK));
@@ -238 +239 @@
   }
 
+  struct sockaddr_in sock_in_tmp;
   if (broadcast == false) {
-    nb_read =
-        recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *)&sock_in, &sinsize);
+    socklen_t sinsize = sizeof(struct sockaddr_in);
+    nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *)&sock_in_tmp, &sinsize);
   } else {
     nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, NULL, NULL);
+  }
+  
+  if(broadcast==false && nb_read>0) {
+    int i;
+    for(i=0;i<sock_in_size;i++) {
+        if(sock_in[i].sin_port==sock_in_tmp.sin_port) {
+            Printf("match found in former adress\n");
+            *src_id=i;
+            break;
+        }
+    }
+    if(i==sock_in_size) {
+        Printf("no match found in former adress\n");
+        sock_in_size++;
+        sock_in=(sockaddr_in*)realloc(sock_in,sock_in_size*sizeof(sockaddr_in));
+        sock_in[sock_in_size-1]=sock_in_tmp;
+        *src_id=sock_in_size-1;
+    }
   }
 #endif
@@ -266 +286 @@
           return -1;
         }
-      } else if (nb_read - index - 1 + 1 >
-                 (int)msg_len) { //+1 pour inserer un 0
-        self->Warn("insufficent msg size (%i/%i)\n", nb_read - index - 1 + 1,
-                   msg_len);
+      } else if (nb_read - index - 1 + 1 > (int)msg_len) { //+1 pour inserer un 0
+        self->Warn("insufficent msg size (%i/%i)\n", nb_read - index - 1 + 1, msg_len);
         return -1;
       }
@@ -329 +347 @@
 
       if (FD_ISSET(caller->fd, &set)) {
-        socklen_t sinsize = sizeof(caller->sock_in);
+        socklen_t sinsize = sizeof(struct sockaddr_in);
         if (caller->broadcast == false) {
           nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0,
@@ -361 +379 @@
           nb_write = sendto(caller->fd, buffer, nb_read, 0,
                             (struct sockaddr *)&(caller->sock_in),
-                            sizeof(caller->sock_in));
+                            sizeof(struct sockaddr_in));
           if (nb_write != nb_read) {
             caller->self->Err("sendto error\n");

trunk/lib/FlairCore/src/unexported/UdpSocket_impl.h

@@ -33 +33 @@
 
   void SendMessage(std::string message);
-  void SendMessage(const char *src, size_t src_len);
+  void SendMessage(const char *msg, size_t msg_len,int dst_id);
   ssize_t RecvMessage(char *msg, size_t msg_len, flair::core::Time timeout,
-                      char *src = NULL, size_t *src_len = NULL);
+                      char *src = NULL, size_t *src_len = NULL,int *src_id=NULL);
 
 private:
@@ -44 +44 @@
   void Init(void);
   const flair::core::UdpSocket *self;
-  struct sockaddr_in sock_in;
+  struct sockaddr_in *sock_in;
+  size_t sock_in_size;
 #ifdef __XENO__
   bool is_running;

trunk/tools/VrpnLite/src/VrpnLite.cpp

@@ -37 +37 @@
   Time dataSocketTimeout;
   char msg[256];
+  int src_id;
   
   while (!ToBeStopped()) {
-    
     if(vrpnobjects.size()>0) {
       //wait for last one to be sure all are up to date
@@ -51 +51 @@
     }
     
-    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout);
+    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
     if(rcv>0) {
       string object=msg;
@@ -57 +57 @@
       memcpy(&id,&msg[rcv-2],sizeof(id));
       dataSocket->HostToNetwork((char*)&id,sizeof(id));
+      
+      if(src_id==vrpnobjects_list.size()) {
+        vector<VrpnObject*> vect;
+        vrpnobjects_list.push_back(vect);
+        Printf("add vect %i\n",src_id);
+      }
       //assume we receive it in the good order
-      if(id==vrpnobjects.size()) {
-        Printf("adding object %s with id %i\n",object.c_str(),id);
+      if(id==vrpnobjects_list.at(src_id).size()) {
+        Printf("adding object %s with id %i from %i\n",object.c_str(),id,src_id);
         VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
         vrpnobjects.push_back(vrpnobject);
+        vrpnobjects_list.at(src_id).push_back(vrpnobject);
       }else {
         Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),id);
@@ -70 +77 @@
 
 void VrpnLite::SendObjects(void) const{
-  int16_t position[3];
+ return; 
+ int16_t position[3];
   int16_t quaternion[4];
   Time time;
   char datas[vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
   char *datasPtr=datas;
-  
-  for (vector<VrpnObject*>::const_iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
-    Vector3Df objectPosition;
-    Quaternion objectQuaternion;
-    const VrpnObject* vrpnobject=*it;
+ 
+  //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
+  for (int i=0;i<vrpnobjects_list.size();i++) {
+      for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
+        Vector3Df objectPosition;
+        Quaternion objectQuaternion;
+        const VrpnObject* vrpnobject=*it;
 
-    vrpnobject->GetPosition(objectPosition);
-    vrpnobject->GetQuaternion(objectQuaternion);
-    time=vrpnobject->GetLastPacketTime();
+        vrpnobject->GetPosition(objectPosition);
+        vrpnobject->GetQuaternion(objectQuaternion);
+        time=vrpnobject->GetLastPacketTime();
 
-    position[0]=ConvertPosition(objectPosition.x);
-    position[1]=ConvertPosition(objectPosition.y);
-    position[2]=ConvertPosition(objectPosition.z);
-    quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
-    quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
-    quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
-    quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
+        position[0]=ConvertPosition(objectPosition.x);
+        position[1]=ConvertPosition(objectPosition.y);
+        position[2]=ConvertPosition(objectPosition.z);
+        quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
+        quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
+        quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
+        quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
 
-    for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
-    for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
+        for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
+        for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
 
-    memcpy(datasPtr,position, sizeof(position));
-    datasPtr+=sizeof(position);
-    memcpy(datasPtr,quaternion, sizeof(quaternion));
-    datasPtr+=sizeof(quaternion);
+        memcpy(datasPtr,position, sizeof(position));
+        datasPtr+=sizeof(position);
+        memcpy(datasPtr,quaternion, sizeof(quaternion));
+        datasPtr+=sizeof(quaternion);
+      }
+      dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
+      memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
+      dataSocket->SendMessage(datas,sizeof(datas),i);
   }
-  
-  dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
-  memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
-  dataSocket->SendMessage(datas,sizeof(datas));
 }
 

trunk/tools/VrpnLite/src/VrpnLite.h

@@ -37 +37 @@
       flair::sensor::VrpnClient* vrpnclient;
       std::vector<flair::sensor::VrpnObject*> vrpnobjects;
+      std::vector<std::vector<flair::sensor::VrpnObject*>> vrpnobjects_list;
       flair::core::UdpSocket* dataSocket;
       void SendObjects(void) const;