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

Last change on this file since 55 was 43, checked in by cfougera, 11 years ago

XML for sensors and players added. LMSxxx player ok. LDMRS still under development.

File size: 7.1 KB
Line 
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
19#include "kernel/Log.h"
20#include "PacpusTools/ShMem.h"
21
22
23#define UTC_MAGIC_WORD 0x55544300
24
25
26namespace pacpus {
27
28using namespace std;
29
30DECLARE_STATIC_LOGGER("pacpus.base.DbtPlySickLMSManager");
31
32/// Construction de la fabrique de composant DbtPlySickLMSManager
33static ComponentFactory<DbtPlySickLMSManager> sFactory("DbtPlySickLMSManager");
34
35static const char * kSickMemoryName = "sickLMS";
36
37//////////////////////////////////////////////////////////////////////////
38/// Constructor.
39DbtPlySickLMSManager::DbtPlySickLMSManager(QString name)
40 : DbtPlyFileManager(name)
41{
42 LOG_TRACE("constructor(" << name << ")");
43
44 // mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanSickData));
45}
46
47//////////////////////////////////////////////////////////////////////////
48/// Destructor.
49DbtPlySickLMSManager::~DbtPlySickLMSManager()
50{
51 LOG_TRACE("destructor");
52 // delete mShMem;
53}
54
55//////////////////////////////////////////////////////////////////////////
56/// Configure the component.
57ComponentBase::COMPONENT_CONFIGURATION DbtPlySickLMSManager::configureComponent(XmlComponentConfig config)
58{
59 DbtPlyFileManager::configureComponent(config);
60
61 mDataFilename = param.getProperty("binFile");
62
63 return ComponentBase::CONFIGURED_OK;
64}
65
66//////////////////////////////////////////////////////////////////////////
67/// Starts the component.
68void DbtPlySickLMSManager::startActivity()
69{
70 // QString dataDir = mEngine->getDataDir();
71
72 // for (int i = 0; i < mDataFilenameList.size(); ++i) {
73 // QString file = mDataFilenameList.at(i);
74 // mDataFilenameList[i] = dataDir + file;
75
76 LOG_TRACE("DbtPlySickLMSManager component is starting.");
77
78 mDataFilename = mEngine->getDataDir() + mDataFilename;
79
80 LOG_TRACE("Opening "<< mDataFilename);
81
82 mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
83 if (!mDataFile) {
84 LOG_ERROR("cannot open file '" << mDataFilename << "'");
85 return;
86 }
87 // }
88 DbtPlyFileManager::startActivity();
89}
90
91//////////////////////////////////////////////////////////////////////////
92/// Stops the component.
93void DbtPlySickLMSManager::stopActivity()
94{
95 DbtPlyFileManager::stopActivity();
96 mDataFile.close();
97}
98
99//////////////////////////////////////////////////////////////////////////
100/// processData
101void DbtPlySickLMSManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
102{
103 if (!buffer) {
104 LOG_DEBUG("no data available: NULL buffer");
105 return;
106 }
107
108 LOG_TRACE("sizeof(SickLMS_dbt) = " << sizeof(SickLMS_dbt));
109 // BOOST_ASSERT(88 == sizeof(SickLMS_dbt));
110 SickLMS_dbt * sickLMS_dbt = static_cast<SickLMS_dbt *>(buffer);
111
112 // // copy the values contained in the dbt file
113 mSickDbt.scanNumber = sickLMS_dbt->scanNumber;
114 mSickDbt.scannerStatus = sickLMS_dbt->scannerStatus;
115 mSickDbt.scanFrequency = sickLMS_dbt->scanFrequency;
116 mSickDbt.angleResolution = sickLMS_dbt->angleResolution;
117 mSickDbt.startAngle = sickLMS_dbt->startAngle;
118 mSickDbt.time = t;
119 mSickDbt.timerange = tr;
120
121
122 int sizes[7];
123 sizes[0] = mSickDbt.dist_len1 = sickLMS_dbt->dist_len1;
124 sizes[1] = mSickDbt.dist_len2 = sickLMS_dbt->dist_len2;
125 sizes[2] = mSickDbt.dist_len3 = sickLMS_dbt->dist_len3;
126 sizes[3] = mSickDbt.dist_len4 = sickLMS_dbt->dist_len4;
127 sizes[4] = mSickDbt.dist_len5 = sickLMS_dbt->dist_len5;
128 sizes[5] = mSickDbt.rssi_len1 = sickLMS_dbt->rssi_len1;
129 sizes[6] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2;
130
131 uint32_t pos[7];
132 pos[0] = mSickDbt.dataPos_dist1 = sickLMS_dbt->dataPos_dist1;
133 pos[1] = mSickDbt.dataPos_dist2 = sickLMS_dbt->dataPos_dist2;
134 pos[2] = mSickDbt.dataPos_dist3 = sickLMS_dbt->dataPos_dist3;
135 pos[3] = mSickDbt.dataPos_dist4 = sickLMS_dbt->dataPos_dist4;
136 pos[4] = mSickDbt.dataPos_dist5 = sickLMS_dbt->dataPos_dist5;
137 pos[5] = mSickDbt.dataPos_rssi1 = sickLMS_dbt->dataPos_rssi1;
138 pos[6] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2;
139
140 LOG_TRACE("(ScanPointsInfo) dist1 points\tSize : " << mSickDbt.dist_len1 <<"\tPos. : "<< mSickDbt.dataPos_dist1);
141 LOG_TRACE("(ScanPointsInfo) dist2 points\tSize : " << mSickDbt.dist_len2 <<"\tPos. : "<< mSickDbt.dataPos_dist2);
142 LOG_TRACE("(ScanPointsInfo) dist3 points\tSize : " << mSickDbt.dist_len3 <<"\tPos. : "<< mSickDbt.dataPos_dist3);
143 LOG_TRACE("(ScanPointsInfo) dist4 points\tSize : " << mSickDbt.dist_len4 <<"\tPos. : "<< mSickDbt.dataPos_dist4);
144 LOG_TRACE("(ScanPointsInfo) dist5 points\tSize : " << mSickDbt.dist_len5 <<"\tPos. : "<< mSickDbt.dataPos_dist5);
145 LOG_TRACE("(ScanPointsInfo) rssi1 points\tSize : " << mSickDbt.rssi_len1 <<"\tPos. : "<< mSickDbt.dataPos_rssi1);
146 LOG_TRACE("(ScanPointsInfo) rssi2 points\tSize : " << mSickDbt.rssi_len2 <<"\tPos. : "<< mSickDbt.dataPos_rssi2);
147
148
149 uint16_t* data[7];
150
151 int sumSizes = 0;
152
153 LOG_TRACE("Reading UTC file ... ");
154 for(int i = 0; i<7; ++i){
155
156 if(sizes[i]){
157 mDataFile.seekg(pos[i]); // set the get pointer to the correct place
158 // // then copy the data contained in the binary file
159 data[i] = (uint16_t*) malloc(sizes[i]*sizeof(uint16_t));
160 for (size_t j = 0; j < sizes[i]; ++j) {
161 mDataFile.read(reinterpret_cast<char*>(data[i]+j), sizeof(uint16_t));
162 if(j%500==0){
163 LOG_TRACE("Data : " << (uint16_t) *(data[i] + j));
164 }
165 }
166 }
167 sumSizes += sizes[i];
168 }
169
170
171 // verify that the last value is the UTC magic word
172 int32_t utcMagicWord = 0;
173 mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(int32_t));
174 if (UTC_MAGIC_WORD != utcMagicWord) {
175 LOG_WARN("corrupted data, do not use them!");
176 LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
177 } else {
178 LOG_TRACE("writing scan ");
179 LOG_WARN("NOT YET IMPLEMENTED");
180 // mShMem->write(&mSickDbt, sizeof(SickLMS_dbt));
181
182 /**********************************/
183 /* TODO : Send data ! */
184 /**********************************/
185 }
186
187 if (mVerbose) {
188 cout << "[SICK LMS]:\t"
189 << "dataSize=" << sumSizes << "\t"
190 << "time=" << t << endl
191 ;
192 }
193 if (mVerbose >= 2) {
194 cout << "[SICK LMS]:\t"
195 << "startAngle=" << mSickDbt.startAngle << "\t"
196 << "angleResolution=" << mSickDbt.angleResolution << std::endl ;
197 }
198
199
200 for(int i=0; i<7; ++i){
201 free(data[i]);
202 }
203}
204
205//////////////////////////////////////////////////////////////////////////
206/// Displays the graphical user interface (GUI)
207void DbtPlySickLMSManager::displayUI()
208{
209 LOG_WARN("GUI not implemented");
210
211 // TODO
212}
213
214} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.