| 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 |
|
|---|
