apollo::drivers::lslidar::parser::LslidarCXV4Parser类 参考

#include <lslidar_parser.h>

类 apollo::drivers::lslidar::parser::LslidarCXV4Parser 继承关系图:

apollo::drivers::lslidar::parser::LslidarCXV4Parser 的协作图:

Public 成员函数
	LslidarCXV4Parser (const Config &config)
	~LslidarCXV4Parser ()
void	decodePacket (const RawPacket_C32 *packet)
void	GeneratePointcloud (const std::shared_ptr< apollo::drivers::lslidar::LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
void	Order (std::shared_ptr< apollo::drivers::PointCloud > cloud)
bool	checkPacketValidity (const RawPacket_C32 *packet)
Public 成员函数 继承自 apollo::drivers::lslidar::parser::LslidarParser
	LslidarParser ()
	LslidarParser (const Config &config)
virtual	~LslidarParser ()
virtual void	GeneratePointcloud (const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)=0
	Set up for data processing.
virtual void	setup ()
	Set up for on-line operation.
const double	get_last_timestamp ()

额外继承的成员函数
Protected 成员函数 继承自 apollo::drivers::lslidar::parser::LslidarParser
bool	is_scan_valid (int rotation, float distance)
void	ComputeCoords (const float &raw_distance, LaserCorrection *corrections, const uint16_t rotation, PointXYZIT *point)
void	ComputeCoords2 (int Laser_ring, int Type, const float &raw_distance, LaserCorrection *corrections, const double rotation, PointXYZIT *point)
Protected 属性 继承自 apollo::drivers::lslidar::parser::LslidarParser
double	scan_altitude [16]
float	sin_azimuth_table [ROTATION_MAX_UNITS]
float	cos_azimuth_table [ROTATION_MAX_UNITS]
double	cos_scan_altitude_caliration [LSC32_SCANS_PER_FIRING]
double	sin_scan_altitude_caliration [LSC32_SCANS_PER_FIRING]
double	last_time_stamp_
double	theat1_s [128]
double	theat2_s [128]
double	theat1_c [128]
double	theat2_c [128]
double	prism_angle [4]
double	prism_angle64 [4]
Config	config_
Calibration	calibration_
bool	need_two_pt_correction_
bool	config_vert_angle
uint64_t	current_packet_time = 0
uint64_t	previous_packet_time = 0

详细描述

在文件 lslidar_parser.h 第 623 行定义.

构造及析构函数说明

LslidarCXV4Parser()

apollo::drivers::lslidar::parser::LslidarCXV4Parser::LslidarCXV4Parser ( const Config &  config )

explicit

在文件 lslidarCXV4_parser.cc 第 24 行定义.

                                                         :
        LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {
    vertical_angle_ = config.vertical_angle();
    distance_unit_ = config.distance_unit();
    is_new_c32w_ = true;
    is_get_scan_altitude_ = false;
    AERROR << "vertical_angle_ " << vertical_angle_;
    if (config_.model() == LSLIDAR_C16_V4) {
        lidar_number_ = 16;
        AERROR << "lidar: c16";
        for (int i = 0; i < 16; ++i) {
            sin_scan_altitude[i] = sin(c16_vertical_angle[i] * DEG_TO_RAD);
            cos_scan_altitude[i] = cos(c16_vertical_angle[i] * DEG_TO_RAD);
        }
    } else if (config_.model() == LSLIDAR_C8_V4) {
        lidar_number_ = 8;
        AERROR << "lidar: c8";
        for (int i = 0; i < 8; ++i) {
            sin_scan_altitude[i] = sin(c8_vertical_angle[i] * DEG_TO_RAD);
            cos_scan_altitude[i] = cos(c8_vertical_angle[i] * DEG_TO_RAD);
        }
    } else if (config_.model() == LSLIDAR_C1_V4) {
        lidar_number_ = 1;
        AERROR << "lidar: c1";
        for (int i = 0; i < 8; ++i) {
            sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD);
            cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD);
        }
    } else if (config_.model() == LSLIDAR_C32_V4) {
        lidar_number_ = 32;
        if (32 == config.vertical_angle()) {
            AERROR << "Vertical angle: 32 degrees";
            for (int i = 0; i < 32; ++i) {
                sin_scan_altitude[i]
                        = sin(c32_32_vertical_angle[i] * DEG_TO_RAD);
                cos_scan_altitude[i]
                        = cos(c32_32_vertical_angle[i] * DEG_TO_RAD);
            }
        } else if (70 == config.vertical_angle()) {
            AERROR << "Vertical angle: 70 degrees";
            for (int k = 0; k < 32; ++k) {
                sin_scan_altitude[k]
                        = sin(c32_70_vertical_angle2[k] * DEG_TO_RAD);
                cos_scan_altitude[k]
                        = cos(c32_70_vertical_angle2[k] * DEG_TO_RAD);
            }
        } else if (90 == config.vertical_angle()) {
            AERROR << "Vertical angle: 90 degrees";
            for (int k = 0; k < 32; ++k) {
                sin_scan_altitude[k]
                        = sin(c32_90_vertical_angle[k] * DEG_TO_RAD);
                cos_scan_altitude[k]
                        = cos(c32_90_vertical_angle[k] * DEG_TO_RAD);
            }
        }
    }
 
    // create the sin and cos table for different azimuth values
    for (int j = 0; j < 36000; ++j) {
        double angle = static_cast<double>(j) / 100.0 * DEG_TO_RAD;
        sin_azimuth_table[j] = sin(angle);
        cos_azimuth_table[j] = cos(angle);
    }
    config_vert_angle = false;
    lslidar_type = 2;
}

