source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlySickLMSManager.cpp@ 125

Last change on this file since 125 was 74, checked in by DHERBOMEZ Gérald, 10 years ago
  • Modification of Sick LMS sensor to get the RSSI signal (reflectivity of the laser)
  • Minor changes in data replay component of Sick LDMRS
  • CanGateway: Choose by default of Vector CAN XL card
File size: 8.7 KB
RevLine 
[43]1// *********************************************************************
2// created: 1014/03/13 - 16:49
3// filename: DbtPlySickLMSManager.cpp
4//
5// authors: Gerald Dherbomez, Cyril Fougeray
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id$
9//
10// purpose:
11// *********************************************************************
12
13#include "DbtPlySickLMSManager.h"
14
15#include <boost/assert.hpp>
16#include <iostream>
17#include <string>
18
[74]19#include "Pacpus/kernel/Log.h"
20#include "Pacpus/PacpusTools/ShMem.h"
[43]21
[74]22#include <Pacpus/kernel/InputOutputInterface.h>
[43]23
[74]24#include <math.h>
25
[43]26#define UTC_MAGIC_WORD 0x55544300
[74]27#define PI 3.1415936
[43]28
29namespace pacpus {
30
31using namespace std;
32
33DECLARE_STATIC_LOGGER("pacpus.base.DbtPlySickLMSManager");
34
35/// Construction de la fabrique de composant DbtPlySickLMSManager
36static ComponentFactory<DbtPlySickLMSManager> sFactory("DbtPlySickLMSManager");
37
38static const char * kSickMemoryName = "sickLMS";
39
40//////////////////////////////////////////////////////////////////////////
41/// Constructor.
42DbtPlySickLMSManager::DbtPlySickLMSManager(QString name)
43 : DbtPlyFileManager(name)
44{
45 LOG_TRACE("constructor(" << name << ")");
46
47 // mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanSickData));
48}
49
50//////////////////////////////////////////////////////////////////////////
51/// Destructor.
52DbtPlySickLMSManager::~DbtPlySickLMSManager()
53{
54 LOG_TRACE("destructor");
55 // delete mShMem;
56}
57
58//////////////////////////////////////////////////////////////////////////
59/// Configure the component.
60ComponentBase::COMPONENT_CONFIGURATION DbtPlySickLMSManager::configureComponent(XmlComponentConfig config)
61{
62 DbtPlyFileManager::configureComponent(config);
63
[74]64 //mDataFilename = param.getProperty("binFile");
65 mDataFilename = config.getProperty("binFile");
[43]66 return ComponentBase::CONFIGURED_OK;
67}
68
69//////////////////////////////////////////////////////////////////////////
70/// Starts the component.
71void DbtPlySickLMSManager::startActivity()
72{
73 // QString dataDir = mEngine->getDataDir();
74
75 // for (int i = 0; i < mDataFilenameList.size(); ++i) {
76 // QString file = mDataFilenameList.at(i);
77 // mDataFilenameList[i] = dataDir + file;
78
79 LOG_TRACE("DbtPlySickLMSManager component is starting.");
80
81 mDataFilename = mEngine->getDataDir() + mDataFilename;
82
83 LOG_TRACE("Opening "<< mDataFilename);
84
85 mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
86 if (!mDataFile) {
87 LOG_ERROR("cannot open file '" << mDataFilename << "'");
88 return;
89 }
90 // }
91 DbtPlyFileManager::startActivity();
92}
93
94//////////////////////////////////////////////////////////////////////////
95/// Stops the component.
96void DbtPlySickLMSManager::stopActivity()
97{
98 DbtPlyFileManager::stopActivity();
99 mDataFile.close();
100}
101
[74]102
[43]103//////////////////////////////////////////////////////////////////////////
104/// processData
105void DbtPlySickLMSManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
106{
107 if (!buffer) {
108 LOG_DEBUG("no data available: NULL buffer");
109 return;
110 }
111
112 LOG_TRACE("sizeof(SickLMS_dbt) = " << sizeof(SickLMS_dbt));
113 // BOOST_ASSERT(88 == sizeof(SickLMS_dbt));
114 SickLMS_dbt * sickLMS_dbt = static_cast<SickLMS_dbt *>(buffer);
115
116 // // copy the values contained in the dbt file
117 mSickDbt.scanNumber = sickLMS_dbt->scanNumber;
118 mSickDbt.scannerStatus = sickLMS_dbt->scannerStatus;
119 mSickDbt.scanFrequency = sickLMS_dbt->scanFrequency;
120 mSickDbt.angleResolution = sickLMS_dbt->angleResolution;
[74]121 mSickDbt.startAngle = sickLMS_dbt->startAngle;
122
123 std::cout << "start angle is " << mSickDbt.startAngle << std::endl;
124 std::cout << "angle step is " << mSickDbt.angleResolution << std::endl;
[43]125 mSickDbt.time = t;
126 mSickDbt.timerange = tr;
127
[74]128 /*int sizes[7];*/
129 int sizes[9];
[43]130 sizes[0] = mSickDbt.dist_len1 = sickLMS_dbt->dist_len1;
131 sizes[1] = mSickDbt.dist_len2 = sickLMS_dbt->dist_len2;
132 sizes[2] = mSickDbt.dist_len3 = sickLMS_dbt->dist_len3;
133 sizes[3] = mSickDbt.dist_len4 = sickLMS_dbt->dist_len4;
134 sizes[4] = mSickDbt.dist_len5 = sickLMS_dbt->dist_len5;
135 sizes[5] = mSickDbt.rssi_len1 = sickLMS_dbt->rssi_len1;
136 sizes[6] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2;
137
[74]138 //sizes[7] = mSickDbt.rssi_len1 = sickLMS_dbt->rssi_len1_8;
139 //sizes[8] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2_8;
140 //uint32_t pos[7];
141 uint32_t pos[9];
[43]142 pos[0] = mSickDbt.dataPos_dist1 = sickLMS_dbt->dataPos_dist1;
143 pos[1] = mSickDbt.dataPos_dist2 = sickLMS_dbt->dataPos_dist2;
144 pos[2] = mSickDbt.dataPos_dist3 = sickLMS_dbt->dataPos_dist3;
145 pos[3] = mSickDbt.dataPos_dist4 = sickLMS_dbt->dataPos_dist4;
146 pos[4] = mSickDbt.dataPos_dist5 = sickLMS_dbt->dataPos_dist5;
147 pos[5] = mSickDbt.dataPos_rssi1 = sickLMS_dbt->dataPos_rssi1;
148 pos[6] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2;
149
[74]150 //pos[7] = mSickDbt.dataPos_rssi1 = sickLMS_dbt->dataPos_rssi1_8;
151 //pos[8] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2_8;
152
[43]153 LOG_TRACE("(ScanPointsInfo) dist1 points\tSize : " << mSickDbt.dist_len1 <<"\tPos. : "<< mSickDbt.dataPos_dist1);
154 LOG_TRACE("(ScanPointsInfo) dist2 points\tSize : " << mSickDbt.dist_len2 <<"\tPos. : "<< mSickDbt.dataPos_dist2);
155 LOG_TRACE("(ScanPointsInfo) dist3 points\tSize : " << mSickDbt.dist_len3 <<"\tPos. : "<< mSickDbt.dataPos_dist3);
156 LOG_TRACE("(ScanPointsInfo) dist4 points\tSize : " << mSickDbt.dist_len4 <<"\tPos. : "<< mSickDbt.dataPos_dist4);
157 LOG_TRACE("(ScanPointsInfo) dist5 points\tSize : " << mSickDbt.dist_len5 <<"\tPos. : "<< mSickDbt.dataPos_dist5);
158 LOG_TRACE("(ScanPointsInfo) rssi1 points\tSize : " << mSickDbt.rssi_len1 <<"\tPos. : "<< mSickDbt.dataPos_rssi1);
159 LOG_TRACE("(ScanPointsInfo) rssi2 points\tSize : " << mSickDbt.rssi_len2 <<"\tPos. : "<< mSickDbt.dataPos_rssi2);
160
161
[74]162
[43]163 uint16_t* data[7];
[74]164 uint8_t* remission_8[2];
[43]165 int sumSizes = 0;
166
167 LOG_TRACE("Reading UTC file ... ");
[74]168 for(int i = 0; i<7; ++i)
169 {
[43]170
[74]171 if(sizes[i])
172 {
[43]173 mDataFile.seekg(pos[i]); // set the get pointer to the correct place
174 // // then copy the data contained in the binary file
175 data[i] = (uint16_t*) malloc(sizes[i]*sizeof(uint16_t));
[74]176 for (size_t j = 0; j < sizes[i]; ++j)
177 {
[43]178 mDataFile.read(reinterpret_cast<char*>(data[i]+j), sizeof(uint16_t));
[74]179 if(j%500==0)
180 {
[43]181 LOG_TRACE("Data : " << (uint16_t) *(data[i] + j));
182 }
[74]183 if(i == 0)
184 {
185
186 //std::cout << mSickLMSScan.point[j].distance << std::endl;
187 }
188 if(i == 5)
189 {
190
191 std::cout << *(data[i]+j) << std::endl;
192 }
[43]193 }
[74]194
195 }
196 sumSizes += sizes[i];
[43]197 }
[74]198
199 // for(int i = 7; i<9; ++i)
200 //{
[43]201
[74]202 // if(sizes[i])
203 // {
204 // mDataFile.seekg(pos[i]); // set the get pointer to the correct place
205 // // // then copy the data contained in the binary file
206 // remission_8[i] = (uint8_t*) malloc(sizes[i]*sizeof(uint8_t));
207 // for (size_t j = 0; j < sizes[i]; ++j)
208 // {
209 // mDataFile.read(reinterpret_cast<char*>(remission_8[i]+j), sizeof(uint8_t));
210 // if(j%500==0)
211 // {
212 // LOG_TRACE("Data : " << (uint16_t) *(remission_8[i] + j));
213 // }
214 // if(i == 0)
215 // {
[43]216
[74]217 // //std::cout << mSickLMSScan.point[j].distance << std::endl;
218 // }
219 // if(i == 7)
220 // {
221
222 // std::cout << "the 8 bit remission" << *(remission_8[i]+j) << std::endl;
223 // }
224 // }
225
226 // }
227 // sumSizes += sizes[i];
228 // }
229 //
230
[43]231 // verify that the last value is the UTC magic word
232 int32_t utcMagicWord = 0;
233 mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(int32_t));
234 if (UTC_MAGIC_WORD != utcMagicWord) {
235 LOG_WARN("corrupted data, do not use them!");
236 LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
237 } else {
238 LOG_TRACE("writing scan ");
239 LOG_WARN("NOT YET IMPLEMENTED");
240 // mShMem->write(&mSickDbt, sizeof(SickLMS_dbt));
241
242 /**********************************/
[74]243
244
[43]245 /**********************************/
246 }
247
248 if (mVerbose) {
249 cout << "[SICK LMS]:\t"
250 << "dataSize=" << sumSizes << "\t"
251 << "time=" << t << endl
252 ;
253 }
254 if (mVerbose >= 2) {
255 cout << "[SICK LMS]:\t"
256 << "startAngle=" << mSickDbt.startAngle << "\t"
257 << "angleResolution=" << mSickDbt.angleResolution << std::endl ;
258 }
259
260
[74]261 for(int i=0; i<7; ++i)
262 {
263 if(sizes[i])
264 {
265 free(data[i]);
266 }
[43]267 }
268}
269
270//////////////////////////////////////////////////////////////////////////
271/// Displays the graphical user interface (GUI)
272void DbtPlySickLMSManager::displayUI()
273{
274 LOG_WARN("GUI not implemented");
275
276 // TODO
277}
278
279} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.