1 | /*********************************************************************
|
---|
2 | // created: 2014/02/11 - 10:48
|
---|
3 | // filename: SickLMSSensor.h
|
---|
4 | //
|
---|
5 | // author: Cyril Fougeray
|
---|
6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
7 | //
|
---|
8 | // version: $Id: $
|
---|
9 | //
|
---|
10 | // purpose: Definition of the SickLMSSensor class
|
---|
11 | *********************************************************************/
|
---|
12 |
|
---|
13 | #ifndef SICKLMSSENSOR_H
|
---|
14 | #define SICKLMSSENSOR_H
|
---|
15 |
|
---|
16 | #include "Pacpus/kernel/ComponentBase.h"
|
---|
17 | #include "Pacpus/kernel/DbiteFile.h"
|
---|
18 |
|
---|
19 | #include "AbstractSickSensor.h"
|
---|
20 | #include "SickLMSData.h"
|
---|
21 |
|
---|
22 | #include <fstream>
|
---|
23 | #include <string>
|
---|
24 |
|
---|
25 | // Export macro for SickLMS DLL for Windows only
|
---|
26 | #ifdef WIN32
|
---|
27 | # ifdef SICKLMS_EXPORTS
|
---|
28 | // make DLL
|
---|
29 | # define SICKLMS_API __declspec(dllexport)
|
---|
30 | # else
|
---|
31 | // use DLL
|
---|
32 | # define SICKLMS_API __declspec(dllimport)
|
---|
33 | # endif
|
---|
34 | #else
|
---|
35 | // On other platforms, simply ignore this
|
---|
36 | # define SICKLMS_API
|
---|
37 | #endif
|
---|
38 |
|
---|
39 | class QEvent;
|
---|
40 |
|
---|
41 | namespace pacpus {
|
---|
42 |
|
---|
43 | class ShMem;
|
---|
44 | class SickComponent;
|
---|
45 |
|
---|
46 |
|
---|
47 | /// The class carrying Sick LMS message.
|
---|
48 | /** This class is used so that we can store every information sent by a Sick LMS sensor.
|
---|
49 | * First, the raw data is stored in \c body. Then, if the message is relevant, \c splitMessage is instanciated in order
|
---|
50 | * to parse easily information from the sensor.
|
---|
51 | * These data are then decoded and stored in the scanData structure.
|
---|
52 | */
|
---|
53 | class SICKLMS_API MessageLMS
|
---|
54 | {
|
---|
55 | public:
|
---|
56 | /// Constructor.
|
---|
57 | MessageLMS()
|
---|
58 | {
|
---|
59 | time = 0;
|
---|
60 | timerange = 0;
|
---|
61 | memset(&data,0,sizeof(data));
|
---|
62 | }
|
---|
63 |
|
---|
64 | /// Destructor.
|
---|
65 | ~MessageLMS(){}
|
---|
66 |
|
---|
67 | scanData data; //!< Every needed information about the scan (general info + scan points).
|
---|
68 |
|
---|
69 | long msgSize; //!< Size of the message
|
---|
70 |
|
---|
71 | std::vector<std::string>* splitMessage; //!< The message is split into an array of string in order to be processed easily.
|
---|
72 | char* body; //!< Raw data
|
---|
73 | road_time_t time; //!< Time when the first packet of the message is received.
|
---|
74 | road_timerange_t timerange; //!< Timerange : roughly, time between measurement of a point and the processing step (not implemented).
|
---|
75 | };
|
---|
76 |
|
---|
77 |
|
---|
78 | //! The class implenting receiving, decoding and storing process of Sick LMS data.
|
---|
79 | /**
|
---|
80 | * This class can be used as a particular thread to acquire data from Sick LDMRS sensors.
|
---|
81 | * The Ethernet interface is used to get data from the sensor. Thus, the goal of this class is to
|
---|
82 | * get packets and decode them. Also, it offers the possibility to store all relevant information in
|
---|
83 | * two files (.dbt and .utc).
|
---|
84 | * It can be managed by SickComponent objects.
|
---|
85 | */
|
---|
86 | class SickLMSSensor : public AbstractSickSensor
|
---|
87 | {
|
---|
88 | Q_OBJECT
|
---|
89 | public:
|
---|
90 | /// Constructor
|
---|
91 | SickLMSSensor(QObject *parent);
|
---|
92 |
|
---|
93 | /**
|
---|
94 | * @brief SickLMSSensor constructor.
|
---|
95 | * @param parent Basically, a SickComponent object.
|
---|
96 | * @param name Name of the sensor in order to write on .dbt and .utc files and to recognize every sensors used.
|
---|
97 | * @param ip The IP address of the remote Sick LMS sensor.
|
---|
98 | * @param port The port of the remote Sick LMS sensor.
|
---|
99 | * @param recording If \c true, data is recorded into dbt + utc files. Data is not recorded otherwise.
|
---|
100 | */
|
---|
101 | SickLMSSensor(QObject *parent, QString name, QString ip, int port, int recording);
|
---|
102 |
|
---|
103 | /// Destructor
|
---|
104 | ~SickLMSSensor();
|
---|
105 |
|
---|
106 | void run() {}
|
---|
107 |
|
---|
108 | void stopActivity(); /*!< To stop the processing thread. */
|
---|
109 | void startActivity(); /*!< To start the processing thread. */
|
---|
110 |
|
---|
111 | /**
|
---|
112 | * @brief reconstituteMessage reconstitute a complete message from received packets
|
---|
113 | * @param packet Raw data coming from the sensor.
|
---|
114 | * @param length Length of the raw data received.
|
---|
115 | * @param time Time of the last received data.
|
---|
116 | *
|
---|
117 | * A message starts with a <STX> (0x02 in ASCII) char and ends with <ETX> (0x03 in ASCII).
|
---|
118 | */
|
---|
119 | void reconstituteMessage(const char * packet, const int length, road_time_t time);
|
---|
120 |
|
---|
121 | // decoupe le message en scan
|
---|
122 | /**
|
---|
123 | * @brief processScanData Parse information and process every needed values.
|
---|
124 | * @param msg Carries a message. splitMessage field of MessageLMS must be filled.
|
---|
125 | * @return Not used for the moment.
|
---|
126 | */
|
---|
127 | int processScanData(MessageLMS *msg);
|
---|
128 |
|
---|
129 | /**
|
---|
130 | * @brief isMessageComplete find the <ETX> character (corresponding to the end of a message).
|
---|
131 | * @param packets Raw data.
|
---|
132 | * @param size Size of raw data.
|
---|
133 | * @return The index of the <ETX> character.
|
---|
134 | */
|
---|
135 | int isMessageComplete(const char* packets, unsigned int size);
|
---|
136 |
|
---|
137 | public Q_SLOTS:
|
---|
138 | //Fonction qui permet de trier les données reçues et de les enregistrer dans une structure point, puis dans un fichier binaire .dbt
|
---|
139 | void customEvent(QEvent * e);
|
---|
140 | void configure();
|
---|
141 |
|
---|
142 | public:
|
---|
143 | SickSocket * S_socket; //on déclare pointeur de socket
|
---|
144 | // SickComponent * myParent;
|
---|
145 |
|
---|
146 | private:
|
---|
147 | // to recognize between several sensors
|
---|
148 | QString name_;
|
---|
149 |
|
---|
150 | // The SickLMS IP or hostname
|
---|
151 | QString ipaddr_;
|
---|
152 |
|
---|
153 | // The SickLMS port
|
---|
154 | int port_;
|
---|
155 |
|
---|
156 | bool recording_; // Enable storing in DBT
|
---|
157 |
|
---|
158 | scanCfg mainCfg;
|
---|
159 |
|
---|
160 | void storePendingBytes(road_time_t time);
|
---|
161 | void fillDataHeader(MessageLMS & msg);
|
---|
162 | void fillScanHeader(MessageLMS & msg);
|
---|
163 | void askScanCfg();
|
---|
164 |
|
---|
165 | int findSTX(const char* packets, const unsigned int size );
|
---|
166 | int splitMessage(MessageLMS* message);
|
---|
167 | long xstol(std::string str);
|
---|
168 |
|
---|
169 | std::string kSickMemoryName;
|
---|
170 | std::string kSickDbtFileName;
|
---|
171 | std::string kSickUtcFileName;
|
---|
172 |
|
---|
173 | std::list<MessageLMS> msgList;
|
---|
174 |
|
---|
175 | MessagePacket pendingBytes;
|
---|
176 |
|
---|
177 | // write the data on the disk in the dbt file and the associated binary file
|
---|
178 | void writeData(MessageLMS &msg);
|
---|
179 |
|
---|
180 | // SickLMS_dbt dbtData_;
|
---|
181 | pacpus::DbiteFile dbtFile_;
|
---|
182 | std::ofstream dataFile_;
|
---|
183 |
|
---|
184 | #ifdef SickLMS_SH_MEM
|
---|
185 | ShMem * shmem_;
|
---|
186 | SickLMS_shMem sickData;
|
---|
187 | #endif
|
---|
188 |
|
---|
189 | };
|
---|
190 |
|
---|
191 | } // namespace pacpus
|
---|
192 |
|
---|
193 | #endif // SICKLMSSENSOR_H
|
---|