224 lines
6.4 KiB
C++
224 lines
6.4 KiB
C++
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <rs_driver/driver/decoder/chan_angles.hpp>
|
|
|
|
using namespace robosense::lidar;
|
|
|
|
TEST(TestChanAngles, genUserChan)
|
|
{
|
|
std::vector<int32_t> vert_angles;
|
|
std::vector<uint16_t> user_chans;
|
|
|
|
vert_angles.push_back(100);
|
|
vert_angles.push_back(0);
|
|
vert_angles.push_back(-100);
|
|
vert_angles.push_back(200);
|
|
|
|
ChanAngles::genUserChan (vert_angles, user_chans);
|
|
ASSERT_EQ(user_chans.size(), 4);
|
|
ASSERT_EQ(user_chans[0], 2);
|
|
ASSERT_EQ(user_chans[1], 1);
|
|
ASSERT_EQ(user_chans[2], 0);
|
|
ASSERT_EQ(user_chans[3], 3);
|
|
}
|
|
|
|
TEST(TestChanAngles, loadFromFile)
|
|
{
|
|
std::vector<int32_t> vert_angles, horiz_angles;
|
|
|
|
// load
|
|
ASSERT_EQ(ChanAngles::loadFromFile ("../test/res/angle.csv", 4, vert_angles, horiz_angles), 0);
|
|
ASSERT_EQ(vert_angles.size(), 4);
|
|
ASSERT_EQ(horiz_angles.size(), 4);
|
|
ASSERT_EQ(vert_angles[0], 500);
|
|
ASSERT_EQ(vert_angles[1], 250);
|
|
ASSERT_EQ(vert_angles[2], 0);
|
|
ASSERT_EQ(vert_angles[3], -250);
|
|
|
|
ASSERT_EQ(horiz_angles[0], 10);
|
|
ASSERT_EQ(horiz_angles[1], -20);
|
|
ASSERT_EQ(horiz_angles[2], 0);
|
|
ASSERT_EQ(horiz_angles[3], -100);
|
|
|
|
// load again
|
|
ASSERT_EQ(ChanAngles::loadFromFile ("../test/res/angle.csv", 4, vert_angles, horiz_angles), 0);
|
|
ASSERT_EQ(vert_angles.size(), 4);
|
|
ASSERT_EQ(horiz_angles.size(), 4);
|
|
|
|
// load non-existing file
|
|
ASSERT_LT(ChanAngles::loadFromFile ("../test/res/non_exist.csv", 4, vert_angles, horiz_angles), 0);
|
|
ASSERT_EQ(vert_angles.size(), 0);
|
|
ASSERT_EQ(horiz_angles.size(), 0);
|
|
}
|
|
|
|
TEST(TestChanAngles, loadFromDifop)
|
|
{
|
|
uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02,
|
|
0x01, 0x03, 0x04,
|
|
0x01, 0x05, 0x06,
|
|
0x00, 0x07, 0x08};
|
|
uint8_t horiz_angle_arr[] = {0x00, 0x01, 0x11,
|
|
0x01, 0x02, 0x22,
|
|
0x00, 0x03, 0x33,
|
|
0x01, 0x04, 0x44};
|
|
|
|
std::vector<int32_t> vert_angles, horiz_angles;
|
|
|
|
// load
|
|
ASSERT_EQ(ChanAngles::loadFromDifop(
|
|
(const RSCalibrationAngle*)vert_angle_arr,
|
|
(const RSCalibrationAngle*)horiz_angle_arr,
|
|
4,
|
|
vert_angles,
|
|
horiz_angles), 0);
|
|
|
|
ASSERT_EQ(vert_angles.size(), 4);
|
|
ASSERT_EQ(horiz_angles.size(), 4);
|
|
ASSERT_EQ(vert_angles[0], 258);
|
|
ASSERT_EQ(vert_angles[1], -772);
|
|
ASSERT_EQ(vert_angles[2], -1286);
|
|
ASSERT_EQ(vert_angles[3], 1800);
|
|
|
|
ASSERT_EQ(horiz_angles[0], 273);
|
|
ASSERT_EQ(horiz_angles[1], -546);
|
|
ASSERT_EQ(horiz_angles[2], 819);
|
|
ASSERT_EQ(horiz_angles[3], -1092);
|
|
|
|
// load again
|
|
ASSERT_EQ(ChanAngles::loadFromDifop(
|
|
(const RSCalibrationAngle*)vert_angle_arr,
|
|
(const RSCalibrationAngle*)horiz_angle_arr,
|
|
4,
|
|
vert_angles,
|
|
horiz_angles), 0);
|
|
ASSERT_EQ(vert_angles.size(), 4);
|
|
ASSERT_EQ(horiz_angles.size(), 4);
|
|
}
|
|
|
|
TEST(TestChanAngles, memberLoadFromFile)
|
|
{
|
|
ChanAngles angles(4);
|
|
|
|
// not loading yet
|
|
ASSERT_EQ(angles.chan_num_, 4);
|
|
ASSERT_EQ(angles.vert_angles_.size(), 4);
|
|
ASSERT_EQ(angles.horiz_angles_.size(), 4);
|
|
ASSERT_EQ(angles.user_chans_.size(), 4);
|
|
|
|
// load
|
|
ASSERT_EQ(angles.loadFromFile ("../test/res/angle.csv"), 0);
|
|
ASSERT_EQ(angles.user_chans_.size(), 4);
|
|
ASSERT_EQ(angles.toUserChan(0), 3);
|
|
ASSERT_EQ(angles.toUserChan(1), 2);
|
|
ASSERT_EQ(angles.toUserChan(2), 1);
|
|
ASSERT_EQ(angles.toUserChan(3), 0);
|
|
}
|
|
|
|
TEST(TestChanAngles, memberLoadFromFile_fail)
|
|
{
|
|
ChanAngles angles(4);
|
|
ASSERT_EQ(angles.chan_num_, 4);
|
|
|
|
// load non-existing file
|
|
ASSERT_LT(angles.loadFromFile ("../test/res/non_exist.csv"), 0);
|
|
ASSERT_EQ(angles.vert_angles_.size(), 4);
|
|
ASSERT_EQ(angles.vert_angles_[0], 0);
|
|
}
|
|
|
|
TEST(TestChanAngles, memberLoadFromDifop)
|
|
{
|
|
uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02,
|
|
0x01, 0x03, 0x04,
|
|
0x01, 0x05, 0x06,
|
|
0x00, 0x07, 0x08};
|
|
uint8_t horiz_angle_arr[] = {0x00, 0x01, 0x11,
|
|
0x01, 0x02, 0x22,
|
|
0x00, 0x03, 0x33,
|
|
0x01, 0x04, 0x44};
|
|
|
|
ChanAngles angles(4);
|
|
ASSERT_EQ(angles.chan_num_, 4);
|
|
|
|
// load
|
|
ASSERT_EQ(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0);
|
|
|
|
ASSERT_EQ(angles.vert_angles_.size(), 4);
|
|
ASSERT_EQ(angles.vert_angles_[0], 258);
|
|
ASSERT_EQ(angles.vert_angles_[1], -772);
|
|
ASSERT_EQ(angles.vert_angles_[2], -1286);
|
|
ASSERT_EQ(angles.vert_angles_[3], 1800);
|
|
|
|
ASSERT_EQ(angles.horiz_angles_[0], 273);
|
|
ASSERT_EQ(angles.horiz_angles_[1], -546);
|
|
ASSERT_EQ(angles.horiz_angles_[2], 819);
|
|
ASSERT_EQ(angles.horiz_angles_[3], -1092);
|
|
|
|
ASSERT_EQ(angles.user_chans_.size(), 4);
|
|
ASSERT_EQ(angles.toUserChan(0), 2);
|
|
ASSERT_EQ(angles.toUserChan(1), 1);
|
|
ASSERT_EQ(angles.toUserChan(2), 0);
|
|
ASSERT_EQ(angles.toUserChan(3), 3);
|
|
}
|
|
|
|
TEST(TestChanAngles, memberLoadFromDifop_fail)
|
|
{
|
|
uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02,
|
|
0x01, 0x03, 0x04,
|
|
0xFF, 0x05, 0x06,
|
|
0xFF, 0x07, 0x08};
|
|
uint8_t horiz_angle_arr[] = {0x00, 0x11, 0x22,
|
|
0x01, 0x33, 0x44,
|
|
0xFF, 0x55, 0x66,
|
|
0xFF, 0x77, 0x88};
|
|
|
|
ChanAngles angles(4);
|
|
ASSERT_EQ(angles.chan_num_, 4);
|
|
|
|
// load invalid difop
|
|
ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0);
|
|
ASSERT_EQ(angles.vert_angles_.size(), 4);
|
|
ASSERT_EQ(angles.vert_angles_[0], 0);
|
|
}
|
|
|
|
TEST(TestChanAngles, memberLoadFromDifop_fail_angle)
|
|
{
|
|
uint8_t vert_angle_arr[] = {0x00, 0x01, 0x02,
|
|
0x01, 0x03, 0x04,
|
|
0x01, 0x05, 0x06,
|
|
0x00, 0x07, 0x08};
|
|
|
|
// -9000 <= angle < 18000
|
|
ChanAngles angles(4);
|
|
ASSERT_EQ(angles.chan_num_, 4);
|
|
|
|
{
|
|
// 9000
|
|
uint8_t horiz_angle_arr[] =
|
|
{
|
|
0x00, 0x01, 0x11,
|
|
0x01, 0x02, 0x22,
|
|
0x00, 0x03, 0x33,
|
|
0x00, 0x46, 0x52 //18000
|
|
};
|
|
|
|
// load
|
|
ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0);
|
|
}
|
|
|
|
{
|
|
// -9001
|
|
uint8_t horiz_angle_arr[] =
|
|
{
|
|
0x00, 0x01, 0x11,
|
|
0x01, 0x02, 0x22,
|
|
0x00, 0x03, 0x33,
|
|
0x01, 0x23, 0x29
|
|
};
|
|
|
|
// load
|
|
ASSERT_LT(angles.loadFromDifop((const RSCalibrationAngle*)vert_angle_arr, (const RSCalibrationAngle*)horiz_angle_arr), 0);
|
|
}
|
|
}
|
|
|