source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyCPTComponent.cpp@ 132

Last change on this file since 132 was 111, checked in by DHERBOMEZ Gérald, 9 years ago

Delete deprecated call to shared memories. Use I/O mechanism instead.
Adjustement of dllimport and dllexport macros;

  • Property svn:executable set to *
File size: 10.6 KB
RevLine 
[108]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
26using namespace pacpus;
27using namespace std;
28
29DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyCPTComponent");
30
31static 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
39static ComponentFactory<DbtPlyCPTComponent> sFactory("DbtPlyCPTComponent");
40
41//////////////////////////////////////////////////////////////////////////
42/// Constructor
43DbtPlyCPTComponent::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
[111]55 // mShMem = new ShMem("SPAN_FRAMES", AllFramesSize); // deprecated
[108]56 allFramesBuffer = new char[AllFramesSize];
57}
58
59//////////////////////////////////////////////////////////////////////////
60/// Destructor
61DbtPlyCPTComponent::~DbtPlyCPTComponent()
62{
63 delete[] allFramesBuffer;
[111]64 // delete mShMem; // deprecated
[108]65}
66
67//////////////////////////////////////////////////////////////////////////
68/// Configure the component
69ComponentBase::COMPONENT_CONFIGURATION DbtPlyCPTComponent::configureComponent(XmlComponentConfig config)
70{
71 return DbtPlyFileManager::configureComponent(config);
72}
73
74void DbtPlyCPTComponent::addInputs()
75{
76 // empty: no inputs
77}
78
79void 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
90void 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
104void DbtPlyCPTComponent::stopActivity()
105{
106 /* if (mShowGui) {
107 mPoseViewer->close();
108 mPoseViewer.reset();
109 }*/
110
111 DbtPlyFileManager::stopActivity();
112}
113
114//////////////////////////////////////////////////////////////////////////
115/// processData
116void 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
[111]139 // mShMem->write(allFramesBuffer, AllFramesSize); // deprecated
[108]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));
[111]158 // mShMem->write(allFramesBuffer, AllFramesSize); // deprecated
[108]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));
[111]191 // mShMem->write(allFramesBuffer, AllFramesSize); // deprecated
[108]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));
[111]238 // mShMem->write(allFramesBuffer, AllFramesSize); // deprecated
[108]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
257void DbtPlyCPTComponent::displayUI()
258{
259
260}
261
Note: See TracBrowser for help on using the repository browser.