1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
#include "SocketStream.h"
#include <cassert>
#include <glib.h>

#include <Tools/Debug/NaoTHAssert.h>

SocketStream::SocketStream()
: mRecvdLen(0),socket(NULL)
{
    mRecvBuf = new char[default_recv_buffer_size + 1];
    mRecvBufSize = default_recv_buffer_size;
}

SocketStream::~SocketStream()
{
  init(NULL);
  delete [] mRecvBuf;
}

void SocketStream::init(GSocket* s)
{
  if ( socket != NULL)
  {
    g_object_unref(socket);
    socket = NULL;
  }

  if ( s != NULL)
  {
    g_object_ref(s);
    socket = s;
  }
  mRecvdLen = 0;
}

void SocketStream::send(const std::string& msg) throw(std::runtime_error)
{
  if(socket == NULL)
  {
    return;
  }
  
  if(g_socket_is_connected(socket))
  {
    GError* err = NULL;

#ifdef NAO
    ASSERT(g_socket_get_blocking(socket) == false);
#endif

    int sendBytes = static_cast<int> (g_socket_send(socket, msg.c_str(), msg.size(), NULL, &err));
    if(sendBytes < 1)
    {
      std::cerr << "ERROR in SocketStream send: sendBytes=" << sendBytes << std::endl;
    }
    if(err)
    {
      std::string errMsg = err->message;
      g_error_free(err);
      throw std::runtime_error(errMsg);
    }
  }
  else
  {
    throw std::runtime_error("Can't send, not connected");
  }
}

SocketStream& SocketStream::send()
{
  send(mSendStr.str());
  mSendStr.str("");
  return *this;
}

int SocketStream::recv(std::string& msg) throw(std::runtime_error)
{
  if(socket == NULL)
  {
    return -1;
  }
  else if(!g_socket_is_connected(socket))
  {
    throw std::runtime_error("Can't receive, not connected");
    return -1;
  }

  memset(mRecvBuf, 0, mRecvBufSize + 1);
  GError* err = NULL;
  int status = static_cast<int> (g_socket_receive(socket, mRecvBuf, mRecvBufSize, NULL, &err));
  if (status < 0)
  {
    std::string errMsg = err->message;
    g_error_free(err);
    throw std::runtime_error(errMsg);
  }
  else if (status > 0) {
    msg = mRecvBuf;
  } else
  {
    msg = "";
  }
  return status;
}

void SocketStream::prefixedSend()
{
  if (!mSendStr.str().empty()) {
    // prefix the message with it's payload length
    unsigned int len = static_cast<unsigned int> (g_htonl(mSendStr.str().size()));
    char preChar[sizeof (unsigned int) ];
    memcpy(preChar, &len, sizeof (unsigned int));
    std::string msg = mSendStr.str();
    msg.insert(0, preChar, sizeof (unsigned int));
    mSendStr.str() = std::string(preChar) + mSendStr.str();
    send(msg);
    mSendStr.str("");
  }
}

bool SocketStream::isFixedLengthDataAvailable(unsigned int len) throw(std::runtime_error)
{
  if(socket == NULL || !g_socket_is_connected(socket))
  {
    return false;
  }

  if (len == 0) return true;

  /* check whether the read_buffer is large enough to handle this request
  if not, reallocate the array */
  if (mRecvBufSize < len) {
    reallocRecvBuffer(len);
  }

  for (;;) {
    /* See if we have enough data to satisfy request */
    if (mRecvdLen >= len) return true;
    /* there was not enough data in the read buffer, so let's try to get some more */
    GError* err = NULL;
    int res = static_cast<int> (g_socket_receive(socket, mRecvBuf + mRecvdLen, mRecvBufSize - mRecvdLen, NULL, &err));
       
    if (res <= 0)
    {
      // is there an error?
      if(err) {
        std::cout << err->message << std::endl;
      } 

      // check if still connected
      int lostConnection = g_socket_condition_check(socket, G_IO_IN) & (G_IO_HUP|G_IO_ERR);
      if ( lostConnection )
      {
        throw std::runtime_error("lost connection");
      } else if(!err) {
        std::cout << "Weird: still connected, but received 0 bytes ad no error." << std::endl;
      }

      return false;
    }
    /* res is > 0 */
    mRecvdLen += static_cast<unsigned int>(res);
  }
}

unsigned int SocketStream::prefixedRecv(std::string& msg)
{
  msg = "";
  unsigned int messLen;

  bool res = isFixedLengthDataAvailable(static_cast<unsigned int> (sizeof (unsigned int)));
  if (!res) return 0;
  //get the message length
  messLen = g_ntohl((*(unsigned int*) mRecvBuf));

  //try and get the data
  res = isFixedLengthDataAvailable(messLen + static_cast<unsigned int> (sizeof (unsigned int)));
  if (!res) return 0;

  //we have to copy, skipping the sizeof(int) bytes at the beginning
  msg.insert(0, mRecvBuf + sizeof (unsigned int), messLen);
  mRecvdLen -= (static_cast<unsigned int> (sizeof (unsigned int)) +messLen);
  memmove(mRecvBuf, mRecvBuf + sizeof (unsigned int) +messLen, mRecvdLen);
  return messLen;
}

void SocketStream::reallocRecvBuffer(unsigned int size)
{
  char* oldBuffer = mRecvBuf;
  mRecvBuf = new char[size + 1];
  memcpy(mRecvBuf, oldBuffer, mRecvdLen);
  mRecvBufSize = size;
  delete [] oldBuffer;
}