MRPT  2.0.3
CVelodyneScanner_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
12 #include <mrpt/system/filesystem.h>
13 #include <test_mrpt_common.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::hwdrivers;
17 using namespace std;
18 
19 #include <mrpt/config.h>
20 #if MRPT_HAS_LIBPCAP
21 
22 TEST(CVelodyneScanner, sample_vlp16_dataset)
23 {
24  const string fil =
25  UNITTEST_BASEDIR + string("/tests/sample_velodyne_vlp16_gps.pcap");
26 
27  if (!mrpt::system::fileExists(fil))
28  {
29  std::cerr << "WARNING: Skipping test due to missing file: " << fil
30  << "\n";
31  return;
32  }
33 
34  CVelodyneScanner velodyne;
35 
37  velodyne.setPCAPInputFile(fil);
38  velodyne.setPCAPInputFileReadOnce(true);
39  velodyne.enableVerbose(false);
40  velodyne.setPCAPVerbosity(false);
41 
42  velodyne.initialize();
43 
44  size_t nScans = 0, nGPS = 0;
45  bool rx_ok = true;
46  for (size_t i = 0; i < 1000 && rx_ok; i++)
47  {
50  rx_ok = velodyne.getNextObservation(scan, gps);
51  if (scan)
52  {
53  nScans++;
54  scan->generatePointCloud();
55  }
56  if (gps) nGPS++;
57  };
58  EXPECT_EQ(nScans, 4U);
59  EXPECT_GT(nGPS, 0U);
60 }
61 
62 TEST(CVelodyneScanner, sample_hdl32_dataset)
63 {
64  const string fil =
65  UNITTEST_BASEDIR + string("/tests/sample_velodyne_hdl32.pcap");
66 
67  if (!mrpt::system::fileExists(fil))
68  {
69  std::cerr << "WARNING: Skipping test due to missing file: " << fil
70  << "\n";
71  return;
72  }
73 
74  CVelodyneScanner velodyne;
75 
77  velodyne.setPCAPInputFile(fil);
78  velodyne.setPCAPInputFileReadOnce(true);
79  velodyne.enableVerbose(false);
80  velodyne.setPCAPVerbosity(false);
81 
82  velodyne.initialize();
83 
84  size_t nScans = 0;
85  // size_t nGPS=0;
86  bool rx_ok = true;
87  for (size_t i = 0; i < 1000 && rx_ok; i++)
88  {
91  rx_ok = velodyne.getNextObservation(scan, gps);
92  if (scan) nScans++;
93  // if (gps) nGPS++;
94  };
95  EXPECT_EQ(nScans, 3U);
96 }
97 
98 #endif // MRPT_HAS_LIBPCAP
filesystem.h
mrpt::hwdrivers::CVelodyneScanner::setPCAPInputFile
void setPCAPInputFile(const std::string &pcap_file)
Enables reading from a PCAP file instead of live UDP packet listening.
Definition: CVelodyneScanner.h:293
mrpt::hwdrivers::CVelodyneScanner::getNextObservation
bool getNextObservation(mrpt::obs::CObservationVelodyneScan::Ptr &outScan, mrpt::obs::CObservationGPS::Ptr &outGPS)
Polls the UDP port for incoming data packets.
Definition: CVelodyneScanner.cpp:196
TEST
TEST(ICP_SLAM_App, MapFromRawlog_PointMap)
Definition: ICP_SLAM_App_unittest.cpp:80
mrpt::hwdrivers::CVelodyneScanner::VLP16
@ VLP16
Definition: CVelodyneScanner.h:177
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CGenericSensor::enableVerbose
void enableVerbose(bool enabled=true)
Enable or disable extra debug info dumped to std::cout during sensor operation.
Definition: CGenericSensor.h:106
mrpt::hwdrivers::CVelodyneScanner::setPCAPVerbosity
void setPCAPVerbosity(const bool verbose)
Enables/disables PCAP info messages to console (default: true)
Definition: CVelodyneScanner.h:291
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::hwdrivers::CVelodyneScanner::setModelName
void setModelName(const model_t model)
See supported model names in the general discussion docs for mrpt::hwdrivers::CVelodyneScanner.
Definition: CVelodyneScanner.h:264
mrpt::hwdrivers::CVelodyneScanner::HDL32
@ HDL32
Definition: CVelodyneScanner.h:178
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
CVelodyneScanner.h
mrpt::obs::CObservationGPS::Ptr
std::shared_ptr< mrpt::obs ::CObservationGPS > Ptr
Definition: CObservationGPS.h:69
mrpt::hwdrivers::CVelodyneScanner::initialize
void initialize() override
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig.
Definition: CVelodyneScanner.cpp:384
mrpt::hwdrivers::CVelodyneScanner
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows.
Definition: CVelodyneScanner.h:165
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
mrpt::obs::CObservationVelodyneScan::Ptr
std::shared_ptr< mrpt::obs ::CObservationVelodyneScan > Ptr
Definition: CObservationVelodyneScan.h:83
EXPECT_GT
EXPECT_GT(out.final_iters, 10UL)
mrpt::hwdrivers::CVelodyneScanner::setPCAPInputFileReadOnce
void setPCAPInputFileReadOnce(bool read_once)
Definition: CVelodyneScanner.h:305



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020