214 lines
6.4 KiB
C++
214 lines
6.4 KiB
C++
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <rs_driver/driver/decoder/decoder_RS32.hpp>
|
|
#include <rs_driver/msg/point_cloud_msg.hpp>
|
|
#include <rs_driver/utility/dbg.hpp>
|
|
|
|
using namespace robosense::lidar;
|
|
|
|
typedef PointXYZIRT PointT;
|
|
typedef PointCloudT<PointT> PointCloud;
|
|
|
|
static ErrCode errCode = ERRCODE_SUCCESS;
|
|
static void errCallback(const Error& err)
|
|
{
|
|
errCode = err.error_code;
|
|
}
|
|
|
|
TEST(TestDecoderRS32, getEchoMode)
|
|
{
|
|
ASSERT_TRUE(DecoderRS32<PointCloud>::getEchoMode(0) == RSEchoMode::ECHO_DUAL);
|
|
ASSERT_TRUE(DecoderRS32<PointCloud>::getEchoMode(1) == RSEchoMode::ECHO_SINGLE);
|
|
ASSERT_TRUE(DecoderRS32<PointCloud>::getEchoMode(2) == RSEchoMode::ECHO_SINGLE);
|
|
}
|
|
|
|
TEST(TestDecoderRS32, RS32DifopPkt2Adapter)
|
|
{
|
|
RSCalibrationAngle v_angle_cali[32] =
|
|
{
|
|
0x01, htons(0x2829), // -10.281
|
|
0x00, htons(0x091d) // 2.333
|
|
};
|
|
|
|
RSCalibrationAngle h_angle_cali[32] =
|
|
{
|
|
0x00, htons(0x01f4), // 0.5
|
|
0x01, htons(0x01c2) // -0.45
|
|
};
|
|
|
|
RS32DifopPkt src;
|
|
src.rpm = 0;
|
|
src.fov = {0};
|
|
src.return_mode = {0};
|
|
src.sn = {0};
|
|
src.eth = {0};
|
|
src.version = {0};
|
|
src.status = {0};
|
|
|
|
memcpy (src.vert_angle_cali, v_angle_cali, 32*sizeof(RSCalibrationAngle));
|
|
memcpy (src.horiz_angle_cali, h_angle_cali, 32*sizeof(RSCalibrationAngle));
|
|
|
|
AdapterDifopPkt dst;
|
|
RS32DifopPkt2Adapter(src, dst);
|
|
|
|
ASSERT_EQ(dst.vert_angle_cali[0].sign, 1);
|
|
ASSERT_EQ(ntohs(dst.vert_angle_cali[0].value), 1028);
|
|
ASSERT_EQ(dst.vert_angle_cali[1].sign, 0);
|
|
ASSERT_EQ(ntohs(dst.vert_angle_cali[1].value), 233);
|
|
|
|
ASSERT_EQ(dst.horiz_angle_cali[0].sign, 0);
|
|
ASSERT_EQ(ntohs(dst.horiz_angle_cali[0].value), 50);
|
|
ASSERT_EQ(dst.horiz_angle_cali[1].sign, 1);
|
|
ASSERT_EQ(ntohs(dst.horiz_angle_cali[1].value), 45);
|
|
}
|
|
|
|
TEST(TestDecoderRS32, decodeDifopPkt)
|
|
{
|
|
// const_param
|
|
RSDecoderParam param;
|
|
DecoderRS32<PointCloud> decoder(param);
|
|
decoder.regCallback(errCallback, nullptr);
|
|
ASSERT_EQ(decoder.blks_per_frame_, 1801);
|
|
ASSERT_EQ(decoder.split_blks_per_frame_, 1801);
|
|
|
|
// rpm = 600, dual return
|
|
RS32DifopPkt pkt;
|
|
pkt.rpm = htons(600);
|
|
pkt.return_mode = 0;
|
|
decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt));
|
|
ASSERT_EQ(decoder.rps_, 10);
|
|
ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_DUAL);
|
|
ASSERT_EQ(decoder.blks_per_frame_, 1801);
|
|
ASSERT_EQ(decoder.split_blks_per_frame_, 3602);
|
|
|
|
// rpm = 1200, single return
|
|
pkt.rpm = htons(1200);
|
|
pkt.return_mode = 1;
|
|
decoder.decodeDifopPkt((uint8_t*)&pkt, sizeof(pkt));
|
|
ASSERT_EQ(decoder.rps_, 20);
|
|
ASSERT_EQ(decoder.echo_mode_, RSEchoMode::ECHO_SINGLE);
|
|
ASSERT_EQ(decoder.blks_per_frame_, 900);
|
|
ASSERT_EQ(decoder.split_blks_per_frame_, 900);
|
|
}
|
|
|
|
static void splitFrame(uint16_t height, double ts)
|
|
{
|
|
}
|
|
|
|
TEST(TestDecoderRS32, decodeMsopPkt)
|
|
{
|
|
uint8_t pkt[] =
|
|
{
|
|
//
|
|
// header
|
|
//
|
|
0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0, // msop id
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_1
|
|
0x15, 0x0a, 0x01, 0x01, 0x02, 0x03, 0x11, 0x22, 0x33, 0x44, // ts_YMD
|
|
0x00, // lidar type
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // reserved_2
|
|
0x18, 0x01, // temprature
|
|
0x00, 0x00, // reserved_3
|
|
|
|
//
|
|
// block_01
|
|
//
|
|
0xFF, 0xEE, // block id
|
|
0x00, 0x00, // azimuth
|
|
0x03, 0xE8, // chan_00, distance
|
|
0x01, // chan_00, intensity
|
|
0x00, 0x00, // chan_01, distance
|
|
0x00, // chan_01, intensity
|
|
0x00, 0x00, // chan_02, distance
|
|
0x00, // chan_02, intensity
|
|
0x00, 0x00, // chan_03, distance
|
|
0x00, // chan_03, intensity
|
|
0x00, 0x00, // chan_04, distance
|
|
0x00, // chan_04, intensity
|
|
0x00, 0x00, // chan_05, distance
|
|
0x00, // chan_05, intensity
|
|
0x00, 0x00, // chan_06, distance
|
|
0x00, // chan_06, intensity
|
|
0x00, 0x00, // chan_07, distance
|
|
0x00, // chan_07, intensity
|
|
0x00, 0x00, // chan_08, distance
|
|
0x00, // chan_08, intensity
|
|
0x00, 0x00, // chan_09, distance
|
|
0x00, // chan_09, intensity
|
|
0x00, 0x00, // chan_10, distance
|
|
0x00, // chan_10, intensity
|
|
0x00, 0x00, // chan_11, distance
|
|
0x00, // chan_11, intensity
|
|
0x00, 0x00, // chan_12, distance
|
|
0x00, // chan_12, intensity
|
|
0x00, 0x00, // chan_13, distance
|
|
0x00, // chan_13, intensity
|
|
0x00, 0x00, // chan_14, distance
|
|
0x00, // chan_14, intensity
|
|
0x00, 0x00, // chan_15, distance
|
|
0x00, // chan_15, intensity
|
|
0x00, 0x00, // chan_16, distance
|
|
0x00, // chan_16, intensity
|
|
0x00, 0x00, // chan_17, distance
|
|
0x00, // chan_17, intensity
|
|
0x00, 0x00, // chan_18, distance
|
|
0x00, // chan_18, intensity
|
|
0x00, 0x00, // chan_19, distance
|
|
0x00, // chan_19, intensity
|
|
0x00, 0x00, // chan_20, distance
|
|
0x00, // chan_20, intensity
|
|
0x00, 0x00, // chan_21, distance
|
|
0x00, // chan_21, intensity
|
|
0x00, 0x00, // chan_22, distance
|
|
0x00, // chan_22, intensity
|
|
0x00, 0x00, // chan_23, distance
|
|
0x00, // chan_23, intensity
|
|
0x00, 0x00, // chan_24, distance
|
|
0x00, // chan_24, intensity
|
|
0x00, 0x00, // chan_25, distance
|
|
0x00, // chan_25, intensity
|
|
0x00, 0x00, // chan_26, distance
|
|
0x00, // chan_26, intensity
|
|
0x00, 0x00, // chan_27, distance
|
|
0x00, // chan_27, intensity
|
|
0x00, 0x00, // chan_28, distance
|
|
0x00, // chan_28, intensity
|
|
0x00, 0x00, // chan_29, distance
|
|
0x00, // chan_29, intensity
|
|
0x00, 0x00, // chan_30, distance
|
|
0x00, // chan_30, intensity
|
|
0x00, 0x00, // chan_31, distance
|
|
0x00, // chan_31, intensity
|
|
|
|
//
|
|
// block_02
|
|
//
|
|
0x00, 0x00, // block id
|
|
};
|
|
|
|
// dense_points = false, use_lidar_clock = true
|
|
RSDecoderParam param;
|
|
DecoderRS32<PointCloud> decoder(param);
|
|
decoder.regCallback(errCallback, splitFrame);
|
|
ASSERT_EQ(decoder.chan_angles_.user_chans_.size(), 32);
|
|
decoder.chan_angles_.user_chans_[0] = 2;
|
|
decoder.chan_angles_.user_chans_[1] = 1;
|
|
decoder.param_.dense_points = false;
|
|
decoder.param_.use_lidar_clock = true;
|
|
|
|
decoder.point_cloud_ = std::make_shared<PointCloud>();
|
|
|
|
decoder.decodeMsopPkt(pkt, sizeof(pkt));
|
|
float temp = 0.0f;
|
|
ASSERT_EQ(decoder.getTemperature(temp), true);
|
|
ASSERT_EQ(temp, 2.1875);
|
|
ASSERT_EQ(decoder.point_cloud_->points.size(), 32);
|
|
|
|
PointT& point = decoder.point_cloud_->points[0];
|
|
ASSERT_EQ(point.intensity, 1);
|
|
ASSERT_NE(point.timestamp, 0);
|
|
ASSERT_EQ(point.ring, 2);
|
|
}
|
|
|