LdmrsScanpointCoordinateApp.cpp
Go to the documentation of this file.
00001 //
00002 // LdmrsScanpointCoordinateApp.cpp
00003 //
00004 // Demo application. Receives scans, and searches each layer for the point at a certain horizontal scan angle.
00005 // Adds up some scans and then prints the distance and coordinates of these points.
00006 // This demo shows the scanpoint coordinates of scanner layers, e.g. the z coordinates. It can be used to verify the
00007 // calculation of the coordinates.
00008 // Also shows how to work with the mirror sides and 4- and 8-layer-scanners.
00009 //
00010 // In the constructor, set the angle and number of scans to the desired values.
00011 // Note that scans must be enabled in the MRS device in order for this application to work.
00012 //
00013 
00014 #include "LdmrsScanpointCoordinateApp.hpp"
00015 #include "../tools/errorhandler.hpp"    // for printInfoMessage()
00016 #include "../tools/toolbox.hpp"                 // for toString()
00017 #include "../datatypes/Scan.hpp"
00018 #include "../datatypes/Object.hpp"
00019 #include "../datatypes/Msg.hpp"
00020 #include "../datatypes/Measurement.hpp"
00021 #include "../datatypes/Fields.hpp"
00022 #include "../datatypes/EvalCases.hpp"
00023 #include "../datatypes/EvalCaseResults.hpp"
00024 
00025 namespace application
00026 {
00027 
00028 //
00029 // Constructor
00030 //
00031 LdmrsScanpointCoordinateApp::LdmrsScanpointCoordinateApp(Manager* manager)
00032 {
00033         // Enable this flag for *very* verbose debug output
00034         m_beVerbose = false;    // true;
00035         
00036         // Number of scans to build the statistics with. Note that for 8-layer scanners, the number of points in each layer
00037         // will be half of this value as the scanner needs two scans for a complete 8-layer-scan.
00038         m_numScansToCount = 20;
00039         
00040         // Horizontal scan angle at which the sampling is done.
00041         // +degrees: "to the left",
00042         // 0.0 : "straight forward",
00043         // -degrees: "to the right"
00044         m_wantedHorzAngle = 0.0 * deg2rad;
00045         
00046         // Some other stuff
00047         initCycle();
00048         m_isFourLayerScanner = true;
00049 
00050         printInfoMessage("LdmrsScanpointCoordinateApp constructor done.", m_beVerbose);
00051 }
00052 
00053 
00054 // Destructor
00055 // Clean up all dynamic data structures
00056 LdmrsScanpointCoordinateApp::~LdmrsScanpointCoordinateApp()
00057 {
00058         printInfoMessage("LdmrsScanpointCoordinateApp says Goodbye!", m_beVerbose);
00059 }
00060 
00061 //
00062 // Initialize all data for a new collection cycle.
00063 //
00064 void LdmrsScanpointCoordinateApp::initCycle()
00065 {
00066         m_scanCounter = 0;
00067         for (UINT8 i = 0; i<8; i++)
00068         {
00069                 m_numPoints[i] = 0;
00070                 m_meanDist[i] = 0.0;
00071                 m_meanXyz[i] = Point3D(0.0, 0.0, 0.0);
00072         }
00073 }
00074 
00075 
00076 //
00077 // Receiver for new data from the manager.
00078 //
00079 void LdmrsScanpointCoordinateApp::setData(BasicData& data)
00080 {
00081         //
00082         // Do something with it.
00083         //
00084         // Here, we sort out scan data and hand it over to the processing.
00085         //
00086         std::string datatypeStr;
00087         std::string sourceIdStr;
00088 
00089         if (data.getDatatype() == Datatype_Scan)
00090         {
00091                 Scan* scan = dynamic_cast<Scan*>(&data);
00092 
00093                 // Search the points and sum them up
00094                 processScan(scan);
00095                 m_scanCounter++;
00096                 
00097                 if (m_scanCounter >= m_numScansToCount)
00098                 {
00099                         // Calculate and print the statistics
00100                         calcAndShowStatistics();
00101                         
00102                         // Begin new cycle, reset all variables
00103                         initCycle();
00104                 }
00105         }
00106         
00107         printInfoMessage("LdmrsScanpointCoordinateApp::setData(): Done.", m_beVerbose);
00108 }
00109 
00110 //
00111 // Calculate the mean values of the coordinates, and print the values.
00112 //
00113 void LdmrsScanpointCoordinateApp::calcAndShowStatistics()
00114 {
00115         // Calc statistics
00116         for (UINT8 layer = 0; layer < 8; layer++)
00117         {
00118                 if (m_numPoints[layer] > 1)
00119                 {
00120                         m_meanXyz[layer] /= static_cast<double>(m_numPoints[layer]);
00121                         m_meanDist[layer] /= static_cast<double>(m_numPoints[layer]);
00122                 }
00123         }
00124         
00125         
00126         // Show statistics
00127         printInfoMessage(" ", true); 
00128         printInfoMessage("Statistics for " + toString(m_numScansToCount) + " scans at angle " +
00129                                                 toString(m_wantedHorzAngle * rad2deg, 1) + " degrees.", true); 
00130 
00131         // 4 or 8 layers?
00132         INT16 startLayer = 3;
00133         if (m_isFourLayerScanner == false)
00134         {
00135                 // 8 layers
00136                 startLayer = 7;
00137         }
00138         
00139         for (INT16 layer = startLayer; layer >= 0; layer -= 1)
00140         {
00141                 printInfoMessage("Layer " + toString(layer + 1) + ": " + toString(m_numPoints[layer]) + " pts;" + 
00142                                                         " x=" + toString(m_meanXyz[layer].getX(), 2) + " m," +
00143                                                         " y=" + toString(m_meanXyz[layer].getY(), 2) + " m," +
00144                                                         " z=" + toString(m_meanXyz[layer].getZ(), 2) + " m," +
00145                                                         "; dist=" + toString(m_meanDist[layer], 2) + " m.", true);
00146         }
00147         printInfoMessage(" ", true); 
00148 }
00149 
00150 //
00151 // Returns true if the scan originates from a 4-layer-scanner. 
00152 //
00153 bool LdmrsScanpointCoordinateApp::isFourLayerScanner(Scan* scan)
00154 {
00155         printInfoMessage("LdmrsScanpointCoordinateApp::isFourLayerScanner: Beam tilt=" + toString(fabs(scan->getScannerInfos().at(0).getBeamTilt() * rad2deg), 1) + " degrees.", m_beVerbose);
00156         
00157         // Check beam tilt
00158         if (fabs(scan->getScannerInfos().at(0).getBeamTilt()) < (0.2 * deg2rad))
00159         {
00160                 // Beam tilt is near 0, so it is a 4-layer-scanner 
00161                 return true;
00162         }
00163         
00164         return false;
00165 }
00166 
00167 //
00168 // Decode and print scan point coordinates.
00169 //
00170 void LdmrsScanpointCoordinateApp::processScan(Scan* scan)
00171 {
00172         ScanPoint point;
00173         bool success;
00174         UINT8 layerOffset = 0;
00175         
00176         // 4-layer or 8-layer scanner?
00177         if (isFourLayerScanner(scan) == true)
00178         {
00179                 // No beam tilt, it is a 4-layer scanner
00180                 m_isFourLayerScanner = true;
00181                 layerOffset = 0;
00182         }
00183         else
00184         {
00185                 // 8-layer
00186                 m_isFourLayerScanner = false;
00187 
00188                 // What layers are we in? Upper or lower 4?
00189                 if (scan->getScannerInfos().at(0).isRearMirrorSide() == true)
00190                 {
00191                         // Rear mirror side is facing upward in 8-layer scanners -> Upper 4 layers
00192                         layerOffset = 4;
00193                 }
00194                 else
00195                 {
00196                         // Lower 4 layers
00197                         layerOffset = 0;
00198                 }
00199         }
00200         
00201 
00202         // Find the points at the desired horizontal angle
00203         for (UINT8 layer = 0; layer < 4; layer++)
00204         {
00205                 success = getClosestScanpoint(layer, m_wantedHorzAngle, scan, &point);
00206                 if (success == true)
00207                 {
00208 //                      printInfoMessage("LdmrsScanpointCoordinateApp::processScan: Point found, hAngle is " + toString(point.getHAngle() * rad2deg, 1) +
00209 //                      " degrees in layer " + toString(layer) + ".", true);
00210                 }
00211                 else
00212                 {
00213                         printInfoMessage("LdmrsScanpointCoordinateApp::processScan: No point found for hAngle " + toString(m_wantedHorzAngle * rad2deg, 1) +
00214                         " degrees in layer " + toString(layer) + ".", true);
00215                 }
00216 
00217                 // Sum up the point coordinates
00218                 m_numPoints[layer + layerOffset]++;
00219                 m_meanXyz[layer + layerOffset] += point.toPoint3D();
00220                 m_meanDist[layer + layerOffset] += point.getDist();
00221         }
00222 }
00223 
00224 //
00225 // Searches the scan for the best matching scanpoint in the given layer and close to the given horizontal scan angle.
00226 // If no point is found within 2 degrees around the wanted angle, no point is returned.
00227 // Returns true if a point was found, false if not.
00228 //
00229 bool LdmrsScanpointCoordinateApp::getClosestScanpoint(const UINT8 layer, const double horzAngle, const Scan* scan, ScanPoint* point)
00230 {
00231         UINT32 i;
00232         ScanPoint bestPoint;
00233         ScanPoint currentPoint;
00234         bool success;
00235         
00236         // Set coordinates to a point far, far away
00237         bestPoint.setPolar(1000, 90 * deg2rad, 90 * deg2rad);
00238         
00239         // Search for best match to search parameters
00240         for (i=0; i<scan->getNumPoints(); i++)
00241         {
00242                 currentPoint = scan->at(i);
00243                 if (currentPoint.getLayer() == layer)
00244                 {
00245 //                      printInfoMessage("Checking angle " + toString(currentPoint.getHAngle() * rad2deg, 1) + " against wanted " +
00246 //                                                              toString(horzAngle * rad2deg, 1) + ".", true); 
00247                         if (fabs(currentPoint.getHAngle() - horzAngle) < fabs(bestPoint.getHAngle() - horzAngle))
00248                         {
00249                                 bestPoint = currentPoint;
00250 //                              printInfoMessage("Best point yet!", true); 
00251                         }
00252                 }
00253         }
00254         
00255         // Is the match good enough?
00256         if (fabs(bestPoint.getHAngle() - horzAngle) < (2.0 * deg2rad))
00257         {
00258                 // Good enough
00259                 *point = bestPoint;
00260                 success = true;
00261         }
00262         else
00263         {
00264                 // No match found
00265                 success = false;
00266         }
00267         
00268         return success;
00269 }
00270 
00271 }       // namespace application


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Thu Jun 6 2019 21:02:36