mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 17:17:47 +08:00
Added SensorCapture and SensorCaptureThread doc and tests
This commit is contained in:
matlabbe
@@ -105,3 +105,13 @@ gtest_discover_tests(test_sensorevent)
|
||||
add_executable(test_sensordata test_sensordata.cpp)
|
||||
target_link_libraries(test_sensordata gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensordata)
|
||||
|
||||
#SensorCapture.h
|
||||
add_executable(test_sensorcapture test_sensorcapture.cpp)
|
||||
target_link_libraries(test_sensorcapture gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensorcapture)
|
||||
|
||||
#SensorCaptureThread.h
|
||||
add_executable(test_sensorcapturethread test_sensorcapturethread.cpp)
|
||||
target_link_libraries(test_sensorcapturethread gtest_main rtabmap_core)
|
||||
gtest_discover_tests(test_sensorcapturethread)
|
||||
|
||||
@@ -0,0 +1,447 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/SensorCapture.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Mock SensorCapture implementation for testing
|
||||
class MockSensorCapture : public SensorCapture
|
||||
{
|
||||
public:
|
||||
MockSensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity()) :
|
||||
SensorCapture(frameRate, localTransform),
|
||||
initCalled_(false),
|
||||
initResult_(true),
|
||||
serial_("MOCK_SENSOR_001"),
|
||||
odomProvided_(false),
|
||||
captureCount_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~MockSensorCapture() {}
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
|
||||
{
|
||||
initCalled_ = true;
|
||||
calibrationFolder_ = calibrationFolder;
|
||||
cameraName_ = cameraName;
|
||||
return initResult_;
|
||||
}
|
||||
|
||||
virtual std::string getSerial() const
|
||||
{
|
||||
return serial_;
|
||||
}
|
||||
|
||||
virtual bool odomProvided() const
|
||||
{
|
||||
return odomProvided_;
|
||||
}
|
||||
|
||||
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06)
|
||||
{
|
||||
if (odomProvided_)
|
||||
{
|
||||
pose = Transform(1, 0, 0, 0, 0, 0, 1);
|
||||
covariance = cv::Mat::eye(6, 6, CV_64FC1);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Test helpers
|
||||
void setInitResult(bool result) { initResult_ = result; }
|
||||
void setSerial(const std::string & serial) { serial_ = serial; }
|
||||
void setOdomProvided(bool provided) { odomProvided_ = true; }
|
||||
|
||||
bool wasInitCalled() const { return initCalled_; }
|
||||
std::string getCalibrationFolder() const { return calibrationFolder_; }
|
||||
std::string getCameraName() const { return cameraName_; }
|
||||
int getCaptureCount() const { return captureCount_; }
|
||||
|
||||
protected:
|
||||
virtual SensorData captureData(SensorCaptureInfo * info = 0)
|
||||
{
|
||||
++captureCount_;
|
||||
|
||||
cv::Mat image = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
SensorData data(image, getNextSeqID(), UTimer::now());
|
||||
return data;
|
||||
}
|
||||
|
||||
private:
|
||||
bool initCalled_;
|
||||
bool initResult_;
|
||||
std::string serial_;
|
||||
bool odomProvided_;
|
||||
std::string calibrationFolder_;
|
||||
std::string cameraName_;
|
||||
int captureCount_;
|
||||
};
|
||||
|
||||
// Constructor Tests
|
||||
|
||||
TEST(SensorCaptureTest, DefaultConstructor)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_EQ(sensor.getFrameRate(), 0.0f);
|
||||
EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
|
||||
EXPECT_FALSE(sensor.wasInitCalled());
|
||||
EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
|
||||
EXPECT_FALSE(sensor.odomProvided());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, ConstructorWithFrameRate)
|
||||
{
|
||||
float frameRate = 30.0f;
|
||||
MockSensorCapture sensor(frameRate);
|
||||
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
|
||||
EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, ConstructorWithLocalTransform)
|
||||
{
|
||||
Transform localTransform(1, 0, 0, 0, 0, 0, 1);
|
||||
MockSensorCapture sensor(0, localTransform);
|
||||
|
||||
EXPECT_EQ(sensor.getLocalTransform(), localTransform);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, ConstructorWithFrameRateAndTransform)
|
||||
{
|
||||
float frameRate = 15.0f;
|
||||
Transform localTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
|
||||
MockSensorCapture sensor(frameRate, localTransform);
|
||||
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
|
||||
EXPECT_EQ(sensor.getLocalTransform(), localTransform);
|
||||
}
|
||||
|
||||
// Init Tests
|
||||
|
||||
TEST(SensorCaptureTest, InitDefault)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_TRUE(sensor.init());
|
||||
EXPECT_TRUE(sensor.wasInitCalled());
|
||||
EXPECT_EQ(sensor.getCalibrationFolder(), ".");
|
||||
EXPECT_EQ(sensor.getCameraName(), "");
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, InitWithCalibrationFolder)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
std::string folder = "/path/to/calibration";
|
||||
|
||||
EXPECT_TRUE(sensor.init(folder));
|
||||
EXPECT_EQ(sensor.getCalibrationFolder(), folder);
|
||||
EXPECT_EQ(sensor.getCameraName(), "");
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, InitWithCalibrationFolderAndCameraName)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
std::string folder = "/path/to/calibration";
|
||||
std::string cameraName = "camera1";
|
||||
|
||||
EXPECT_TRUE(sensor.init(folder, cameraName));
|
||||
EXPECT_EQ(sensor.getCalibrationFolder(), folder);
|
||||
EXPECT_EQ(sensor.getCameraName(), cameraName);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, InitFailure)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setInitResult(false);
|
||||
|
||||
EXPECT_FALSE(sensor.init());
|
||||
EXPECT_TRUE(sensor.wasInitCalled());
|
||||
}
|
||||
|
||||
// Serial Tests
|
||||
|
||||
TEST(SensorCaptureTest, GetSerial)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
|
||||
|
||||
sensor.setSerial("CUSTOM_SERIAL_123");
|
||||
EXPECT_EQ(sensor.getSerial(), "CUSTOM_SERIAL_123");
|
||||
}
|
||||
|
||||
// Frame Rate Tests
|
||||
|
||||
TEST(SensorCaptureTest, SetGetFrameRate)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_EQ(sensor.getFrameRate(), 0.0f);
|
||||
|
||||
sensor.setFrameRate(30.0f);
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), 30.0f);
|
||||
|
||||
sensor.setFrameRate(0.0f);
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), 0.0f);
|
||||
|
||||
sensor.setFrameRate(60.0f);
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), 60.0f);
|
||||
}
|
||||
|
||||
// Local Transform Tests
|
||||
|
||||
TEST(SensorCaptureTest, SetGetLocalTransform)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
|
||||
|
||||
Transform transform(1, 0, 0, 0, 0, 0, 1);
|
||||
sensor.setLocalTransform(transform);
|
||||
EXPECT_EQ(sensor.getLocalTransform(), transform);
|
||||
|
||||
Transform transform2(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
|
||||
sensor.setLocalTransform(transform2);
|
||||
EXPECT_EQ(sensor.getLocalTransform(), transform2);
|
||||
}
|
||||
|
||||
// Reset Timer Tests
|
||||
|
||||
TEST(SensorCaptureTest, ResetTimer)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
// Reset should not throw
|
||||
EXPECT_NO_THROW(sensor.resetTimer());
|
||||
|
||||
// After reset, timer should be ready for frame rate control
|
||||
sensor.setFrameRate(10.0f);
|
||||
sensor.resetTimer();
|
||||
|
||||
// First capture should be fast (no throttling needed)
|
||||
SensorData data = sensor.takeData();
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
// TakeData Tests
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataBasic)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
SensorData data = sensor.takeData();
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_GT(data.id(), 0);
|
||||
EXPECT_GT(data.stamp(), 0.0);
|
||||
EXPECT_FALSE(data.imageRaw().empty());
|
||||
EXPECT_EQ(sensor.getCaptureCount(), 1);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataWithInfo)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
SensorCaptureInfo info;
|
||||
|
||||
SensorData data = sensor.takeData(&info);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(info.id, data.id());
|
||||
EXPECT_DOUBLE_EQ(info.stamp, data.stamp());
|
||||
EXPECT_GT(info.timeCapture, 0.0);
|
||||
EXPECT_LE(info.timeCapture, 0.1); // Should be very fast for mock
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataSequenceIDs)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
SensorData data1 = sensor.takeData();
|
||||
SensorData data2 = sensor.takeData();
|
||||
SensorData data3 = sensor.takeData();
|
||||
|
||||
EXPECT_EQ(data1.id() + 1, data2.id());
|
||||
EXPECT_EQ(data2.id() + 1, data3.id());
|
||||
EXPECT_EQ(sensor.getCaptureCount(), 3);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataNoFrameRateThrottling)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setFrameRate(0.0f); // Unlimited
|
||||
|
||||
UTimer timer;
|
||||
SensorData data1 = sensor.takeData();
|
||||
SensorData data2 = sensor.takeData();
|
||||
double elapsed = timer.ticks();
|
||||
|
||||
// Without throttling, captures should be very fast
|
||||
EXPECT_LT(elapsed, 0.1);
|
||||
EXPECT_TRUE(data1.isValid());
|
||||
EXPECT_TRUE(data2.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataWithFrameRateThrottling)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setFrameRate(10.0f); // 10 Hz = 100ms per frame
|
||||
sensor.resetTimer();
|
||||
|
||||
UTimer timer;
|
||||
SensorData data1 = sensor.takeData();
|
||||
SensorData data2 = sensor.takeData();
|
||||
double elapsed = timer.ticks();
|
||||
|
||||
// With 10 Hz throttling, two captures should take at least ~100ms
|
||||
EXPECT_GE(elapsed, 0.2); // Should be at least 200 ms for two frames
|
||||
EXPECT_LE(elapsed, 0.21); // Slight margin
|
||||
EXPECT_TRUE(data1.isValid());
|
||||
EXPECT_TRUE(data2.isValid());
|
||||
}
|
||||
|
||||
// OdomProvided Tests
|
||||
|
||||
TEST(SensorCaptureTest, OdomProvidedDefault)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
EXPECT_FALSE(sensor.odomProvided());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, OdomProvidedEnabled)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setOdomProvided(true);
|
||||
|
||||
EXPECT_TRUE(sensor.odomProvided());
|
||||
}
|
||||
|
||||
// GetPose Tests
|
||||
|
||||
TEST(SensorCaptureTest, GetPoseDefault)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
Transform pose;
|
||||
cv::Mat covariance;
|
||||
|
||||
EXPECT_FALSE(sensor.getPose(0.0, pose, covariance));
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, GetPoseWithOdom)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setOdomProvided(true);
|
||||
|
||||
Transform pose;
|
||||
cv::Mat covariance;
|
||||
|
||||
EXPECT_TRUE(sensor.getPose(0.0, pose, covariance));
|
||||
EXPECT_FALSE(pose.isNull());
|
||||
EXPECT_FALSE(covariance.empty());
|
||||
EXPECT_EQ(covariance.rows, 6);
|
||||
EXPECT_EQ(covariance.cols, 6);
|
||||
}
|
||||
|
||||
// Comprehensive Usage Test
|
||||
|
||||
TEST(SensorCaptureTest, ComprehensiveUsage)
|
||||
{
|
||||
// Create sensor with frame rate and transform
|
||||
float frameRate = 20.0f;
|
||||
Transform localTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
|
||||
MockSensorCapture sensor(frameRate, localTransform);
|
||||
|
||||
// Initialize
|
||||
EXPECT_TRUE(sensor.init("/calib", "test_camera"));
|
||||
EXPECT_EQ(sensor.getCalibrationFolder(), "/calib");
|
||||
EXPECT_EQ(sensor.getCameraName(), "test_camera");
|
||||
|
||||
// Verify settings
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
|
||||
EXPECT_EQ(sensor.getLocalTransform(), localTransform);
|
||||
EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
|
||||
|
||||
// Change settings
|
||||
sensor.setFrameRate(30.0f);
|
||||
Transform newTransform = Transform::getIdentity();
|
||||
sensor.setLocalTransform(newTransform);
|
||||
sensor.setSerial("NEW_SERIAL");
|
||||
|
||||
EXPECT_FLOAT_EQ(sensor.getFrameRate(), 30.0f);
|
||||
EXPECT_EQ(sensor.getLocalTransform(), newTransform);
|
||||
EXPECT_EQ(sensor.getSerial(), "NEW_SERIAL");
|
||||
|
||||
// Capture data
|
||||
sensor.resetTimer();
|
||||
SensorCaptureInfo info;
|
||||
SensorData data = sensor.takeData(&info);
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_EQ(info.id, data.id());
|
||||
EXPECT_DOUBLE_EQ(info.stamp, data.stamp());
|
||||
EXPECT_GT(info.timeCapture, 0.0);
|
||||
EXPECT_EQ(sensor.getCaptureCount(), 1);
|
||||
}
|
||||
|
||||
// Edge Cases
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataMultipleCalls)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
for (int i = 0; i < 10; ++i)
|
||||
{
|
||||
SensorData data = sensor.takeData();
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
EXPECT_EQ(sensor.getCaptureCount(), 10);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, TakeDataWithNullInfo)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
|
||||
// Should not crash with null info
|
||||
SensorData data = sensor.takeData(nullptr);
|
||||
EXPECT_TRUE(data.isValid());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, HighFrameRate)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setFrameRate(1000.0f); // Very high frame rate
|
||||
sensor.resetTimer();
|
||||
|
||||
// Should handle high frame rate gracefully
|
||||
UTimer timer;
|
||||
SensorData data = sensor.takeData();
|
||||
double elapsed = timer.ticks();
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_GE(elapsed, 0.001);
|
||||
EXPECT_LT(elapsed, 0.01);
|
||||
}
|
||||
|
||||
TEST(SensorCaptureTest, VeryLowFrameRate)
|
||||
{
|
||||
MockSensorCapture sensor;
|
||||
sensor.setFrameRate(2.0f); // Low frame rate (500 ms seconds per frame)
|
||||
sensor.resetTimer();
|
||||
|
||||
UTimer timer;
|
||||
SensorData data = sensor.takeData();
|
||||
double elapsed = timer.ticks();
|
||||
|
||||
EXPECT_TRUE(data.isValid());
|
||||
EXPECT_GE(elapsed, 0.5);
|
||||
EXPECT_LT(elapsed, 0.51);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,572 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/SensorCaptureThread.h>
|
||||
#include <rtabmap/core/Camera.h>
|
||||
#include <rtabmap/core/Lidar.h>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/SensorCaptureInfo.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/core/LaserScan.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
// Mock Camera implementation for testing
|
||||
class MockCamera : public Camera
|
||||
{
|
||||
public:
|
||||
MockCamera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()) :
|
||||
Camera(imageRate, localTransform),
|
||||
initCalled_(false),
|
||||
initResult_(true),
|
||||
serial_("MOCK_CAMERA_001"),
|
||||
calibrated_(false),
|
||||
odomProvided_(false),
|
||||
captureCount_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~MockCamera() {}
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
|
||||
{
|
||||
initCalled_ = true;
|
||||
calibrationFolder_ = calibrationFolder;
|
||||
cameraName_ = cameraName;
|
||||
return initResult_;
|
||||
}
|
||||
|
||||
virtual std::string getSerial() const
|
||||
{
|
||||
return serial_;
|
||||
}
|
||||
|
||||
virtual bool isCalibrated() const
|
||||
{
|
||||
return calibrated_;
|
||||
}
|
||||
|
||||
virtual bool odomProvided() const
|
||||
{
|
||||
return odomProvided_;
|
||||
}
|
||||
|
||||
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06)
|
||||
{
|
||||
if (odomProvided_)
|
||||
{
|
||||
pose = Transform(1, 0, 0, 0, 0, 0, 1);
|
||||
covariance = cv::Mat::eye(6, 6, CV_64FC1);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Test helpers
|
||||
void setInitResult(bool result) { initResult_ = result; }
|
||||
void setSerial(const std::string & serial) { serial_ = serial; }
|
||||
void setCalibrated(bool calibrated) { calibrated_ = calibrated; }
|
||||
void setOdomProvided(bool provided) { odomProvided_ = provided; }
|
||||
|
||||
bool wasInitCalled() const { return initCalled_; }
|
||||
std::string getCalibrationFolder() const { return calibrationFolder_; }
|
||||
std::string getCameraName() const { return cameraName_; }
|
||||
int getCaptureCount() const { return captureCount_; }
|
||||
|
||||
protected:
|
||||
virtual SensorData captureImage(SensorCaptureInfo * info = 0)
|
||||
{
|
||||
++captureCount_;
|
||||
|
||||
cv::Mat image = cv::Mat::ones(480, 640, CV_8UC3) * 128;
|
||||
SensorData data(image, getNextSeqID(), UTimer::now());
|
||||
return data;
|
||||
}
|
||||
|
||||
private:
|
||||
bool initCalled_;
|
||||
bool initResult_;
|
||||
std::string serial_;
|
||||
bool calibrated_;
|
||||
bool odomProvided_;
|
||||
std::string calibrationFolder_;
|
||||
std::string cameraName_;
|
||||
int captureCount_;
|
||||
};
|
||||
|
||||
// Mock Lidar implementation for testing
|
||||
class MockLidar : public Lidar
|
||||
{
|
||||
public:
|
||||
MockLidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
|
||||
Lidar(lidarRate, localTransform),
|
||||
initCalled_(false),
|
||||
initResult_(true),
|
||||
serial_("MOCK_LIDAR_001"),
|
||||
captureCount_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~MockLidar() {}
|
||||
|
||||
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
|
||||
{
|
||||
initCalled_ = true;
|
||||
calibrationFolder_ = calibrationFolder;
|
||||
cameraName_ = cameraName;
|
||||
return initResult_;
|
||||
}
|
||||
|
||||
virtual std::string getSerial() const
|
||||
{
|
||||
return serial_;
|
||||
}
|
||||
|
||||
// Test helpers
|
||||
void setInitResult(bool result) { initResult_ = result; }
|
||||
void setSerial(const std::string & serial) { serial_ = serial; }
|
||||
|
||||
bool wasInitCalled() const { return initCalled_; }
|
||||
std::string getCalibrationFolder() const { return calibrationFolder_; }
|
||||
std::string getCameraName() const { return cameraName_; }
|
||||
int getCaptureCount() const { return captureCount_; }
|
||||
|
||||
protected:
|
||||
virtual SensorData captureData(SensorCaptureInfo * info = 0)
|
||||
{
|
||||
++captureCount_;
|
||||
|
||||
// Create a simple laser scan
|
||||
LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1, 100, CV_32FC2));
|
||||
SensorData data;
|
||||
data.setLaserScan(scan);
|
||||
data.setStamp(UTimer::now());
|
||||
return data;
|
||||
}
|
||||
|
||||
private:
|
||||
bool initCalled_;
|
||||
bool initResult_;
|
||||
std::string serial_;
|
||||
std::string calibrationFolder_;
|
||||
std::string cameraName_;
|
||||
int captureCount_;
|
||||
};
|
||||
|
||||
// Constructor Tests
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorCameraOnly)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_TRUE(thread.isPaused());
|
||||
EXPECT_EQ(thread.camera(), camera);
|
||||
EXPECT_EQ(thread.lidar(), nullptr);
|
||||
EXPECT_EQ(thread.odomSensor(), nullptr);
|
||||
EXPECT_FALSE(thread.odomProvided());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorLidarOnly)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
SensorCaptureThread thread(lidar);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_TRUE(thread.isPaused());
|
||||
EXPECT_EQ(thread.lidar(), lidar);
|
||||
EXPECT_EQ(thread.camera(), nullptr);
|
||||
EXPECT_EQ(thread.odomSensor(), nullptr);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorLidarWithCamera)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(lidar, camera);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_EQ(thread.lidar(), lidar);
|
||||
EXPECT_EQ(thread.camera(), camera);
|
||||
EXPECT_EQ(thread.odomSensor(), nullptr);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorCameraWithOdomSensor)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
MockCamera * odomSensor = new MockCamera();
|
||||
Transform extrinsics = Transform::getIdentity();
|
||||
|
||||
SensorCaptureThread thread(camera, odomSensor, extrinsics, 0.0, 1.0f, 0.1);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_EQ(thread.camera(), camera);
|
||||
EXPECT_EQ(thread.odomSensor(), odomSensor);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorLidarWithOdomSensor)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
MockCamera * odomSensor = new MockCamera();
|
||||
|
||||
SensorCaptureThread thread(lidar, odomSensor, 0.0, 1.0f, 0.1);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_EQ(thread.lidar(), lidar);
|
||||
EXPECT_EQ(thread.odomSensor(), odomSensor);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ConstructorLidarCameraOdom)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
MockCamera * camera = new MockCamera();
|
||||
MockCamera * odomSensor = new MockCamera();
|
||||
Transform extrinsics = Transform::getIdentity();
|
||||
|
||||
SensorCaptureThread thread(lidar, camera, odomSensor, extrinsics, 0.0, 1.0f, 0.1);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_EQ(thread.lidar(), lidar);
|
||||
EXPECT_EQ(thread.camera(), camera);
|
||||
EXPECT_EQ(thread.odomSensor(), odomSensor);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
// Configuration Tests
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetMirroring)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setMirroringEnabled(true);
|
||||
// No getter, so we can't verify directly, but it shouldn't crash
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetStereoExposureCompensation)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setStereoExposureCompensation(true);
|
||||
// No getter, so we can't verify directly, but it shouldn't crash
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetColorOnly)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setColorOnly(true);
|
||||
// No getter, so we can't verify directly, but it shouldn't crash
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetImageDecimation)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setImageDecimation(2);
|
||||
// No getter, so we can't verify directly, but it shouldn't crash
|
||||
|
||||
thread.setImageDecimation(4);
|
||||
thread.setImageDecimation(1);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetHistogramMethod)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setHistogramMethod(0); // None
|
||||
thread.setHistogramMethod(1); // EqualizeHist
|
||||
thread.setHistogramMethod(2); // CLAHE
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetStereoToDepth)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setStereoToDepth(true);
|
||||
thread.setStereoToDepth(false);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetGetFrameRate)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setFrameRate(30.0f);
|
||||
thread.setFrameRate(0.0f);
|
||||
thread.setFrameRate(60.0f);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetOdomAsGroundTruth)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
MockCamera * odomSensor = new MockCamera();
|
||||
Transform extrinsics = Transform::getIdentity();
|
||||
|
||||
SensorCaptureThread thread(camera, odomSensor, extrinsics);
|
||||
|
||||
thread.setOdomAsGroundTruth(true);
|
||||
thread.setOdomAsGroundTruth(false);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, EnableDisableBilateralFiltering)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.enableBilateralFiltering(5.0f, 50.0f);
|
||||
thread.disableBilateralFiltering();
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, EnableDisableIMUFiltering)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
ParametersMap params;
|
||||
thread.enableIMUFiltering(1, params, false); // Complementary filter
|
||||
thread.disableIMUFiltering();
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, EnableDisableFeatureDetection)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
ParametersMap params;
|
||||
thread.enableFeatureDetection(params);
|
||||
thread.disableFeatureDetection();
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetScanParameters)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
SensorCaptureThread thread(lidar);
|
||||
|
||||
thread.setScanParameters(
|
||||
false, // fromDepth
|
||||
1, // downsampleStep
|
||||
0.0f, // rangeMin
|
||||
10.0f, // rangeMax
|
||||
0.05f, // voxelSize
|
||||
10, // normalsK
|
||||
0.1f, // normalsRadius
|
||||
0.8f, // groundNormalsUp
|
||||
true // deskewing
|
||||
);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, SetScanParametersFromDepth)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
SensorCaptureThread thread(lidar);
|
||||
|
||||
thread.setScanParameters(
|
||||
true, // fromDepth
|
||||
2, // downsampleStep (image decimation)
|
||||
0.0f, // rangeMin
|
||||
0.0f, // rangeMax
|
||||
0.0f, // voxelSize
|
||||
0, // normalsK
|
||||
0.0f, // normalsRadius
|
||||
0.0f, // groundNormalsUp
|
||||
false // deskewing
|
||||
);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
// Thread State Tests
|
||||
|
||||
TEST(SensorCaptureThreadTest, InitialState)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_TRUE(thread.isPaused());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, StartStopThread)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
camera->init(); // Initialize camera before starting thread
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
// Start thread
|
||||
thread.start();
|
||||
|
||||
// Give it a moment to start
|
||||
uSleep(50);
|
||||
|
||||
EXPECT_TRUE(thread.isCapturing());
|
||||
EXPECT_FALSE(thread.isPaused());
|
||||
|
||||
// Stop thread
|
||||
thread.kill();
|
||||
thread.join();
|
||||
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_TRUE(thread.isPaused());
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, OdomProvided)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
MockCamera * odomSensor = new MockCamera();
|
||||
odomSensor->setOdomProvided(true);
|
||||
Transform extrinsics = Transform::getIdentity();
|
||||
|
||||
SensorCaptureThread thread(camera, odomSensor, extrinsics);
|
||||
|
||||
EXPECT_TRUE(thread.odomProvided());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, OdomNotProvided)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
EXPECT_FALSE(thread.odomProvided());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
// Comprehensive Usage Test
|
||||
|
||||
TEST(SensorCaptureThreadTest, ComprehensiveConfiguration)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
ParametersMap params;
|
||||
SensorCaptureThread thread(camera, params);
|
||||
|
||||
// Configure all settings
|
||||
thread.setMirroringEnabled(true);
|
||||
thread.setStereoExposureCompensation(false);
|
||||
thread.setColorOnly(false);
|
||||
thread.setImageDecimation(2);
|
||||
thread.setHistogramMethod(2); // CLAHE
|
||||
thread.setStereoToDepth(false);
|
||||
thread.setFrameRate(30.0f);
|
||||
thread.setOdomAsGroundTruth(false);
|
||||
thread.enableBilateralFiltering(5.0f, 50.0f);
|
||||
|
||||
// Verify thread state
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
EXPECT_TRUE(thread.isPaused());
|
||||
EXPECT_EQ(thread.camera(), camera);
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
// Edge Cases
|
||||
|
||||
TEST(SensorCaptureThreadTest, MultipleConfigurationChanges)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
// Change settings multiple times
|
||||
for (int i = 0; i < 10; ++i)
|
||||
{
|
||||
thread.setImageDecimation(i % 4 + 1);
|
||||
thread.setHistogramMethod(i % 3);
|
||||
thread.setFrameRate((float)(i * 10));
|
||||
}
|
||||
|
||||
// Should not crash
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ZeroFrameRate)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setFrameRate(0.0f); // Unlimited
|
||||
|
||||
// Should handle zero frame rate gracefully
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, VeryHighFrameRate)
|
||||
{
|
||||
MockCamera * camera = new MockCamera();
|
||||
SensorCaptureThread thread(camera);
|
||||
|
||||
thread.setFrameRate(1000.0f); // Very high frame rate
|
||||
|
||||
// Should handle high frame rate gracefully
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
TEST(SensorCaptureThreadTest, ScanParametersEdgeCases)
|
||||
{
|
||||
MockLidar * lidar = new MockLidar();
|
||||
SensorCaptureThread thread(lidar);
|
||||
|
||||
// Test with all zeros (disabled features)
|
||||
thread.setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, false);
|
||||
|
||||
// Test with maximum values
|
||||
thread.setScanParameters(false, 10, 100.0f, 1000.0f, 1.0f, 100, 5.0f, 1.0f, true);
|
||||
|
||||
// Should not crash
|
||||
EXPECT_FALSE(thread.isCapturing());
|
||||
|
||||
// Thread was never started, destructor will handle cleanup
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user