source: flair-src/trunk/lib/FlairIpc/src/IpcController.cpp @ 397

Last change on this file since 397 was 397, checked in by Sanahuja Guillaume, 8 months ago

add ipc lib

  • Property svn:eol-style set to native
File size: 11.6 KB
RevLine 
[397]1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2021/03/03
6//  filename:   IpcController.cpp
7//
8//  author:     Sébastien Ambroziak
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    class that gets remote controls through an ethernet connection.
14//              Typical use case: a remote control is plugged in a workstation
15//              and sends remote control
16//              data to a distant target (this class) through Wifi//
17//
18/*********************************************************************/
19#include "IpcController.h"
20#include <Controller.h>
21#include <FrameworkManager.h>
22#include <TcpSocket.h>
23#include <UdpSocket.h>
24#include <cstring>
25#include <string>
26#include <Matrix.h>
27#include <stdexcept>
28
29using namespace flair::core;
30using namespace flair::gui;
31using std::string;
32
33namespace flair {
34namespace sensor {
35
36IpcController::IpcController(string name, uint16_t _port,
37                                         uint8_t priority)
38    : TargetController(name, priority), listeningPort(_port),
39      receiveCurrentPosition(0) {
40 
41  Thread::Err("ipc is not yet implemented!\n");
42       
43  const bool blocking = true;
44  listeningSocket =
45      new TcpSocket(getFrameworkManager(), "TEC_listening_socket", blocking, blocking);
46  dataSocket =
47      new UdpSocket(getFrameworkManager(), "TEC_data_socket", _port + 1); // receiving side
48}
49
50IpcController::~IpcController() {
51  // We are (currently) the server side. We must ask the client side to initiate
52  // tcp connexion closing to avoid the server socket
53  // to get stuck in TIME_WAIT state
54  Message msg(32);
55  if (controlSocket) {
56    Message cancelAcquisition(sizeof(ControllerAction));
57    ControllerAction exit = ControllerAction::Exit;
58    memcpy(cancelAcquisition.buffer, &exit, sizeof(ControllerAction));
59    controlSocket->SendMessage(cancelAcquisition.buffer,
60                               cancelAcquisition.bufferSize);
61    // We don't expect any more data from the client, we're just waiting for the
62    // socket to be closed by the client
63    controlSocket->RecvMessage(msg.buffer, msg.bufferSize);
64    delete controlSocket;
65  }
66
67  // TargetController calls IpcController methods in its run
68  // we must stop the thread now
69  SafeStop();
70  Join();
71  delete dataSocket;
72  delete listeningSocket;
73}
74
75bool IpcController::IsConnected() const {
76  // TODO
77}
78
79bool IpcController::IsDataFrameReady() {
80  // read up to the last data
81  ssize_t received;
82  size_t bytesToReceive = dataFrameSize - receiveCurrentPosition;
83  bool fullDatagramReceived = false;
84
85    do {
86        received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
87        if (received>0) {
88            receiveCurrentPosition+=received;
89            bytesToReceive-=received;
90            if (bytesToReceive==0) {
91                //a full datagram has been read in receiveFrameBuffer
92                fullDatagramReceived=true;
93                //we swap the data and reception buffers to avoid copy
94                char *swapFrameBuffer=dataFrameBuffer;
95                dataFrameBuffer=receiveFrameBuffer;
96                receiveFrameBuffer=swapFrameBuffer;
97                receiveCurrentPosition=0;
98                bytesToReceive=dataFrameSize;
99            }
100        }
101    } while (!(received<0));
102
103  return fullDatagramReceived;
104}
105
106uint8_t IpcController::getByteOrNull(char *buffer, int byte,
107                                           size_t bufferSize) {
108  if (byte < bufferSize)
109    return buffer[byte];
110  else
111    return 0;
112}
113
114uint32_t IpcController::charBufferToUint32(char *buffer,
115                                                 size_t bufferSize) {
116  union {
117    uint32_t int32;
118    char byte[4];
119  } bitField;
120  if (!IsBigEndian()) {
121    bitField.byte[0] = getByteOrNull(buffer, 3, bufferSize);
122    bitField.byte[1] = getByteOrNull(buffer, 2, bufferSize);
123    bitField.byte[2] = getByteOrNull(buffer, 1, bufferSize);
124    bitField.byte[3] = getByteOrNull(buffer, 0, bufferSize);
125  } else {
126    bitField.byte[0] = getByteOrNull(buffer, 0, bufferSize);
127    bitField.byte[1] = getByteOrNull(buffer, 1, bufferSize);
128    bitField.byte[2] = getByteOrNull(buffer, 2, bufferSize);
129    bitField.byte[3] = getByteOrNull(buffer, 3, bufferSize);
130  }
131  return bitField.int32;
132}
133
134// read _up_to_16_bits_ in a buffer
135uint16_t IpcController::readBits(uint8_t offsetInBits,
136                                       uint8_t valueSizeInBits, char *buffer,
137                                       size_t bufferSize) {
138  // parameters check
139  if (valueSizeInBits > 16)
140    throw std::range_error("bitfield should be at max 16bits wide");
141  size_t minBufferSize = (offsetInBits + valueSizeInBits) / 8;
142  if ((offsetInBits + valueSizeInBits) % 8 != 0)
143    minBufferSize++;
144  if (bufferSize < minBufferSize)
145    throw std::range_error("buffer too small");
146  // skip first bytes
147  size_t bytesToSkip = offsetInBits / 8;
148  buffer += bytesToSkip;
149  bufferSize -= bytesToSkip;
150  offsetInBits -= bytesToSkip * 8;
151  // take care of endianness
152  uint32_t value = charBufferToUint32(buffer, bufferSize);
153  value >>= 32 - offsetInBits - valueSizeInBits;
154  value &= (1 << valueSizeInBits) - 1;
155  return (uint16_t)value;
156}
157
158void IpcController::AcquireAxisData(core::Matrix &axis) {
159  axis.GetMutex();
160  // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
161  for (unsigned int i = 0; i < axisNumber; i++) {
162    uint16_t rawAxisValue =
163        readBits(i * bitsPerAxis, bitsPerAxis, dataFrameBuffer, dataFrameSize);
164    //        uint16_t
165    //        rawAxisValue=readBits(i*bitsPerAxis,bitsPerAxis,testFrameBuffer);
166    uint16_t scale = 1 << (bitsPerAxis - 1);
167    // Thread::Info("RawAxisValue=%d, scale=%d => scaled rawValue=%d, float
168    // value)%f\n",rawAxisValue,scale,rawAxisValue-scale,(rawAxisValue-scale)/(float)scale);
169    axis.SetValueNoMutex(i, 0, (rawAxisValue - scale) / (float)scale);
170  }
171  axis.ReleaseMutex();
172}
173
174void IpcController::AcquireButtonData(core::Matrix &button) {
175  uint8_t buttonValue;
176  int currentButton = 0;
177  button.GetMutex();
178  /*
179  for (unsigned int i=0;i<buttonNumber;i++) {
180      memcpy(&buttonValue,buttonOffset+i*sizeof(bool),sizeof(bool));
181      dataSocket->NetworkToHost((char*)&buttonValue,sizeof(bool));
182      button.SetValueNoMutex(i,0,buttonValue);
183//        if (buttonValue) Thread::Info("Debug: button '%s' pressed\n",
184GetButtonName(i).c_str());
185  }*/
186  for (unsigned int i = 0; i < buttonNumber / 8; i++) {
187    memcpy(&buttonValue, dataFrameBuffer + buttonOffset + i * sizeof(uint8_t),
188           sizeof(uint8_t));
189    //        dataSocket->NetworkToHost((char*)&buttonValue,sizeof(uint8_t));
190    for (unsigned int j = 0; j < 8; j++) {
191      button.SetValueNoMutex(currentButton, 0, (buttonValue >> j) & 0x01);
192      currentButton++;
193    }
194  }
195  button.ReleaseMutex();
196}
197
198string IpcController::GetAxisName(unsigned int axisId) const {
199  // TODO: should throw an exception if axisName==NULL or axisId>axisNumber
200  return axisName[axisId];
201}
202
203string IpcController::GetButtonName(unsigned int buttonId) const {
204  // TODO: should throw an exception if buttonName==NULL or
205  // buttonId>buttonNumber
206  return buttonName[buttonId];
207}
208
209bool IpcController::ProcessMessage(Message *msg) {
210  return !(controlSocket->SendMessage(msg->buffer, msg->bufferSize, TIME_INFINITE) < 0);
211}
212
213bool IpcController::IsControllerActionSupported(
214    ControllerAction action) const {
215  // TODO: here we should ask the remote side (host). Probably through the
216  // control socket
217  switch (action) {
218  case ControllerAction::SetLedOn:
219    return true;
220  case ControllerAction::SetLedOff:
221    return true;
222  case ControllerAction::Rumble:
223    return true;
224  case ControllerAction::FlashLed:
225    return true;
226  default:
227    return false;
228  }
229}
230
231bool IpcController::ControllerInitialization() {
232  char message[1024];
233  ssize_t received;
234  bool connected = false;
235  bool mustReadMore;
236
237  listeningSocket->Listen(listeningPort);
238  Thread::Info("Debug: Listening to port %d\n", listeningPort);
239  // accept incoming connection
240  while (!controlSocket) {
241    try {
242      controlSocket = listeningSocket->Accept(100000000);
243    } catch (std::logic_error &e) {
244      Thread::Err("%s\n",e.what());
245      if (ToBeStopped())
246        return false;
247    } catch (std::runtime_error e) {
248      // timeout
249      //Thread::Err("%s\n",e.what());
250      if (ToBeStopped())
251        return false;
252    }
253  }
254  Thread::Info("Debug: Connexion accepted\n");
255
256  // get axis stuff
257  bool axisNumberRead = false;
258  while (!axisNumberRead) {
259    try {
260      axisNumber = controlSocket->ReadUInt32(100000000);
261      // Thread::Info("Debug: axisNumber %d\n", axisNumber);
262      axisNumberRead = true;
263    } catch (std::runtime_error e) {
264      // timeout
265      if (ToBeStopped())
266        return false;
267    }
268  }
269  bool bitsPerAxisRead = false;
270  while (!bitsPerAxisRead) {
271    try {
272      bitsPerAxis = controlSocket->ReadUInt32(100000000);
273      // Thread::Info("Debug: bits per axis %d\n", bitsPerAxis);
274      bitsPerAxisRead = true;
275    } catch (std::runtime_error e) {
276      // timeout
277      if (ToBeStopped())
278        return false;
279    }
280  }
281  axisName = new string[axisNumber];
282  for (unsigned int i = 0; i < axisNumber; i++) {
283    // read string size
284    int stringSize;
285    bool stringSizeRead = false;
286    while (!stringSizeRead) {
287      try {
288        stringSize = controlSocket->ReadUInt32(100000000);
289        stringSizeRead = true;
290      } catch (std::runtime_error e) {
291        // timeout
292        if (ToBeStopped())
293          return false;
294      }
295    }
296    // read string
297    bool axisNameRead = false;
298    while (!axisNameRead) {
299      try {
300        axisName[i] = controlSocket->ReadString(stringSize, 100000000);
301        axisNameRead = true;
302      } catch (std::runtime_error e) {
303        // timeout
304        if (ToBeStopped())
305          return false;
306      }
307    }
308    // Thread::Info("Debug: axisName for axis %d %s\n", i, axisName[i].c_str());
309  }
310
311  // get button stuff
312  bool buttonNumberRead = false;
313  while (!buttonNumberRead) {
314    try {
315      buttonNumber = controlSocket->ReadUInt32(100000000);
316      buttonNumberRead = true;
317    } catch (std::runtime_error e) {
318      // timeout
319      if (ToBeStopped())
320        return false;
321    }
322  }
323  // Thread::Info("Debug: buttonNumber %d\n", buttonNumber);
324  buttonName = new string[buttonNumber];
325  for (unsigned int i = 0; i < buttonNumber; i++) {
326    // read string size
327    int stringSize;
328    bool stringSizeRead = false;
329    while (!stringSizeRead) {
330      try {
331        stringSize = controlSocket->ReadUInt32(100000000);
332        stringSizeRead = true;
333      } catch (std::runtime_error e) {
334        // timeout
335        if (ToBeStopped())
336          return false;
337      }
338    }
339    // read string
340    bool buttonNameRead = false;
341    while (!buttonNameRead) {
342      try {
343        buttonName[i] = controlSocket->ReadString(stringSize, 10000000);
344        buttonNameRead = true;
345      } catch (std::runtime_error e) {
346        // timeout
347        if (ToBeStopped())
348          return false;
349      }
350    }
351    // Thread::Info("Debug: buttonName for button %d %s\n", i,
352    // buttonName[i].c_str());
353  }
354
355  //    dataFrameSize=axisNumber*sizeof(float)+buttonNumber/8*sizeof(uint8_t);
356  buttonOffset = (axisNumber * bitsPerAxis) / 8;
357  if ((axisNumber * bitsPerAxis) % 8 != 0)
358    buttonOffset++;
359  dataFrameSize = buttonOffset + (buttonNumber / 8) * sizeof(uint8_t);
360  if ((buttonNumber % 8) != 0)
361    dataFrameSize++;
362  dataFrameBuffer = new char[dataFrameSize];
363  receiveFrameBuffer = new char[dataFrameSize];
364
365  Thread::Info("Controller connected with host side\n");
366  if (buttonNumber % 8 != 0)
367    Thread::Err("Button number is not multiple of 8\n");
368  return true;
369}
370
371} // end namespace sensor
372} // end namespace flair
Note: See TracBrowser for help on using the repository browser.