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