~LslidarCXV4Parser()

apollo::drivers::lslidar::parser::LslidarCXV4Parser::~LslidarCXV4Parser ( )

inline

在文件 lslidar_parser.h 第 626 行定义.

{}

成员函数说明

checkPacketValidity()

bool apollo::drivers::lslidar::parser::LslidarCXV4Parser::checkPacketValidity

(

const RawPacket_C32 *

packet

)

在文件 lslidarCXV4_parser.cc 第 158 行定义.

                                                                       {
    for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
        if (packet->blocks[blk_idx].header != UPPER_BANK) {
            return false;
        }
    }
    return true;
}

decodePacket()

void apollo::drivers::lslidar::parser::LslidarCXV4Parser::decodePacket

(

const RawPacket_C32 *

packet

)

在文件 lslidarCXV4_parser.cc 第 91 行定义.

                                                                {
    // couputer azimuth angle for each firing, 12
    for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) {
        firings.firing_azimuth[b_idx] = packet->blocks[b_idx].rotation
                % 36000;  //* 0.01 * DEG_TO_RAD;
    }
    for (size_t block_idx = 0; block_idx < BLOCKS_PER_PACKET; ++block_idx) {
        const RawBlock &raw_block = packet->blocks[block_idx];
 
        int32_t azimuth_diff_b = 0;
        if (config_.return_mode() == 1) {
            if (block_idx < BLOCKS_PER_PACKET - 1) {
                azimuth_diff_b = firings.firing_azimuth[block_idx + 1]
                        - firings.firing_azimuth[block_idx];
                azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000
                                                    : azimuth_diff_b;
 
            } else {
                azimuth_diff_b = firings.firing_azimuth[block_idx]
                        - firings.firing_azimuth[block_idx - 1];
 
                azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000
                                                    : azimuth_diff_b;
            }
        } else {
            // return mode 2
            if (block_idx < BLOCKS_PER_PACKET - 2) {
                azimuth_diff_b = firings.firing_azimuth[block_idx + 2]
                        - firings.firing_azimuth[block_idx];
                azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000
                                                    : azimuth_diff_b;
 
            } else {
                azimuth_diff_b = firings.firing_azimuth[block_idx]
                        - firings.firing_azimuth[block_idx - 2];
 
                azimuth_diff_b = azimuth_diff_b < 0 ? azimuth_diff_b + 36000
                                                    : azimuth_diff_b;
            }
        }
 
        // 32 scan
        for (size_t scan_fir_idx = 0; scan_fir_idx < SCANS_PER_FIRING_C32;
             ++scan_fir_idx) {
            size_t byte_idx = RAW_SCAN_SIZE * scan_fir_idx;
            // azimuth
            firings.azimuth[block_idx * 32 + scan_fir_idx]
                    = firings.firing_azimuth[block_idx]
                    + scan_fir_idx * azimuth_diff_b / FIRING_TOFFSET_C32;
            firings.azimuth[block_idx * 32 + scan_fir_idx]
                    = firings.azimuth[block_idx * 32 + scan_fir_idx] % 36000;
            // distance
            TwoBytes raw_distance{};
            raw_distance.bytes[0] = raw_block.data[byte_idx];
            raw_distance.bytes[1] = raw_block.data[byte_idx + 1];
            firings.distance[block_idx * 32 + scan_fir_idx]
                    = static_cast<double>(raw_distance.distance)
                    * DISTANCE_RESOLUTION * distance_unit_;
 
            // intensity
            firings.intensity[block_idx * 32 + scan_fir_idx]
                    = static_cast<double>(raw_block.data[byte_idx + 2]);
        }
    }
    return;
}

