| 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 "../PoseViewer/PoseViewer.h"
|
|---|
| 16 |
|
|---|
| 17 | #include "kernel/DbiteFileTypes.h"
|
|---|
| 18 | #include "kernel/Log.h"
|
|---|
| 19 | #include "PacpusTools/ShMem.h"
|
|---|
| 20 | #include "Pacpus/kernel/inputOutputInterface.h"
|
|---|
| 21 |
|
|---|
| 22 | #include <iostream>
|
|---|
| 23 | #include <QMetaType>
|
|---|
| 24 |
|
|---|
| 25 | using namespace pacpus;
|
|---|
| 26 | using namespace std;
|
|---|
| 27 |
|
|---|
| 28 | DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyCPTComponent");
|
|---|
| 29 |
|
|---|
| 30 | static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
|
|---|
| 31 | + sizeof(TimestampedRawimusaFrame)
|
|---|
| 32 | + sizeof(TimestampedInspvaaFrame)
|
|---|
| 33 | + sizeof(TimestampedInscovFrame)
|
|---|
| 34 | ;
|
|---|
| 35 |
|
|---|
| 36 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 37 | /// Construction de la fabrique de composant DbtPlySerialS8Logger
|
|---|
| 38 | static ComponentFactory<DbtPlyCPTComponent> sFactory("DbtPlyCPTComponent");
|
|---|
| 39 |
|
|---|
| 40 | void DbtPlyCPTComponent::addInputOutput()
|
|---|
| 41 | {
|
|---|
| 42 | ADD_OUTPUT("bestposegpsposa",DbtPlyCPTComponent,TimestampedBestgpsposaFrame);
|
|---|
| 43 | ADD_OUTPUT("rawimusa",DbtPlyCPTComponent,TimestampedRawimusaFrame);
|
|---|
| 44 | ADD_OUTPUT("inspvaaa",DbtPlyCPTComponent,TimestampedInspvaaFrame);
|
|---|
| 45 | ADD_OUTPUT("inscov",DbtPlyCPTComponent,TimestampedInscovFrame);
|
|---|
| 46 | ADD_OUTPUT("genericpose",DbtPlyCPTComponent,Pose3D);
|
|---|
| 47 | }
|
|---|
| 48 |
|
|---|
| 49 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 50 | /// Constructor
|
|---|
| 51 | DbtPlyCPTComponent::DbtPlyCPTComponent(QString name)
|
|---|
| 52 | : DbtPlyFileManager(name)
|
|---|
| 53 | {
|
|---|
| 54 | LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
|
|---|
| 55 | LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
|
|---|
| 56 | LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
|
|---|
| 57 | LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
|
|---|
| 58 | LOG_INFO("sizeof(trame_bestgpsposa)=" << sizeof(trame_bestgpsposa));
|
|---|
| 59 | LOG_INFO("sizeof(trame_rawimusa)=" << sizeof(trame_rawimusa));
|
|---|
| 60 | LOG_INFO("sizeof(trame_inspvaa)=" << sizeof(trame_inspvaa));
|
|---|
| 61 | LOG_INFO("sizeof(trame_inscov)=" << sizeof(trame_inscov));
|
|---|
| 62 |
|
|---|
| 63 | mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
|
|---|
| 64 | allFramesBuffer = new char[AllFramesSize];
|
|---|
| 65 | addInputOutput();
|
|---|
| 66 | }
|
|---|
| 67 |
|
|---|
| 68 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 69 | /// Destructor
|
|---|
| 70 | DbtPlyCPTComponent::~DbtPlyCPTComponent()
|
|---|
| 71 | {
|
|---|
| 72 | delete[] allFramesBuffer;
|
|---|
| 73 | delete mShMem;
|
|---|
| 74 | }
|
|---|
| 75 |
|
|---|
| 76 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 77 | /// Configure the component
|
|---|
| 78 | ComponentBase::COMPONENT_CONFIGURATION DbtPlyCPTComponent::configureComponent(XmlComponentConfig config)
|
|---|
| 79 | {
|
|---|
| 80 | return DbtPlyFileManager::configureComponent(config);
|
|---|
| 81 | }
|
|---|
| 82 |
|
|---|
| 83 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 84 | /// Start the component
|
|---|
| 85 | void DbtPlyCPTComponent::startActivity()
|
|---|
| 86 | {
|
|---|
| 87 | DbtPlyFileManager::startActivity();
|
|---|
| 88 |
|
|---|
| 89 | mDataFilename = mEngine->getDataDir() + mDataFilename;
|
|---|
| 90 |
|
|---|
| 91 | // user interface
|
|---|
| 92 | if (mShowGui) {
|
|---|
| 93 | displayUI();
|
|---|
| 94 | }
|
|---|
| 95 | }
|
|---|
| 96 |
|
|---|
| 97 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 98 | /// Stops the component
|
|---|
| 99 | void DbtPlyCPTComponent::stopActivity()
|
|---|
| 100 | {
|
|---|
| 101 | /* if (mShowGui) {
|
|---|
| 102 | mPoseViewer->close();
|
|---|
| 103 | mPoseViewer.reset();
|
|---|
| 104 | }*/
|
|---|
| 105 |
|
|---|
| 106 | DbtPlyFileManager::stopActivity();
|
|---|
| 107 | }
|
|---|
| 108 |
|
|---|
| 109 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 110 | /// processData
|
|---|
| 111 | void DbtPlyCPTComponent::processData(road_time_t t, road_timerange_t tr , void * buf)
|
|---|
| 112 | {
|
|---|
| 113 | if (buf) {
|
|---|
| 114 | // look at the dbt index in file manager and get the identifier of dbt structure
|
|---|
| 115 | hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType();
|
|---|
| 116 | LOG_TRACE("DBT structure id = " << id);
|
|---|
| 117 |
|
|---|
| 118 | switch (id) {
|
|---|
| 119 | case TRAME_BESTGPSPOSA:
|
|---|
| 120 | memcpy(&(bestgpsposaFrame_.frame), buf, sizeof(trame_bestgpsposa));
|
|---|
| 121 | bestgpsposaFrame_.time = t;
|
|---|
| 122 | bestgpsposaFrame_.timerange = tr;
|
|---|
| 123 |
|
|---|
| 124 | LOG_TRACE("[BESTGPSPOSE] frame replayed at " << bestgpsposaFrame_.time);
|
|---|
| 125 | LOG_DEBUG("[BESTGPSPOSE]" << "\t"
|
|---|
| 126 | << "STATUS=" << ((bestgpsposaFrame_.frame.Status==SOL_COMPUTED) ? "SOL_COMPUTED" : "NO SOLUTION AVAILABLE"));
|
|---|
| 127 | LOG_DEBUG("[BESTGPSPOSE]" << "\t"
|
|---|
| 128 | << "Lat=" << bestgpsposaFrame_.frame.Lat << "\t"
|
|---|
| 129 | << "Lon=" << bestgpsposaFrame_.frame.Lon << "\t"
|
|---|
| 130 | << "Alt=" << bestgpsposaFrame_.frame.Hgt);
|
|---|
| 131 |
|
|---|
| 132 | memcpy(allFramesBuffer, &bestgpsposaFrame_, sizeof(TimestampedBestgpsposaFrame));
|
|---|
| 133 | //ShMem refresh
|
|---|
| 134 | mShMem->write(allFramesBuffer, AllFramesSize);
|
|---|
| 135 |
|
|---|
| 136 | GET_OUTPUT("bestposegpsposa",DbtPlyCPTComponent,TimestampedBestgpsposaFrame)->send(bestgpsposaFrame_);
|
|---|
| 137 | /*{
|
|---|
| 138 | // TODO: emit pose
|
|---|
| 139 | Pose p;
|
|---|
| 140 | // emit position
|
|---|
| 141 | p.p.lon = bestgpsposaFrame_.frame.Lon;
|
|---|
| 142 | p.p.lat = bestgpsposaFrame_.frame.Lat;
|
|---|
| 143 | p.p.alt = bestgpsposaFrame_.frame.Hgt;
|
|---|
| 144 | Q_EMIT(newPoseAvailable(p));
|
|---|
| 145 | }*/
|
|---|
| 146 | break;
|
|---|
| 147 |
|
|---|
| 148 | case TRAME_RAWIMUSA:
|
|---|
| 149 | memcpy(&(rawimuFrame_.frame), buf, sizeof(trame_rawimusa));
|
|---|
| 150 | rawimuFrame_.time = t;
|
|---|
| 151 | rawimuFrame_.timerange = tr;
|
|---|
| 152 | LOG_TRACE("[RAWIMUSA] frame replayed at " << rawimuFrame_.time);
|
|---|
| 153 |
|
|---|
| 154 | memcpy(allFramesBuffer
|
|---|
| 155 | + sizeof(TimestampedBestgpsposaFrame),
|
|---|
| 156 | &rawimuFrame_, sizeof(TimestampedRawimusaFrame));
|
|---|
| 157 | mShMem->write(allFramesBuffer, AllFramesSize);
|
|---|
| 158 |
|
|---|
| 159 | GET_OUTPUT("rawimusa",DbtPlyCPTComponent,TimestampedRawimusaFrame)->send(rawimuFrame_);
|
|---|
| 160 |
|
|---|
| 161 | break;
|
|---|
| 162 |
|
|---|
| 163 | case TRAME_INSPVAA:
|
|---|
| 164 | memcpy(&(inspvaFrame_.frame), buf, sizeof(trame_inspvaa));
|
|---|
| 165 | inspvaFrame_.time = t;
|
|---|
| 166 | inspvaFrame_.timerange = tr;
|
|---|
| 167 | LOG_TRACE("[INSPVAA] frame replayed at " << inspvaFrame_.time);
|
|---|
| 168 |
|
|---|
| 169 | LOG_DEBUG("[INSPVAA]" << "\t"
|
|---|
| 170 | << "STATUS=" << ((inspvaFrame_.frame.Status == INS_SOLUTION_GOOD) ? "INS_SOLUTION_GOOD" : "INS Unreliable"));
|
|---|
| 171 | LOG_DEBUG("[INSPVAA]" << "\t"
|
|---|
| 172 | << "Lat=" << inspvaFrame_.frame.Lat << "\t"
|
|---|
| 173 | << "Lon=" << inspvaFrame_.frame.Lon << "\t"
|
|---|
| 174 | << "Alt=" << inspvaFrame_.frame.Hgt << "\t"
|
|---|
| 175 | << "E=" << inspvaFrame_.frame.e << "\t"
|
|---|
| 176 | << "N=" << inspvaFrame_.frame.n << "\t"
|
|---|
| 177 | << "U=" << inspvaFrame_.frame.u << "\t"
|
|---|
| 178 | << "Azimu=" << inspvaFrame_.frame.Azimuth << "\t"
|
|---|
| 179 | << "Roll =" << inspvaFrame_.frame.Roll << "\t"
|
|---|
| 180 | << "Pitch=" << inspvaFrame_.frame.Pitch);
|
|---|
| 181 |
|
|---|
| 182 | memcpy(allFramesBuffer
|
|---|
| 183 | + sizeof(TimestampedBestgpsposaFrame) +sizeof(TimestampedRawimusaFrame),
|
|---|
| 184 | &inspvaFrame_, sizeof(TimestampedInspvaaFrame));
|
|---|
| 185 | mShMem->write(allFramesBuffer, AllFramesSize);
|
|---|
| 186 |
|
|---|
| 187 | GET_OUTPUT("inspvaaa",DbtPlyCPTComponent,TimestampedInspvaaFrame)->send(inspvaFrame_);
|
|---|
| 188 |
|
|---|
| 189 | genericEnuPose_.time = t;
|
|---|
| 190 | genericEnuPose_.timerange = tr;
|
|---|
| 191 | genericEnuPose_.position = QVector3D(inspvaFrame_.frame.e, inspvaFrame_.frame.n, inspvaFrame_.frame.u);
|
|---|
| 192 | 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));
|
|---|
| 193 |
|
|---|
| 194 | // for (int i=0;i<3;i++) {
|
|---|
| 195 | // for(int j=0;j<3;j++) {
|
|---|
| 196 | // pose.posconv[i][j] = d.frame.PosCov[i][j];
|
|---|
| 197 | // pose.angcov[i][j] = d.frame.AttCov[i][j];
|
|---|
| 198 | // }
|
|---|
| 199 | // }
|
|---|
| 200 | GET_OUTPUT("genericpose",DbtPlyCPTComponent,Pose3D)->send(genericEnuPose_);
|
|---|
| 201 |
|
|---|
| 202 | break;
|
|---|
| 203 |
|
|---|
| 204 | case TRAME_INSCOV:
|
|---|
| 205 | memcpy(&(inscovFrame_.frame), buf, sizeof(trame_inscov));
|
|---|
| 206 | inscovFrame_.time = t;
|
|---|
| 207 | inscovFrame_.timerange = tr;
|
|---|
| 208 | LOG_TRACE("[INSCOV] frame replayed at " << inscovFrame_.time);
|
|---|
| 209 |
|
|---|
| 210 | LOG_DEBUG("[INSCOV]" << "\t"
|
|---|
| 211 | << "Week=" << inscovFrame_.frame.Week << "\t"
|
|---|
| 212 | << "Secs=" << inscovFrame_.frame.Seconds << "\t"
|
|---|
| 213 | << "AttCov=" << inscovFrame_.frame.AttCov[0][0] << "\t"
|
|---|
| 214 | << "PosCov=" << inscovFrame_.frame.PosCov[0][0] << "\t"
|
|---|
| 215 | << "VelCov=" << inscovFrame_.frame.VelCov[0][0]);
|
|---|
| 216 |
|
|---|
| 217 | memcpy(allFramesBuffer
|
|---|
| 218 | + sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame) + sizeof(TimestampedInspvaaFrame),
|
|---|
| 219 | &inscovFrame_, sizeof(TimestampedInscovFrame));
|
|---|
| 220 | mShMem->write(allFramesBuffer, AllFramesSize);
|
|---|
| 221 |
|
|---|
| 222 | GET_OUTPUT("inscov",DbtPlyCPTComponent,TimestampedInscovFrame)->send(inscovFrame_);
|
|---|
| 223 | break;
|
|---|
| 224 |
|
|---|
| 225 | default:
|
|---|
| 226 | LOG_WARN("unknown frame type");
|
|---|
| 227 | }
|
|---|
| 228 |
|
|---|
| 229 | }
|
|---|
| 230 | }
|
|---|
| 231 |
|
|---|
| 232 | //////////////////////////////////////////////////////////////////////////
|
|---|
| 233 | /// Display GUI
|
|---|
| 234 | void DbtPlyCPTComponent::displayUI()
|
|---|
| 235 | {
|
|---|
| 236 | /* LOG_INFO("display GUI");
|
|---|
| 237 |
|
|---|
| 238 | mPoseViewer.reset(new PoseViewer());
|
|---|
| 239 | //mPoseViewer->setMutex(imageMutex_);
|
|---|
| 240 | mPoseViewer->setWindowTitle(componentName);
|
|---|
| 241 |
|
|---|
| 242 | qRegisterMetaType<Pose>("Pose");
|
|---|
| 243 | BOOST_VERIFY(
|
|---|
| 244 | connect(this, SIGNAL(newPoseAvailable(Pose)), mPoseViewer.get(), SLOT(addPose(Pose)))
|
|---|
| 245 | );
|
|---|
| 246 | mPoseViewer->show();*/
|
|---|
| 247 | }
|
|---|
| 248 |
|
|---|
