Changeset 104 in pacpussensors

Timestamp:

Nov 22, 2015, 11:00:29 PM (9 years ago)

Author:

xuphilip

Message:

Updated I/O for Sick LDMRS

Location:

trunk

Files:

• Sick/SickLDMRSData.h (modified) (5 diffs)
• StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp (modified) (7 diffs)
• StdDbtPlayerComponents/DbtPlySickLDMRSManager.h (modified) (2 diffs)

trunk/Sick/SickLDMRSData.h

@@ -17 +17 @@
 #include "Pacpus/kernel/cstdint.h"
 #include "Pacpus/kernel/road_time.h"
+#include <QVector>
 
 // Export macro for SickLDMRS DLL for Windows only
@@ -83 +84 @@
     uint64_t endNtpTime;   //!< NTP time last measurement
     uint16_t ticksPerRot;  //!< Angle ticks per rotation (used to compute the real angle of a point)
-    int16_t startAngle;     //!< Angle of the first measured value
-    int16_t endAngle;       //!< Angle of the last measured value
+    int16_t  startAngle;     //!< Angle of the first measured value
+    int16_t  endAngle;       //!< Angle of the last measured value
     uint16_t numPoints;    //!< Number of scanned points during this scan @see ScanPoint
 
@@ -124 +125 @@
  * Data type 0x2221 @see DataHeader
  */
+
 struct SICKLDMRS_API ScanObject{
     // TODO
@@ -136 +138 @@
 typedef struct
 {
-    uint64_t timeStartFromSensor;  //!< NTP time (creation of the message on sensor).
+    uint64_t timeStartFromSensor;   //!< NTP time (creation of the message on sensor).
     ScanHeader hScan;               //!< General information about points recorded. @see ScanHeader
     road_time_t time;               //!< DBT timestamp.
@@ -142 +144 @@
     int32_t dataPos;                //!< The position of the data in the binary file associated to the dbt file (utc file).
 } SickLDMRS_dbt;
+
+/*!
+ * \brief The Sick LDMRS scan struct
+ *
+ * Used to describe one complete scan
+ * @see DataHeader
+ */
+struct SICKLDMRS_API SickLDMRSScan{
+    uint64_t timeStartFromSensor;   //!< NTP time (creation of the message on sensor).
+    ScanHeader header;              //!< General information about points recorded.
+    QVector<ScanPoint> points;      //!< Scan point cloud
+    road_time_t time;               //!< DBT timestamp.
+    road_timerange_t timerange;     //!< DBT timerange.
+};
 
 #ifdef SICKLDMRS_SH_MEM

trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp

@@ -35 +35 @@
 static const char * kSickMemoryName = "sickLDMRS";
 
-//////////////////////////////////////////////////////////////////////////
-/// Constructor.
+/************************************************************************
+ * Constructor
+ ************************************************************************/
 DbtPlySickLDMRSManager::DbtPlySickLDMRSManager(QString name)
     : DbtPlyFileManager(name)
 {
     LOG_TRACE("constructor(" << name << ")");
-
-    // mShMem = new ShMem(kAlaskaMemoryName, sizeof(ScanSickData));
 }
 
-//////////////////////////////////////////////////////////////////////////
-/// Destructor.
+/************************************************************************
+ * Destructor
+ ************************************************************************/
 DbtPlySickLDMRSManager::~DbtPlySickLDMRSManager()
 {
     LOG_TRACE("destructor");
-    // delete mShMem;
 }
 
-//////////////////////////////////////////////////////////////////////////
-/// Configure the component.
+/************************************************************************
+ * Configuration of the component, called by the ComponentManager after
+ * the construction of the object
+ ************************************************************************/
 ComponentBase::COMPONENT_CONFIGURATION DbtPlySickLDMRSManager::configureComponent(XmlComponentConfig config)
 {
@@ -64 +65 @@
 }
 
-//////////////////////////////////////////////////////////////////////////
-/// Starts the component.
+/************************************************************************
+ * Start function, called by the ComponentManager when a start()
+ * command is received
+ ************************************************************************/
 void DbtPlySickLDMRSManager::startActivity()
 {
-    // QString dataDir = mEngine->getDataDir();
-
-    // for (int i = 0; i < mDataFilenameList.size(); ++i) {
-    //     QString file = mDataFilenameList.at(i);
-        // mDataFilenameList[i] = dataDir + file;
-
     LOG_TRACE("DbtPlySickLDMRSManager component is starting.");
 
@@ -85 +82 @@
         return;
     }
-    // }
+
+    outScan = getTypedOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");
+
     DbtPlyFileManager::startActivity();
 }
 
-//////////////////////////////////////////////////////////////////////////
-/// Stops the component.
+/************************************************************************
+ * Stop function, called by the ComponentManager when a stop()
+ * command is received
+ ************************************************************************/
 void DbtPlySickLDMRSManager::stopActivity()
 {
@@ -97 +98 @@
 }
 
