Changeset 104 in pacpussensors for trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp

Timestamp:

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

Author:

xuphilip

Message:

Updated I/O for Sick LDMRS

File:

• trunk/StdDbtPlayerComponents/DbtPlySickLDMRSManager.cpp (modified) (7 diffs)

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);
 }