source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp@ 123

Last change on this file since 123 was 104, checked in by xuphilip, 9 years ago

Updated I/O for Sick LDMRS

File size: 5.9 KB
Line 
1// *********************************************************************
2// created: 1014/03/27 - 11:37
3// filename: DbtPlySickLDMRSManager.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 "DbtPlySickLDMRSManager.h"
14
15#include <boost/assert.hpp>
16#include <iostream>
17#include <string>
18
19#include "Pacpus/kernel/Log.h"
20#include "Pacpus/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.DbtPlySickLDMRSManager");
31
32/// Construction de la fabrique de composant DbtPlySickLDMRSManager
33static ComponentFactory<DbtPlySickLDMRSManager> sFactory("DbtPlySickLDMRSManager");
34
35static const char * kSickMemoryName = "sickLDMRS";
36
37/************************************************************************
38 * Constructor
39 ************************************************************************/
40DbtPlySickLDMRSManager::DbtPlySickLDMRSManager(QString name)
41 : DbtPlyFileManager(name)
42{
43 LOG_TRACE("constructor(" << name << ")");
44}
45
46/************************************************************************
47 * Destructor
48 ************************************************************************/
49DbtPlySickLDMRSManager::~DbtPlySickLDMRSManager()
50{
51 LOG_TRACE("destructor");
52}
53
54/************************************************************************
55 * Configuration of the component, called by the ComponentManager after
56 * the construction of the object
57 ************************************************************************/
58ComponentBase::COMPONENT_CONFIGURATION DbtPlySickLDMRSManager::configureComponent(XmlComponentConfig config)
59{
60 DbtPlyFileManager::configureComponent(config);
61
62 mDataFilename = config.getProperty("binFile");
63
64 return ComponentBase::CONFIGURED_OK;
65}
66
67/************************************************************************
68 * Start function, called by the ComponentManager when a start()
69 * command is received
70 ************************************************************************/
71void DbtPlySickLDMRSManager::startActivity()
72{
73 LOG_TRACE("DbtPlySickLDMRSManager component is starting.");
74
75 mDataFilename = mEngine->getDataDir() + mDataFilename;
76
77 LOG_TRACE("Opening "<< mDataFilename);
78
79 mDataFile.open(mDataFilename.toLatin1().data(),std::ios_base::in|std::ios_base::binary);
80 if (!mDataFile) {
81 LOG_ERROR("cannot open file '" << mDataFilename << "'");
82 return;
83 }
84
85 outScan = getTypedOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");
86
87 DbtPlyFileManager::startActivity();
88}
89
90/************************************************************************
91 * Stop function, called by the ComponentManager when a stop()
92 * command is received
93 ************************************************************************/
94void DbtPlySickLDMRSManager::stopActivity()
95{
96 DbtPlyFileManager::stopActivity();
97 mDataFile.close();
98}
99
100
101/************************************************************************
102 * Called by the framework at initialization
103 ************************************************************************/
104void DbtPlySickLDMRSManager::addInputs()
105{
106 // uncomment to add an input
107}
108
109
110/************************************************************************
111 * Called by the framework at initialization
112 ************************************************************************/
113void DbtPlySickLDMRSManager::addOutputs()
114{
115 // empty: no output
116 addOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");
117}
118
119
120/************************************************************************
121 * Process LiDAR data
122 ************************************************************************/
123void DbtPlySickLDMRSManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
124{
125 if (!buffer) {
126 LOG_DEBUG("no data available: NULL buffer");
127 return;
128 }
129
130 LOG_TRACE("sizeof(sickLDMRS_dbt) = " << sizeof(SickLDMRS_dbt));
131 // BOOST_ASSERT(88 == sizeof(SickLMS_dbt));
132 SickLDMRS_dbt * sickLDMRS_dbt = static_cast<SickLDMRS_dbt *>(buffer);
133
134 // // copy the values contained in the dbt file
135 mScan.timeStartFromSensor = sickLDMRS_dbt->timeStartFromSensor;
136 mScan.header = sickLDMRS_dbt->hScan;
137 mScan.time = t;
138 mScan.timerange = tr;
139 mScan.points = QVector<ScanPoint>(mScan.header.numPoints);
140
141 LOG_TRACE("Number of points " << mScan.header.numPoints);
142
143 LOG_TRACE("Reading UTC file ... ");
144
145 mDataFile.seekg(sickLDMRS_dbt->dataPos); // set the get pointer to the correct place
146
147 // then copy the data contained in the binary file
148 for (unsigned int i = 0 ; i < mScan.header.numPoints ; ++i) {
149 mDataFile.read(reinterpret_cast<char *>(&(mScan.points[i])), sizeof(ScanPoint));
150 }
151
152 // verify that the last value is the UTC magic word
153 int32_t utcMagicWord = 0;
154 mDataFile.read(reinterpret_cast<char *>(&(utcMagicWord)), sizeof(int32_t));
155 if (UTC_MAGIC_WORD != utcMagicWord) {
156 LOG_WARN("corrupted data, do not use them!");
157 LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
158 } else {
159 LOG_TRACE("sending scan ");
160 checkedSend(outScan, mScan);
161 }
162
163 if (mVerbose) {
164 cout << "[SICK LDMRS]:\t"
165 << "numPoints=" << mScan.header.numPoints << "\t"
166 << "time=" << t << endl
167 ;
168 }
169 if (mVerbose >= 2) {
170 cout << "[SICK LDMRS]:\t"
171 << "startAngle=" << mScan.header.startAngle << "\t"
172 << "endAngle=" << mScan.header.endAngle << std::endl ;
173 }
174}
175
176//////////////////////////////////////////////////////////////////////////
177/// Displays the graphical user interface (GUI)
178void DbtPlySickLDMRSManager::displayUI()
179{
180 LOG_WARN("GUI not implemented");
181
182 // TODO
183}
184
185} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.