1 | /*********************************************************************
|
---|
2 | // created: 2014/02/02
|
---|
3 | // filename: SickSocket.cpp
|
---|
4 | //
|
---|
5 | // author: Cyril Fougeray
|
---|
6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
7 | //
|
---|
8 | // version: $Id: $
|
---|
9 | //
|
---|
10 | // purpose: Management of the socket connection with Sick sensors
|
---|
11 | *********************************************************************/
|
---|
12 |
|
---|
13 | #include "SickSocket.h"
|
---|
14 |
|
---|
15 | #include <QCoreApplication>
|
---|
16 | #include <QDebug>
|
---|
17 |
|
---|
18 |
|
---|
19 | #include "Pacpus/kernel/Log.h"
|
---|
20 |
|
---|
21 | namespace pacpus {
|
---|
22 |
|
---|
23 | DECLARE_STATIC_LOGGER("pacpus.base.SickSocket");
|
---|
24 |
|
---|
25 | //////////////////////////////////////////////////////////////////////////
|
---|
26 | /// Constructor
|
---|
27 | SickSocket::SickSocket(AbstractSickSensor * parent)
|
---|
28 | : myParent(parent)
|
---|
29 | {
|
---|
30 | socket = new QTcpSocket(this);
|
---|
31 | connect( socket, SIGNAL(connected()), this,SLOT(socketConnected()) );
|
---|
32 | connect( socket, SIGNAL(disconnected()),this, SLOT(socketConnectionClosed()) );
|
---|
33 | connect( socket, SIGNAL(readyRead()),this, SLOT(socketReadyRead()) );
|
---|
34 | connect( socket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(socketError(QAbstractSocket::SocketError)) );
|
---|
35 | }
|
---|
36 |
|
---|
37 |
|
---|
38 | //////////////////////////////////////////////////////////////////////////
|
---|
39 | /// Destructor
|
---|
40 | SickSocket::~SickSocket()
|
---|
41 | {
|
---|
42 | delete socket;
|
---|
43 | }
|
---|
44 |
|
---|
45 | //////////////////////////////////////////////////////////////////////////
|
---|
46 | /// public slot
|
---|
47 | /// connection to server
|
---|
48 | void SickSocket::connectToServer(QString host, int port)
|
---|
49 | {
|
---|
50 | qDebug("trying to connect to server");
|
---|
51 | socket->connectToHost(host,port);
|
---|
52 | }
|
---|
53 |
|
---|
54 | //////////////////////////////////////////////////////////////////////////
|
---|
55 | /// protected slot
|
---|
56 | int SickSocket::socketConnected()
|
---|
57 | {
|
---|
58 | qDebug() << "we are connected to the server " << socket->peerName().toLatin1()
|
---|
59 | << " at the port " << socket->peerPort() << " via the socket " << socket->socketDescriptor();
|
---|
60 | emit configuration();
|
---|
61 |
|
---|
62 | return 1;
|
---|
63 | }
|
---|
64 |
|
---|
65 | //////////////////////////////////////////////////////////////////////////
|
---|
66 | /// protected slot
|
---|
67 | void SickSocket::socketConnectionClosed()
|
---|
68 | {
|
---|
69 | qDebug("the connection was closed");
|
---|
70 | }
|
---|
71 |
|
---|
72 | //////////////////////////////////////////////////////////////////////////
|
---|
73 | /// protected slot
|
---|
74 | /// new incoming data to read
|
---|
75 | void SickSocket::socketReadyRead()
|
---|
76 | {
|
---|
77 | SickFrame * frame = new SickFrame();
|
---|
78 | frame->time = road_time();
|
---|
79 | frame->size = socket->bytesAvailable();
|
---|
80 | frame->msg = new char[frame->size];
|
---|
81 |
|
---|
82 | if (!frame->msg) {
|
---|
83 | LOG_FATAL("cannot allocate memory of size " << frame->size
|
---|
84 | << " - file: " << __FILE__
|
---|
85 | << " line: " << __LINE__
|
---|
86 | );
|
---|
87 | }
|
---|
88 |
|
---|
89 | frame->size = socket->read(frame->msg, (qint64) frame->size);
|
---|
90 |
|
---|
91 | SickFrameEvent *e = new SickFrameEvent;
|
---|
92 | e->frame = frame;
|
---|
93 | QCoreApplication::postEvent((QObject*)myParent, e);
|
---|
94 | }
|
---|
95 |
|
---|
96 | //////////////////////////////////////////////////////////////////////////
|
---|
97 | /// protected slot
|
---|
98 | void SickSocket::socketError(QAbstractSocket::SocketError e)
|
---|
99 | {
|
---|
100 | qWarning("socket error number %d",e);
|
---|
101 | }
|
---|
102 |
|
---|
103 | //////////////////////////////////////////////////////////////////////////
|
---|
104 | /// Send data to server via a text stream
|
---|
105 | void SickSocket::sendToServer(QString data) //a adapter aux données binaires
|
---|
106 | {
|
---|
107 | mutex.lock();
|
---|
108 | QTextStream os(socket);
|
---|
109 | os << data.toAscii() << endl;
|
---|
110 | mutex.unlock();
|
---|
111 | qDebug() << "data sent to server: " << data.toAscii() ; //a adapter aussi
|
---|
112 | }
|
---|
113 |
|
---|
114 | } // namespace pacpus
|
---|