1 | /*********************************************************************
|
---|
2 | // created: 2007/11/13 - 16:49
|
---|
3 | // filename: DbtPlyCPTComponent.cpp
|
---|
4 | //
|
---|
5 | // author: Sergio Rodriguez
|
---|
6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
7 | //
|
---|
8 | // version: $Id: DbtPlyCPTComponent.cpp 863 2011-07-06 15:10:17Z srodrigu $
|
---|
9 | //
|
---|
10 | // purpose:
|
---|
11 | *********************************************************************/
|
---|
12 |
|
---|
13 | #include "DbtPlyCPTComponent.h"
|
---|
14 |
|
---|
15 | #include "kernel/DbiteFileTypes.h"
|
---|
16 | #include "kernel/Log.h"
|
---|
17 | #include "PacpusTools/ShMem.h"
|
---|
18 | #include "kernel/InputOutputInterface.h"
|
---|
19 |
|
---|
20 | #include <iostream>
|
---|
21 | #include <QMetaType>
|
---|
22 |
|
---|
23 | #define _USE_MATH_DEFINES
|
---|
24 | #include <math.h>
|
---|
25 |
|
---|
26 | using namespace pacpus;
|
---|
27 | using namespace std;
|
---|
28 |
|
---|
29 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyCPTComponent");
|
---|
30 |
|
---|
31 | static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
|
---|
32 | + sizeof(TimestampedRawimusaFrame)
|
---|
33 | + sizeof(TimestampedInspvaaFrame)
|
---|
34 | + sizeof(TimestampedInscovFrame)
|
---|
35 | ;
|
---|
36 |
|
---|
37 | //////////////////////////////////////////////////////////////////////////
|
---|
38 | /// Construction de la fabrique de composant DbtPlySerialS8Logger
|
---|
39 | static ComponentFactory<DbtPlyCPTComponent> sFactory("DbtPlyCPTComponent");
|
---|
40 |
|
---|
41 | //////////////////////////////////////////////////////////////////////////
|
---|
42 | /// Constructor
|
---|
43 | DbtPlyCPTComponent::DbtPlyCPTComponent(QString name)
|
---|
44 | : DbtPlyFileManager(name)
|
---|
45 | {
|
---|
46 | LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
|
---|
47 | LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
|
---|
48 | LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
|
---|
49 | LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
|
---|
50 | LOG_INFO("sizeof(trame_bestgpsposa)=" << sizeof(trame_bestgpsposa));
|
---|
51 | LOG_INFO("sizeof(trame_rawimusa)=" << sizeof(trame_rawimusa));
|
---|
52 | LOG_INFO("sizeof(trame_inspvaa)=" << sizeof(trame_inspvaa));
|
---|
53 | LOG_INFO("sizeof(trame_inscov)=" << sizeof(trame_inscov));
|
---|
54 |
|
---|
55 | mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
|
---|
56 | allFramesBuffer = new char[AllFramesSize];
|
---|
57 | }
|
---|
58 |
|
---|
59 | //////////////////////////////////////////////////////////////////////////
|
---|
60 | /// Destructor
|
---|
61 | DbtPlyCPTComponent::~DbtPlyCPTComponent()
|
---|
62 | {
|
---|
63 | delete[] allFramesBuffer;
|
---|
64 | delete mShMem;
|
---|
65 | }
|
---|
66 |
|
---|
67 | //////////////////////////////////////////////////////////////////////////
|
---|
68 | /// Configure the component
|
---|
69 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyCPTComponent::configureComponent(XmlComponentConfig config)
|
---|
70 | {
|
---|
71 | return DbtPlyFileManager::configureComponent(config);
|
---|
72 | }
|
---|
73 |
|
---|
74 | void DbtPlyCPTComponent::addInputs()
|
---|
75 | {
|
---|
76 | // empty: no inputs
|
---|
77 | }
|
---|
78 |
|
---|
79 | void DbtPlyCPTComponent::addOutputs()
|
---|
80 | {
|
---|
81 | addOutput<TimestampedBestgpsposaFrame, DbtPlyCPTComponent>("bestposegpsposa");
|
---|
82 | addOutput<TimestampedRawimusaFrame, DbtPlyCPTComponent>("rawimusa");
|
---|
83 | addOutput<TimestampedInspvaaFrame, DbtPlyCPTComponent>("inspvaaa");
|
---|
84 | addOutput<TimestampedInscovFrame, DbtPlyCPTComponent>("inscov");
|
---|
85 | addOutput<Pose3D, DbtPlyCPTComponent>("genericpose");
|
---|
86 | }
|
---|
87 |
|
---|
88 | //////////////////////////////////////////////////////////////////////////
|
---|
89 | /// Start the component
|
---|
90 | void DbtPlyCPTComponent::startActivity()
|
---|
91 | {
|
---|
92 | DbtPlyFileManager::startActivity();
|
---|
93 |
|
---|
94 | mDataFilename = mEngine->getDataDir() + mDataFilename;
|
---|
95 |
|
---|
96 | // user interface
|
---|
97 | //if (mShowGui) {
|
---|
98 | displayUI();
|
---|
99 | //}
|
---|
100 | }
|
---|
101 |
|
---|
102 | //////////////////////////////////////////////////////////////////////////
|
---|
103 | /// Stops the component
|
---|
104 | void DbtPlyCPTComponent::stopActivity()
|
---|
105 | {
|
---|
106 | /* if (mShowGui) {
|
---|
107 | mPoseViewer->close();
|
---|
108 | mPoseViewer.reset();
|
---|
109 | }*/
|
---|
110 |
|
---|
111 | DbtPlyFileManager::stopActivity();
|
---|
112 | }
|
---|
113 |
|
---|
114 | //////////////////////////////////////////////////////////////////////////
|
---|
115 | /// processData
|
---|
116 | void DbtPlyCPTComponent::processData(road_time_t t, road_timerange_t tr , void * buf)
|
---|
117 | {
|
---|
118 | if (buf) {
|
---|
119 | // look at the dbt index in file manager and get the identifier of dbt structure
|
---|
120 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType();
|
---|
121 | LOG_TRACE("DBT structure id = " << id);
|
---|
122 |
|
---|
123 | switch (id) {
|
---|
124 | case TRAME_BESTGPSPOSA:
|
---|
125 | memcpy(&(bestgpsposaFrame_.frame), buf, sizeof(trame_bestgpsposa));
|
---|
126 | bestgpsposaFrame_.time = t;
|
---|
127 | bestgpsposaFrame_.timerange = tr;
|
---|
128 |
|
---|
129 | LOG_TRACE("[BESTGPSPOSE] frame replayed at " << bestgpsposaFrame_.time);
|
---|
130 | LOG_DEBUG("[BESTGPSPOSE]" << "\t"
|
---|
131 | << "STATUS=" << ((bestgpsposaFrame_.frame.Status==SOL_COMPUTED) ? "SOL_COMPUTED" : "NO SOLUTION AVAILABLE"));
|
---|
132 | LOG_DEBUG("[BESTGPSPOSE]" << "\t"
|
---|
133 | << "Lat=" << bestgpsposaFrame_.frame.Lat << "\t"
|
---|
134 | << "Lon=" << bestgpsposaFrame_.frame.Lon << "\t"
|
---|
135 | << "Alt=" << bestgpsposaFrame_.frame.Hgt);
|
---|
136 |
|
---|
137 | memcpy(allFramesBuffer, &bestgpsposaFrame_, sizeof(TimestampedBestgpsposaFrame));
|
---|
138 | //ShMem refresh
|
---|
139 | mShMem->write(allFramesBuffer, AllFramesSize);
|
---|
140 |
|
---|
141 | {
|
---|
142 | OutputInterface<TimestampedBestgpsposaFrame, DbtPlyCPTComponent> * bestposegpsposaOutput = getTypedOutput<TimestampedBestgpsposaFrame, DbtPlyCPTComponent>("bestposegpsposa");
|
---|
143 | if (bestposegpsposaOutput && bestposegpsposaOutput->hasConnection()) {
|
---|
144 | bestposegpsposaOutput->send(bestgpsposaFrame_);
|
---|
145 | }
|
---|
146 | }
|
---|
147 | break;
|
---|
148 |
|
---|
149 | case TRAME_RAWIMUSA:
|
---|
150 | memcpy(&(rawimuFrame_.frame), buf, sizeof(trame_rawimusa));
|
---|
151 | rawimuFrame_.time = t;
|
---|
152 | rawimuFrame_.timerange = tr;
|
---|
153 | LOG_TRACE("[RAWIMUSA] frame replayed at " << rawimuFrame_.time);
|
---|
154 |
|
---|
155 | memcpy(allFramesBuffer
|
---|
156 | + sizeof(TimestampedBestgpsposaFrame),
|
---|
157 | &rawimuFrame_, sizeof(TimestampedRawimusaFrame));
|
---|
158 | mShMem->write(allFramesBuffer, AllFramesSize);
|
---|
159 |
|
---|
160 | {
|
---|
161 | OutputInterface<TimestampedRawimusaFrame, DbtPlyCPTComponent> * rawimusaOutput = getTypedOutput<TimestampedRawimusaFrame, DbtPlyCPTComponent>("rawimusa");
|
---|
162 | if (rawimusaOutput && rawimusaOutput->hasConnection()) {
|
---|
163 | rawimusaOutput->send(rawimuFrame_);
|
---|
164 | }
|
---|
165 | }
|
---|
166 |
|
---|
167 | break;
|
---|
168 |
|
---|
169 | case TRAME_INSPVAA:
|
---|
170 | memcpy(&(inspvaFrame_.frame), buf, sizeof(trame_inspvaa));
|
---|
171 | inspvaFrame_.time = t;
|
---|
172 | inspvaFrame_.timerange = tr;
|
---|
173 | LOG_TRACE("[INSPVAA] frame replayed at " << inspvaFrame_.time);
|
---|
174 |
|
---|
175 | LOG_DEBUG("[INSPVAA]" << "\t"
|
---|
176 | << "STATUS=" << ((inspvaFrame_.frame.Status == INS_SOLUTION_GOOD) ? "INS_SOLUTION_GOOD" : "INS Unreliable"));
|
---|
177 | LOG_DEBUG("[INSPVAA]" << "\t"
|
---|
178 | << "Lat=" << inspvaFrame_.frame.Lat << "\t"
|
---|
179 | << "Lon=" << inspvaFrame_.frame.Lon << "\t"
|
---|
180 | << "Alt=" << inspvaFrame_.frame.Hgt << "\t"
|
---|
181 | << "E=" << inspvaFrame_.frame.e << "\t"
|
---|
182 | << "N=" << inspvaFrame_.frame.n << "\t"
|
---|
183 | << "U=" << inspvaFrame_.frame.u << "\t"
|
---|
184 | << "Azimu=" << inspvaFrame_.frame.Azimuth << "\t"
|
---|
185 | << "Roll =" << inspvaFrame_.frame.Roll << "\t"
|
---|
186 | << "Pitch=" << inspvaFrame_.frame.Pitch);
|
---|
187 |
|
---|
188 | memcpy(allFramesBuffer
|
---|
189 | + sizeof(TimestampedBestgpsposaFrame) +sizeof(TimestampedRawimusaFrame),
|
---|
190 | &inspvaFrame_, sizeof(TimestampedInspvaaFrame));
|
---|
191 | mShMem->write(allFramesBuffer, AllFramesSize);
|
---|
192 |
|
---|
193 | {
|
---|
194 | OutputInterface<TimestampedInspvaaFrame, DbtPlyCPTComponent> * inspvaaaOutput = getTypedOutput<TimestampedInspvaaFrame, DbtPlyCPTComponent>("inspvaaa");
|
---|
195 | if (inspvaaaOutput && inspvaaaOutput->hasConnection()) {
|
---|
196 | inspvaaaOutput->send(inspvaFrame_);
|
---|
197 | }
|
---|
198 | }
|
---|
199 |
|
---|
200 | genericEnuPose_.time = t;
|
---|
201 | genericEnuPose_.timerange = tr;
|
---|
202 | genericEnuPose_.position = QVector3D(inspvaFrame_.frame.e, inspvaFrame_.frame.n, inspvaFrame_.frame.u);
|
---|
203 | // FIXME: use Geodesie: DegToRad / RadToDeg
|
---|
204 | genericEnuPose_.angle = QVector3D((inspvaFrame_.frame.Azimuth+90.0)*(M_PI/180.0),inspvaFrame_.frame.Pitch*(M_PI/180.0),inspvaFrame_.frame.Roll*(M_PI/180.0));
|
---|
205 |
|
---|
206 | // FIXME: it was: send(genericEnuPose_, road_time(), 100000) -- was it OK?
|
---|
207 | {
|
---|
208 | OutputInterface<Pose3D, DbtPlyCPTComponent> * poseOutput = getTypedOutput<Pose3D, DbtPlyCPTComponent>("genericpose");
|
---|
209 | if (poseOutput && poseOutput->hasConnection()) {
|
---|
210 | poseOutput->send(genericEnuPose_, genericEnuPose_.time, genericEnuPose_.timerange);
|
---|
211 | }
|
---|
212 | }
|
---|
213 | // for (int i=0;i<3;i++) {
|
---|
214 | // for(int j=0;j<3;j++) {
|
---|
215 | // pose.posconv[i][j] = d.frame.PosCov[i][j];
|
---|
216 | // pose.angcov[i][j] = d.frame.AttCov[i][j];
|
---|
217 | // }
|
---|
218 | // }
|
---|
219 |
|
---|
220 | break;
|
---|
221 |
|
---|
222 | case TRAME_INSCOV:
|
---|
223 | memcpy(&(inscovFrame_.frame), buf, sizeof(trame_inscov));
|
---|
224 | inscovFrame_.time = t;
|
---|
225 | inscovFrame_.timerange = tr;
|
---|
226 | LOG_TRACE("[INSCOV] frame replayed at " << inscovFrame_.time);
|
---|
227 |
|
---|
228 | LOG_DEBUG("[INSCOV]" << "\t"
|
---|
229 | << "Week=" << inscovFrame_.frame.Week << "\t"
|
---|
230 | << "Secs=" << inscovFrame_.frame.Seconds << "\t"
|
---|
231 | << "AttCov=" << inscovFrame_.frame.AttCov[0][0] << "\t"
|
---|
232 | << "PosCov=" << inscovFrame_.frame.PosCov[0][0] << "\t"
|
---|
233 | << "VelCov=" << inscovFrame_.frame.VelCov[0][0]);
|
---|
234 |
|
---|
235 | memcpy(allFramesBuffer
|
---|
236 | + sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame) + sizeof(TimestampedInspvaaFrame),
|
---|
237 | &inscovFrame_, sizeof(TimestampedInscovFrame));
|
---|
238 | mShMem->write(allFramesBuffer, AllFramesSize);
|
---|
239 |
|
---|
240 | {
|
---|
241 | OutputInterface<TimestampedInscovFrame, DbtPlyCPTComponent> * inscovOutput = getTypedOutput<TimestampedInscovFrame, DbtPlyCPTComponent>("inscov");
|
---|
242 | if (inscovOutput && inscovOutput->hasConnection()) {
|
---|
243 | inscovOutput->send(inscovFrame_);
|
---|
244 | }
|
---|
245 | }
|
---|
246 | break;
|
---|
247 |
|
---|
248 | default:
|
---|
249 | LOG_WARN("unknown frame type: " << id);
|
---|
250 | }
|
---|
251 |
|
---|
252 | }
|
---|
253 | }
|
---|
254 |
|
---|
255 | //////////////////////////////////////////////////////////////////////////
|
---|
256 | /// Display GUI
|
---|
257 | void DbtPlyCPTComponent::displayUI()
|
---|
258 | {
|
---|
259 |
|
---|
260 | }
|
---|
261 |
|
---|