source: flair-src/branches/sanscv/lib/FlairSensorActuator/src/TargetEthController.cpp@ 370

Last change on this file since 370 was 324, checked in by Sanahuja Guillaume, 5 years ago

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