-//////////////////////////////////////////////////////////////////////////
-/// processData
+
+/************************************************************************
+ * Called by the framework at initialization
+ ************************************************************************/
+void DbtPlySickLDMRSManager::addInputs()
+{
+  // uncomment to add an input
+}
+
+
+/************************************************************************
+ * Called by the framework at initialization
+ ************************************************************************/
+void DbtPlySickLDMRSManager::addOutputs()
+{
+  // empty: no output
+  addOutput<SickLDMRSScan, DbtPlySickLDMRSManager>("scan");
+}
+
+
+/************************************************************************
+ * Process LiDAR data
+ ************************************************************************/
 void DbtPlySickLDMRSManager::processData(road_time_t t, road_timerange_t tr, void * buffer)
 {
@@ -111 +133 @@
 
     // // copy the values contained in the dbt file
-    mSickDbt.timeStartFromSensor = sickLDMRS_dbt->timeStartFromSensor;
-    mSickDbt.hScan = sickLDMRS_dbt->hScan;
-    mSickDbt.dataPos = sickLDMRS_dbt->dataPos;
-    mSickDbt.time = t;
-    mSickDbt.timerange = tr;
+    mScan.timeStartFromSensor = sickLDMRS_dbt->timeStartFromSensor;
+    mScan.header = sickLDMRS_dbt->hScan;
+    mScan.time = t;
+    mScan.timerange = tr;
+    mScan.points = QVector<ScanPoint>(mScan.header.numPoints);
 
-    LOG_TRACE("Number of points " << mSickDbt.hScan.numPoints);
+    LOG_TRACE("Number of points " << mScan.header.numPoints);
 
     LOG_TRACE("Reading UTC file ... ");
 
-    mDataFile.seekg(mSickDbt.dataPos); // set the get pointer to the correct place
-    
-    ScanPoint* points = (ScanPoint*) malloc(mSickDbt.hScan.numPoints * sizeof(ScanPoint));
+    mDataFile.seekg(sickLDMRS_dbt->dataPos); // set the get pointer to the correct place
 
     // then copy the data contained in the binary file
-    for (unsigned int i = 0 ; i < mSickDbt.hScan.numPoints ; ++i) {
-        mDataFile.read(reinterpret_cast<char *>(&(points[i])), sizeof(ScanPoint));
+    for (unsigned int i = 0 ; i < mScan.header.numPoints ; ++i) {
+        mDataFile.read(reinterpret_cast<char *>(&(mScan.points[i])), sizeof(ScanPoint));
     }
-
 
     // verify that the last value is the UTC magic word
@@ -138 +157 @@
         LOG_DEBUG("wrong magic word: EXPECTED=" << UTC_MAGIC_WORD << ", ACTUAL=" << utcMagicWord);
     } else {
-        LOG_TRACE("writing scan ");
-        LOG_WARN("NOT YET IMPLEMENTED");
-        // mShMem->write(&mSickDbt, sizeof(SickLMS_dbt));
-
-        /**********************************/
-        /*      TODO : Send data !        */
-        /**********************************/
+        LOG_TRACE("sending scan ");
+        checkedSend(outScan, mScan);
     }
 
     if (mVerbose) {
         cout << "[SICK LDMRS]:\t"
-             << "numPoints=" << mSickDbt.hScan.numPoints << "\t"
+             << "numPoints=" << mScan.header.numPoints << "\t"
              << "time=" << t << endl
                 ;
@@ -155 +169 @@
     if (mVerbose >= 2)  {
         cout << "[SICK LDMRS]:\t"
-             << "startAngle=" << mSickDbt.hScan.startAngle << "\t" 
-             << "endAngle=" << mSickDbt.hScan.endAngle << std::endl ;
+             << "startAngle=" << mScan.header.startAngle << "\t"
+             << "endAngle=" << mScan.header.endAngle << std::endl ;
     }
-    free(points);
 }
 

trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.h

@@ -17 +17 @@
 
 #include "Pacpus/DbitePlayer/DbtPlyFileManager.h"
+#include <Pacpus/kernel/InputOutputInterface.h>
 #include "../Sick/SickLDMRSData.h"
 
@@ -44 +45 @@
     ~DbtPlySickLDMRSManager();
 
+    virtual void startActivity();
+    virtual void stopActivity();
+    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
+
+    virtual void addInputs();
+    virtual void addOutputs();
+
 protected:
     void processData(road_time_t t, road_timerange_t tr, void * buffer);
     void displayUI();
 
-    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
-    virtual void startActivity();
-    virtual void stopActivity();
-
 private:
-    // ShMem * mShMem;
     SickLDMRS_dbt mSickDbt;
 
+    // .dbt file ("sickldmrs_0.dbt")
     std::ifstream mDataFile;
+    // .utc data file ("sickldmrs_0_data.utc")
     QString mDataFilename;
 
-    QStringList mDataFilenameList;
+    // local copy of each scan
+    SickLDMRSScan mScan;
+
+    // Declaration of outputs
+    OutputInterface<SickLDMRSScan, DbtPlySickLDMRSManager>* outScan;
 };