GeneratePointcloud()

void apollo::drivers::lslidar::parser::LslidarCXV4Parser::GeneratePointcloud	(	const std::shared_ptr< apollo::drivers::lslidar::LslidarScan > &	scan_msg,
		const std::shared_ptr< apollo::drivers::PointCloud > &	out_msg
	)

在文件 lslidarCXV4_parser.cc 第 167 行定义.

                                                  {
    // allocate a point cloud with same time and frame ID as raw data
    out_msg->mutable_header()->set_timestamp_sec(
            scan_msg->basetime() / 1000000000.0);
    out_msg->mutable_header()->set_module_name(
            scan_msg->header().module_name());
    out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
    out_msg->set_height(1);
    out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
    out_msg->mutable_header()->set_sequence_num(
            scan_msg->header().sequence_num());
    gps_base_usec_ = scan_msg->basetime();
 
    const unsigned char *difop_ptr
            = (const unsigned char *)scan_msg->difop_pkts(0).data().c_str();
    if (difop_ptr[0] == 0xa5 && difop_ptr[1] == 0xff && difop_ptr[2] == 0x00
        && difop_ptr[3] == 0x5a) {
        AINFO << "设备包，暂时用不上...";  // todo 暂时不用设备包
    }
 
    size_t packets_size = scan_msg->firing_pkts_size();
    block_num = 0;
    packet_number_ = packets_size;
 
    AINFO << "packets_size :" << packets_size;
 
    for (size_t i = 0; i < packets_size; ++i) {
        Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg, i);
        last_time_stamp_ = out_msg->measurement_time();
        ADEBUG << "stamp: " << std::fixed << last_time_stamp_;
    }
 
    if (out_msg->point().empty()) {
        // we discard this pointcloud if empty
        AERROR << "All points is NAN!Please check lslidar:" << config_.model();
    }
 
    // set default width
    out_msg->set_width(out_msg->point_size());
}

Order()

void apollo::drivers::lslidar::parser::LslidarCXV4Parser::Order ( std::shared_ptr< apollo::drivers::PointCloud >  cloud )

virtual

实现了 apollo::drivers::lslidar::parser::LslidarParser.

在文件 lslidarCXV4_parser.cc 第 364 行定义.

                                                             {
    int width = 32;
    cloud->set_width(width);
    int height = cloud->point_size() / cloud->width();
    cloud->set_height(height);
 
    std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
    cloud_origin->CopyFrom(*cloud);
 
    for (int i = 0; i < width; ++i) {
        int col = ORDER_32[i];
 
        for (int j = 0; j < height; ++j) {
            // make sure offset is initialized, should be init at setup() just
            // once
            int target_index = j * width + i;
            int origin_index = j * width + col;
            cloud->mutable_point(target_index)
                    ->CopyFrom(cloud_origin->point(origin_index));
        }
    }
}

---

该类的文档由以下文件生成:

• modules/drivers/lidar/lslidar/parser/lslidar_parser.h
• modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc