source: pacpussensors/trunk/VelodyneComponent/VelodyneComponent.cpp@ 77

Last change on this file since 77 was 66, checked in by DHERBOMEZ Gérald, 10 years ago

Modifications:

  • migration of alasca component to pacpus framework 0.1.X
  • new component for Velodyne HDL64E sensor
  • addition of xml example files for alasca, gps, cangateway and velodyne
File size: 14.0 KB
Line 
1/*********************************************************************
2// created: 2010/03/26 - 14:38
3// filename: VelodyneComponent.cpp
4//
5// author: Gerald Dherbomez
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: The acquisition component of the Velodyne HDL64S2 sensor
11//
12*********************************************************************/
13
14#include "VelodyneComponent.h"
15
16#include "Pacpus/kernel/cstdint.h"
17#include "Pacpus/kernel/ComponentFactory.h"
18#include "Pacpus/kernel/DbiteFileTypes.h"
19#include "Pacpus/kernel/Log.h"
20
21#include <QtEndian>
22#include <QUdpSocket>
23#include <string>
24
25using namespace pacpus;
26using namespace std;
27
28DECLARE_STATIC_LOGGER("pacpus.base.VelodyneComponent");
29
30/// Construct the factory
31static ComponentFactory<VelodyneComponent> sFactory("VelodyneComponent");
32
33/// Default host address = 0.0.0.0 (any address)
34static const string kDefaultHostAddress = "0.0.0.0";
35
36/// Default host port = 2368
37static const uint16_t kDefaultHostPort = 2368;
38
39static const string kPropertyRecording = "recording";
40
41static const string kVelodyneSharedMemoryName = "VELODYNE";
42static const string kDefaultOutputFilename = "velodyne_spheric.dbt";
43
44//////////////////////////////////////////////////////////////////////////
45/// Constructor
46VelodyneComponent::VelodyneComponent(QString name)
47 : ComponentBase(name)
48 , mPort(kDefaultHostPort)
49{
50 LOG_TRACE("constructor(" << name << ")");
51
52 ComponentManager * mgr = ComponentManager::getInstance();
53 LOG_DEBUG("manager = " << mgr);
54
55 setRecording (true);
56
57 // default values
58 if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) {
59 LOG_ERROR("cannot set address");
60 }
61 LOG_DEBUG("host address = " << mHost.toString());
62}
63
64//////////////////////////////////////////////////////////////////////////
65/// Destructor
66VelodyneComponent::~VelodyneComponent()
67{
68}
69
70//////////////////////////////////////////////////////////////////////////
71/// Called by the ComponentManager to start the component
72void VelodyneComponent::startActivity()
73{
74 mStartOfScan = false;
75 mEndOfScan = false;
76 mBlockIndex = 0;
77 mCurrentVelodyneData = 0;
78 mPreviousAngle = 0;
79 mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]);
80
81 mRunning = true;
82 initialize();
83}
84
85//////////////////////////////////////////////////////////////////////////
86/// TODO: doc
87void VelodyneComponent::run()
88{
89 initialize();
90 exec(); // launch the exec loop, blocking until receiving exit() signal ...
91 close();
92}
93
94//////////////////////////////////////////////////////////////////////////
95/// TODO: doc
96void VelodyneComponent::initialize()
97{
98 if (isRecording()) {
99 mVelodyneSphericDataFile.open(kDefaultOutputFilename, WriteMode, VELODYNE_RAW_SPHERIC_DATA, sizeof(VelodynePolarData));
100 }
101
102 initSocket();
103 if (!connect(mSocket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()))) {
104 LOG_ERROR("cannot connect SIGNAL(readyRead()) to SLOT(readPendingDatagrams())");
105 }
106
107 mShMem = new ShMem(kVelodyneSharedMemoryName.c_str(), sizeof(VelodynePolarData) );
108 if (!mShMem) {
109 LOG_FATAL("cannot create Velodyne shared memory");
110 return;
111 }
112}
113
114//////////////////////////////////////////////////////////////////////////
115/// TODO: doc
116void VelodyneComponent::initSocket()
117{
118 if (!(mHost.setAddress(kDefaultHostAddress.c_str()))) {
119 LOG_FATAL("failed to set address");
120 }
121 mPort = kDefaultHostPort;
122 mSocket = new QUdpSocket();
123 if (!mSocket) {
124 LOG_FATAL("cannot create socket");
125 return;
126 }
127 if (!(mSocket->bind(mHost, mPort))) {
128 LOG_ERROR("error when binding velodyne");
129 }
130}
131
132//////////////////////////////////////////////////////////////////////////
133/// Called by the ComponentManager to stop the component
134void VelodyneComponent::stopActivity()
135{
136 mRunning = false;
137 /*
138 exit();
139 if (!wait(2000)) {
140 // wait termination during 2 seconds
141 LOG_ERROR("thread blocking, terminate it");
142 terminate();
143 }
144 */
145
146 close();
147}
148
149//////////////////////////////////////////////////////////////////////////
150/// TODO: doc
151void VelodyneComponent::close()
152{
153 closeSocket();
154
155 if (isRecording()) {
156 mVelodyneSphericDataFile.close();
157 }
158
159 if (mShMem) {
160 delete mShMem;
161 mShMem = NULL;
162 }
163}
164
165//////////////////////////////////////////////////////////////////////////
166/// TODO: doc
167void VelodyneComponent::closeSocket()
168{
169 if (mSocket) {
170 mSocket->close();
171 delete mSocket;
172 mSocket = NULL;
173 }
174}
175
176//////////////////////////////////////////////////////////////////////////
177/// Called by the ComponentManager to pass the XML parameters to the
178/// component
179ComponentBase::COMPONENT_CONFIGURATION VelodyneComponent::configureComponent(XmlComponentConfig config)
180{
181 QString recordingParam = config.getProperty(kPropertyRecording.c_str());
182 if (!recordingParam.isNull()) {
183 setRecording( recordingParam.toInt() );
184 }
185 LOG_INFO("property " << kPropertyRecording << "=\"" << recordingParam << "\"");
186 /*
187 if (!param.getProperty("velodyneIP").isNull()) {
188 host_ = param.getProperty("velodyneIP");
189 }
190 if (!param.getProperty("velodynePort").isNull()) {
191 port_ = param.getProperty("velodynePort").toInt();
192 }
193 */
194 return ComponentBase::CONFIGURED_OK;
195}
196
197//////////////////////////////////////////////////////////////////////////
198/// new data coming from Velodyne sensor
199void VelodyneComponent::readPendingDatagrams()
200{
201 // get a timestamp
202 road_time_t t = road_time();
203 if (mRunning) {
204 while (mSocket->hasPendingDatagrams()) {
205 QByteArray datagram;
206 datagram.resize(mSocket->pendingDatagramSize());
207 mSocket->readDatagram(datagram.data(), datagram.size(), &mHost, &mPort);
208 processTheDatagram(t, datagram);
209 if (mEndOfScan) {
210 // we have a complete scan of 360° so we can expose it the application
211 exposeData();
212 if (isRecording()) {
213 record();
214 }
215 mEndOfScan = false;
216 }
217 }
218 }
219}
220
221//////////////////////////////////////////////////////////////////////////
222/// TODO: doc
223void VelodyneComponent::processTheDatagram(road_time_t time, QByteArray data)
224{
225 // envoi des paquets de 1206 octets 12 x 100 + 6
226 // 12 fois :
227 // 2 octets identifiant le bloc laser (haut ou bas)
228 // 2 octets 0 35999, /100 => angle en degré
229 // 96 octets : 32 laser beams, 2 octets distance 0.2 cm increment et 1 octet sur intensité
230 // 6 octets : 0xhhhhDegC : température ou version firmware Vxxx
231
232 const int packetSize = data.size();
233
234 // check the size of the packet
235 if (packetSize != VELODYNE_PACKET_SIZE) {
236 LOG_WARN("strange packet size:"
237 << " EXPECTED = " << VELODYNE_PACKET_SIZE
238 << " ACTUAL = " << packetSize
239 );
240 return;
241 }
242
243 // check angle to know if we have done a complete revolution
244 //LOG_TRACE("1st angle of packet = " << firstAngle);
245 LOG_TRACE(data.data());
246 //LOG_TRACE(data.mid(2,2).toHex());
247
248 unsigned short scanCount;
249 int angle;
250 if (!mStartOfScan) {
251 for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) {
252 {
253 bool ok = false;
254 // for each 100 data bytes, we extract the corresponding azimuth angle
255 angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16));
256 scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(&ok, 16));
257 LOG_TRACE("1:" << "start of scan = " << mStartOfScan << "\t"
258 << "angle = " << angle << "\t"
259 << "ok = " << ok << "\t"
260 << "size = " << packetSize
261 );
262 }
263 int delta = angle - mPreviousAngle;
264 LOG_TRACE("angle = " << angle);
265 LOG_TRACE("delta = " << delta);
266 LOG_TRACE("#scans = " << scanCount);
267
268 if (delta < 0) {
269 // we are looking for a new revolution
270 mPreviousAngle = angle;
271 mStartOfScan=true;
272 mVelodyneData->time = time;
273
274 int sizeToCopy = packetSize - i*VELODYNE_BLOCK_SIZE - 6;
275 memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid(i*VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy);
276
277/* samuel // Copy the time in each blocks.
278 for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - i; ++j)
279 mVelodyneData->dataTime[mBlockIndex + j] = time;
280//*/
281 mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET - i;
282 LOG_TRACE("block index = " << mBlockIndex);
283 LOG_TRACE("#scans = " << scanCount);
284 break;
285 } else {
286 mPreviousAngle = angle;
287 }
288 }
289 } else {
290 // start of scan
291 int lastBlockIndex = 0;
292 for (int i = 0; i < VELODYNE_NB_BLOCKS_PER_PACKET; ++i) {
293 {
294 bool ok = false;
295 angle = qFromBigEndian(data.mid(VELODYNE_BLOCK_SIZE*i+2, 2).toHex().toUShort(&ok, 16));
296 scanCount = qFromBigEndian(data.mid(1202, 2).toHex().toUShort(NULL, 16));
297 LOG_TRACE("2:" << "start of scan = " << mStartOfScan << "\t"
298 << "angle = " << angle << "\t"
299 << "ok = " << ok << "\t"
300 << "size = " << packetSize
301 );
302 }
303 int delta = angle - mPreviousAngle;
304 LOG_TRACE("angle = " << angle);
305 LOG_TRACE("delta = " << delta);
306 LOG_TRACE("#scans = " << scanCount);
307
308 if (delta < 0) {
309 // we are looking for a new revolution
310 LOG_TRACE("block index = " << mBlockIndex);
311 LOG_TRACE("#scans = " << scanCount);
312 //char c;
313 //cin >> c;
314 mEndOfScan = true;
315 // we add +1 because we detect the new revolution in the upper block and we have to copy the lower block too!
316 lastBlockIndex = i+1;
317 }
318 mPreviousAngle = angle;
319 }
320 if (!mEndOfScan) {
321 // we don't reach a complete revolution so only copy bytes in the current buffer
322 memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.data(), VELODYNE_PACKET_SIZE - 6);
323/*samuel // Copy the time in each blocks.
324 for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET; ++j)
325 mVelodyneData->dataTime[mBlockIndex + j] = time;
326 //*/
327 // Next offset.
328 mBlockIndex += VELODYNE_NB_BLOCKS_PER_PACKET;
329 } else {
330 // we have a complete revolution, we copy the starting data to the current buffer, then switch buffer
331 // and copy the rest of datagram in the new buffer.
332 if (lastBlockIndex > 1) {
333 int sizeToCopy = packetSize - (VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1)) * VELODYNE_BLOCK_SIZE - 6;
334 memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.left(sizeToCopy).data(), sizeToCopy);
335/* samuel // Copy the time in each blocks.
336 for (size_t j = 0; j < VELODYNE_NB_BLOCKS_PER_PACKET - (lastBlockIndex - 1); ++j)
337 mVelodyneData->dataTime[mBlockIndex + j] = time;
338 //*/
339 }
340
341 mVelodyneData->range = mBlockIndex+(lastBlockIndex - 1);
342 LOG_DEBUG("range = " << mVelodyneData->range);
343 mVelodyneData->timerange = time - mVelodyneData->time;
344 mBlockIndex = 0;
345 switchBuffer(); // switch the circular buffer of data. Previous one will be exported later.
346
347 // copy the rest of incoming data in the new buffer
348 mVelodyneData->time = time;
349 int sizeToCopy = packetSize - (lastBlockIndex - 1)*VELODYNE_BLOCK_SIZE - 6;
350 memcpy(&(mVelodyneData->polarData[mBlockIndex]), data.mid((lastBlockIndex-1) * VELODYNE_BLOCK_SIZE, sizeToCopy).data(), sizeToCopy);
351 /* samuel // Copy the time in each blocks.
352 for (size_t j = 0; j < lastBlockIndex - 1; ++j)
353 mVelodyneData->dataTime[mBlockIndex + j] = time;
354 //*/
355 mBlockIndex += sizeToCopy / VELODYNE_BLOCK_SIZE;
356 }
357 }
358}
359
360/// TODO: doc
361void VelodyneComponent::record()
362{
363 LOG_TRACE("record()");
364
365 //qDebug() << fullBuffer_ << velodyneData_<< &(velodyneDataBuffer_[0]) << &(velodyneDataBuffer_[1]);
366 /*LOG_DEBUG(mFullBuffer->time);
367 LOG_DEBUG(mFullBuffer->timerange);
368 LOG_DEBUG((intptr_t)mFullBuffer);
369 LOG_DEBUG((intptr_t)mVelodyneData);
370 LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[0]));
371 LOG_DEBUG((intptr_t) &(mVelodyneDataBuffer[1]));*/
372 if (isRecording()) {
373 size_t dataSize = sizeof(VelodynePolarData);
374 mVelodyneSphericDataFile.writeRecord(mFullBuffer->time, mFullBuffer->timerange, (const char *) mFullBuffer, dataSize);
375 }
376
377 LOG_INFO("record succeed");
378}
379
380/// TODO: doc
381void VelodyneComponent::switchBuffer()
382{
383 mFullBuffer = mVelodyneData;
384 /*
385 LOG_DEBUG("full buffer = " << mFullBuffer);
386 LOG_DEBUG("data = " << mVelodyneData);
387 LOG_DEBUG("current data = " << mVelodyneData);*/
388 mCurrentVelodyneData = (mCurrentVelodyneData+1)%2; // a hack to switch between 0 & 1
389 //LOG_DEBUG("current data = " << mVelodyneData);
390 // switch circular buffer
391 mVelodyneData = &(mVelodyneDataBuffer[mCurrentVelodyneData]);
392}
393
394/// TODO: doc
395void VelodyneComponent::exposeData()
396{
397 mShMem->write(mFullBuffer, sizeof(VelodynePolarData) );
398}
Note: See TracBrowser for help on using the repository browser.