Changeset 363 in flair-src for trunk


Ignore:
Timestamp:
Sep 3, 2020, 8:54:16 AM (15 months ago)
Author:
Sanahuja Guillaume
Message:

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

Location:
trunk
Files:
6 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/UdpSocket.cpp

    r178 r363  
    3939UdpSocket::~UdpSocket() { delete pimpl_; }
    4040
    41 void UdpSocket::SendMessage(const char *message, size_t message_len) {
    42   pimpl_->SendMessage(message, message_len);
     41void UdpSocket::SendMessage(const char *message, size_t message_len,int dst_id) {
     42  pimpl_->SendMessage(message, message_len,dst_id);
    4343}
    4444
     
    4646
    4747ssize_t UdpSocket::RecvMessage(char *buf, size_t buf_len, Time timeout, char *src,
    48                             size_t *src_len) {
    49   return pimpl_->RecvMessage(buf, buf_len, timeout, src, src_len);
     48                            size_t *src_len,int *src_id) {
     49  return pimpl_->RecvMessage(buf, buf_len, timeout, src, src_len,src_id);
    5050}
    5151
  • trunk/lib/FlairCore/src/UdpSocket.h

    r178 r363  
    1414#define UDPSOCKET_H
    1515
    16 #include <unistd.h>
    17 #include <stdint.h>
    1816#include <Object.h>
    1917
     
    7674  * \param message message
    7775  * \param message_len message length
     76  * \param dst_id id of the dst if multiple connection (quick and dirty hack for sido, only works on NRT)
    7877  */
    79   void SendMessage(const char *message, size_t message_len);
     78  void SendMessage(const char *message, size_t message_len,int dst_id=0);
    8079
    8180  /*!
     
    8483  * Receive a message and wait up to timeout. \n
    8584  * If src and src_len are specified, the source of the message will be
    86   * copied in the src buffer. \n
     85  * copied in the src buffer. Source is the Flair name of sender\n
    8786  * Note that in case of a broadcast socket, own messages are filtered and
    8887  * are not received.
     
    9392  * \param src buffer to put source name
    9493  * \param src_len buffer length
     94  * \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)
    9595  *
    9696  * \return size of the received message
    9797  */
    9898  ssize_t RecvMessage(char *buf, size_t buf_len, Time timeout, char *src = NULL,
    99                       size_t *src_len = NULL);
     99                      size_t *src_len = NULL,int *src_id=NULL);
    100100
    101101  void NetworkToHost(char *data, size_t dataSize);
  • trunk/lib/FlairCore/src/UdpSocket_impl.cpp

    r213 r363  
    5353void UdpSocket_impl::Init(void) {
    5454  int yes = 1;
     55  sock_in_size=0;
     56  sock_in=NULL;
    5557
    5658  fd = socket(AF_INET, SOCK_DGRAM, 0); // UDP
     
    136138      self->Err("gethostbyname\n");
    137139    }
    138 
    139     sock_in.sin_addr = *(in_addr *)hostinfo->h_addr;
    140     sock_in.sin_port = htons(port);
    141     sock_in.sin_family = AF_INET;
     140    sock_in=(struct sockaddr_in*)malloc(sizeof(struct sockaddr_in));
     141    sock_in[0].sin_addr = *(in_addr *)hostinfo->h_addr;
     142    sock_in[0].sin_port = htons(port);
     143    sock_in[0].sin_family = AF_INET;
     144    sock_in_size++;
    142145  }
    143146}
     
    155158#endif
    156159  close(fd);
    157 }
    158 
    159 void UdpSocket_impl::SendMessage(const char *src, size_t src_len) {
     160  free(sock_in);
     161}
     162
     163void UdpSocket_impl::SendMessage(const char *msg, size_t msg_len,int dst_id) {
    160164  ssize_t written;
    161165  string to_send;
    162166       
    163167  if (broadcast == true) {
    164     to_send = getFrameworkManager()->ObjectName() + ":" + string(src, src_len);
    165     src_len = to_send.size();
    166     src = (char *)to_send.c_str();
     168    to_send = getFrameworkManager()->ObjectName() + ":" + string(msg, msg_len);
     169    msg_len = to_send.size();
     170    msg = (char *)to_send.c_str();
    167171  }
    168172
    169173#ifdef __XENO__
    170174  // Printf("send pipe %s\n",src);
    171   written = rt_pipe_write(&pipe, src, src_len, P_NORMAL);
     175  written = rt_pipe_write(&pipe, msg, msg_len, P_NORMAL);
    172176
    173177  if (written < 0) {
    174178    char errorMsg[256];
    175179    self->Err("rt_pipe_write error (%s)\n", strerror_r(-written, errorMsg, sizeof(errorMsg)));
    176   } else if (written != (ssize_t)src_len) {
     180  } else if (written != (ssize_t)msg_len) {
    177181    self->Err("rt_pipe_write error %i/%i\n", written, to_send.size());
    178182  }
    179183#else
    180   written =
    181       sendto(fd, src, src_len, 0, (struct sockaddr *)&sock_in, sizeof(sock_in));
     184  written = sendto(fd, msg, msg_len, 0, (struct sockaddr *)&(sock_in[dst_id]), sizeof(struct sockaddr_in));
    182185  if(written==-1) {
    183186                char errorMsg[256];
    184187    self->Err("sendto error (%s)\n",strerror_r(errno, errorMsg, sizeof(errorMsg)));
    185   } else if (written != (ssize_t)src_len) {
    186     self->Err("sendto error %i/%i\n",written,src_len);
     188  } else if (written != (ssize_t)msg_len) {
     189    self->Err("sendto error %i/%i\n",written,msg_len);
    187190  }
    188191#endif
     
    205208  }
    206209#else
    207   written = sendto(fd, message.c_str(), message.size(), 0,
    208                    (struct sockaddr *)&sock_in, sizeof(sock_in));
     210  written = sendto(fd, message.c_str(), message.size(), 0, (struct sockaddr *)&sock_in, sizeof(struct sockaddr_in));
    209211  if (written != (ssize_t)message.size()) {
    210212    self->Err("sendto error\n");
     
    215217
    216218ssize_t UdpSocket_impl::RecvMessage(char *msg, size_t msg_len, Time timeout,
    217                                  char *src, size_t *src_len) {
     219                                 char *src, size_t *src_len,int* src_id) {
    218220  ssize_t nb_read;
    219221  char buffer[128];
     
    221223  nb_read = rt_pipe_read(&pipe, &buffer, sizeof(buffer), timeout);
    222224#else
    223   socklen_t sinsize = sizeof(sock_in);
    224   struct timeval tv;
    225225
    226226  if (timeout != TIME_NONBLOCK) {
     227    struct timeval tv;
    227228    int attr = fcntl(fd, F_GETFL, 0);
    228229    fcntl(fd, F_SETFL, attr & (~O_NONBLOCK));
     
    238239  }
    239240
     241  struct sockaddr_in sock_in_tmp;
    240242  if (broadcast == false) {
    241     nb_read =
    242         recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *)&sock_in, &sinsize);
     243    socklen_t sinsize = sizeof(struct sockaddr_in);
     244    nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, (sockaddr *)&sock_in_tmp, &sinsize);
    243245  } else {
    244246    nb_read = recvfrom(fd, buffer, sizeof(buffer), 0, NULL, NULL);
     247  }
     248 
     249  if(broadcast==false && nb_read>0) {
     250    int i;
     251    for(i=0;i<sock_in_size;i++) {
     252        if(sock_in[i].sin_port==sock_in_tmp.sin_port) {
     253            Printf("match found in former adress\n");
     254            *src_id=i;
     255            break;
     256        }
     257    }
     258    if(i==sock_in_size) {
     259        Printf("no match found in former adress\n");
     260        sock_in_size++;
     261        sock_in=(sockaddr_in*)realloc(sock_in,sock_in_size*sizeof(sockaddr_in));
     262        sock_in[sock_in_size-1]=sock_in_tmp;
     263        *src_id=sock_in_size-1;
     264    }
    245265  }
    246266#endif
     
    266286          return -1;
    267287        }
    268       } else if (nb_read - index - 1 + 1 >
    269                  (int)msg_len) { //+1 pour inserer un 0
    270         self->Warn("insufficent msg size (%i/%i)\n", nb_read - index - 1 + 1,
    271                    msg_len);
     288      } else if (nb_read - index - 1 + 1 > (int)msg_len) { //+1 pour inserer un 0
     289        self->Warn("insufficent msg size (%i/%i)\n", nb_read - index - 1 + 1, msg_len);
    272290        return -1;
    273291      }
     
    329347
    330348      if (FD_ISSET(caller->fd, &set)) {
    331         socklen_t sinsize = sizeof(caller->sock_in);
     349        socklen_t sinsize = sizeof(struct sockaddr_in);
    332350        if (caller->broadcast == false) {
    333351          nb_read = recvfrom(caller->fd, buffer, sizeof(buffer), 0,
     
    361379          nb_write = sendto(caller->fd, buffer, nb_read, 0,
    362380                            (struct sockaddr *)&(caller->sock_in),
    363                             sizeof(caller->sock_in));
     381                            sizeof(struct sockaddr_in));
    364382          if (nb_write != nb_read) {
    365383            caller->self->Err("sendto error\n");
  • trunk/lib/FlairCore/src/unexported/UdpSocket_impl.h

    r178 r363  
    3333
    3434  void SendMessage(std::string message);
    35   void SendMessage(const char *src, size_t src_len);
     35  void SendMessage(const char *msg, size_t msg_len,int dst_id);
    3636  ssize_t RecvMessage(char *msg, size_t msg_len, flair::core::Time timeout,
    37                       char *src = NULL, size_t *src_len = NULL);
     37                      char *src = NULL, size_t *src_len = NULL,int *src_id=NULL);
    3838
    3939private:
     
    4444  void Init(void);
    4545  const flair::core::UdpSocket *self;
    46   struct sockaddr_in sock_in;
     46  struct sockaddr_in *sock_in;
     47  size_t sock_in_size;
    4748#ifdef __XENO__
    4849  bool is_running;
  • trunk/tools/VrpnLite/src/VrpnLite.cpp

    r334 r363  
    3737  Time dataSocketTimeout;
    3838  char msg[256];
     39  int src_id;
    3940 
    4041  while (!ToBeStopped()) {
    41    
    4242    if(vrpnobjects.size()>0) {
    4343      //wait for last one to be sure all are up to date
     
    5151    }
    5252   
    53     ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout);
     53    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
    5454    if(rcv>0) {
    5555      string object=msg;
     
    5757      memcpy(&id,&msg[rcv-2],sizeof(id));
    5858      dataSocket->HostToNetwork((char*)&id,sizeof(id));
     59     
     60      if(src_id==vrpnobjects_list.size()) {
     61        vector<VrpnObject*> vect;
     62        vrpnobjects_list.push_back(vect);
     63        Printf("add vect %i\n",src_id);
     64      }
    5965      //assume we receive it in the good order
    60       if(id==vrpnobjects.size()) {
    61         Printf("adding object %s with id %i\n",object.c_str(),id);
     66      if(id==vrpnobjects_list.at(src_id).size()) {
     67        Printf("adding object %s with id %i from %i\n",object.c_str(),id,src_id);
    6268        VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
    6369        vrpnobjects.push_back(vrpnobject);
     70        vrpnobjects_list.at(src_id).push_back(vrpnobject);
    6471      }else {
    6572        Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),id);
     
    7077
    7178void VrpnLite::SendObjects(void) const{
    72   int16_t position[3];
     79 return;
     80 int16_t position[3];
    7381  int16_t quaternion[4];
    7482  Time time;
    7583  char datas[vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
    7684  char *datasPtr=datas;
    77  
    78   for (vector<VrpnObject*>::const_iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
    79     Vector3Df objectPosition;
    80     Quaternion objectQuaternion;
    81     const VrpnObject* vrpnobject=*it;
     85 
     86  //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
     87  for (int i=0;i<vrpnobjects_list.size();i++) {
     88      for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
     89        Vector3Df objectPosition;
     90        Quaternion objectQuaternion;
     91        const VrpnObject* vrpnobject=*it;
    8292
    83     vrpnobject->GetPosition(objectPosition);
    84     vrpnobject->GetQuaternion(objectQuaternion);
    85     time=vrpnobject->GetLastPacketTime();
     93        vrpnobject->GetPosition(objectPosition);
     94        vrpnobject->GetQuaternion(objectQuaternion);
     95        time=vrpnobject->GetLastPacketTime();
    8696
    87     position[0]=ConvertPosition(objectPosition.x);
    88     position[1]=ConvertPosition(objectPosition.y);
    89     position[2]=ConvertPosition(objectPosition.z);
    90     quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
    91     quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
    92     quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
    93     quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
     97        position[0]=ConvertPosition(objectPosition.x);
     98        position[1]=ConvertPosition(objectPosition.y);
     99        position[2]=ConvertPosition(objectPosition.z);
     100        quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
     101        quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
     102        quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
     103        quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
    94104
    95     for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
    96     for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
     105        for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
     106        for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
    97107
    98     memcpy(datasPtr,position, sizeof(position));
    99     datasPtr+=sizeof(position);
    100     memcpy(datasPtr,quaternion, sizeof(quaternion));
    101     datasPtr+=sizeof(quaternion);
     108        memcpy(datasPtr,position, sizeof(position));
     109        datasPtr+=sizeof(position);
     110        memcpy(datasPtr,quaternion, sizeof(quaternion));
     111        datasPtr+=sizeof(quaternion);
     112      }
     113      dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
     114      memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
     115      dataSocket->SendMessage(datas,sizeof(datas),i);
    102116  }
    103  
    104   dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
    105   memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
    106   dataSocket->SendMessage(datas,sizeof(datas));
    107117}
    108118
  • trunk/tools/VrpnLite/src/VrpnLite.h

    r330 r363  
    3737      flair::sensor::VrpnClient* vrpnclient;
    3838      std::vector<flair::sensor::VrpnObject*> vrpnobjects;
     39      std::vector<std::vector<flair::sensor::VrpnObject*>> vrpnobjects_list;
    3940      flair::core::UdpSocket* dataSocket;
    4041      void SendObjects(void) const;
Note: See TracChangeset for help on using the changeset viewer.