diff --git a/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt index a50e65ad037..a12ca204762 100644 --- a/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt +++ b/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt @@ -1,7 +1,7 @@ model: LSLIDAR16P -device_ip: "192.168.1.200" -msop_port: 2368 -difop_port: 2369 +device_ip: "192.168.1.201" +msop_port: 2370 +difop_port: 2371 return_mode: 1 degree_mode: 2 #2: 均匀2度校准两列 1://均匀1.33度校准两列 distance_unit: 0.25 diff --git a/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt index 369c5c2cd18..9b96e476029 100644 --- a/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt +++ b/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt @@ -9,7 +9,7 @@ packet_size: 1206 time_synchronization: false add_multicast: false group_ip: "224.1.1.2" -rpm: 600 #雷达转速 10hz:600rpm, 20hz:1200rpm, 5hz:300rpm +rpm: 300 #雷达转速 10hz:600rpm, 20hz:1200rpm, 5hz:300rpm convert_channel_name: "/apollo/sensor/lslidar32/PointCloud2" frame_id: "lslidar32" scan_channel: "/apollo/sensor/lslidar32/Scan" diff --git a/modules/drivers/lidar/lslidar/driver/driver.cc b/modules/drivers/lidar/lslidar/driver/driver.cc index 1333c33cdd0..fd5825b6cd9 100755 --- a/modules/drivers/lidar/lslidar/driver/driver.cc +++ b/modules/drivers/lidar/lslidar/driver/driver.cc @@ -16,458 +16,497 @@ #include "modules/drivers/lidar/lslidar/driver/driver.h" -#include -#include -#include -#include - -#include "cyber/cyber.h" -#include "modules/drivers/lidar/lslidar/proto/config.pb.h" -#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" - namespace apollo { - namespace drivers { - namespace lslidar { +namespace drivers { +namespace lslidar { - LslidarDriver::~LslidarDriver() { - if (difop_thread_.joinable()) { - difop_thread_.join(); - } - } +LslidarDriver::~LslidarDriver() { + if (difop_thread_.joinable()) { + difop_thread_.join(); + } +} - void LslidarDriver::Init() { - int packets_rate; +void LslidarDriver::Init() { + int packets_rate; - if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR_C16_V4) { - packets_rate = 840; - } else if (config_.model() == LSLIDAR_C8_V4) { - packets_rate = 420; - } else if (config_.model() == LSLIDAR_C1_V4) { - packets_rate = 420; - } else if (config_.model() == LSLIDAR_C32_V4) { - packets_rate = 1693; //64000/384 - } else if (config_.model() == LSLIDAR32P) { - if (1 == config_.degree_mode()) - packets_rate = 1693; //65000/384 - if (2 == config_.degree_mode()) - packets_rate = 1700; //64000/384 - } else if (config_.model() == LSLIDAR_CH16) - packets_rate = 3571; - else if (config_.model() == LSLIDAR_CH32) - packets_rate = 3512; - else if (config_.model() == LSLIDAR_CH64) - packets_rate = 3388; - else if (config_.model() == LSLIDAR_CH64w) - packets_rate = 11228; - else if (config_.model() == LSLIDAR_CH128) - packets_rate = 3571; - else if (config_.model() == LSLIDAR_CH128X1) - packets_rate = 6720; - else if (config_.model() == LSLIDAR_LS128S2) - packets_rate = 12440; - else - packets_rate = 4561; + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR_C16_V4) { + packets_rate = 840; + } else if (config_.model() == LSLIDAR_C8_V4) { + packets_rate = 420; + } else if (config_.model() == LSLIDAR_C1_V4) { + packets_rate = 420; + } else if (config_.model() == LSLIDAR_C32_V4) { + packets_rate = 1693; // 64000/384 + } else if (config_.model() == LSLIDAR32P) { + if (1 == config_.degree_mode()) packets_rate = 1693; // 65000/384 + if (2 == config_.degree_mode()) packets_rate = 1700; // 64000/384 + } else if (config_.model() == LSLIDAR_CH16) { + packets_rate = 3571; + } else if (config_.model() == LSLIDAR_CH32) { + packets_rate = 3512; + } else if (config_.model() == LSLIDAR_CH64) { + packets_rate = 3388; + } else if (config_.model() == LSLIDAR_CH64w) { + packets_rate = 11228; + } else if (config_.model() == LSLIDAR_CH128) { + packets_rate = 3571; + } else if (config_.model() == LSLIDAR_CH128X1) { + packets_rate = 6720; + } else if (config_.model() == LSLIDAR_LS128S2) { + packets_rate = 12440; + } else { + packets_rate = 4561; + } - // default number of packets for each scan is a single revolution - if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || config_.model() == LSLIDAR_C32_V4 - || config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || - config_.model() == LSLIDAR_C1_V4) - config_.set_npackets( - static_cast(ceil(packets_rate * 60 / config_.rpm()) * config_.return_mode() * 1.1)); - else - config_.set_npackets(static_cast(ceil(packets_rate * 60 / config_.rpm() * 1.1))); + // default number of packets for each scan is a single revolution + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) + config_.set_npackets(static_cast( + ceil(packets_rate * 60 / config_.rpm()) * config_.return_mode() * 1.1)); + else + config_.set_npackets( + static_cast(ceil(packets_rate * 60 / config_.rpm() * 1.1))); - AERROR << "config_.pcap_path(): " << config_.pcap_path(); + AERROR << "config_.pcap_path(): " << config_.pcap_path(); - if (!config_.pcap_path().empty()) { - if (config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || - config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) { - // open Lslidar input device + if (!config_.pcap_path().empty()) { + if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + // open Lslidar input device - input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), config_.packet_size(), - packets_rate, config_.pcap_path())); //数据包 - positioning_input_.reset(new InputPCAP(config_.difop_port(), config_.device_ip(), 1206, 1, - config_.pcap_path())); //设备包 - } else { - // open Lslidar input device - input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), config_.packet_size(), - packets_rate, config_.pcap_path())); //数据包 - positioning_input_.reset( - new InputPCAP(config_.difop_port(), config_.device_ip(), config_.packet_size(), 1, - config_.pcap_path())); //设备包 - } - } else { - if (config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || - config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) { - // open Lslidar input device - input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), - config_.packet_size())); //数据包 - positioning_input_.reset( - new InputSocket(config_.difop_port(), config_.device_ip(), 1206)); //设备包 - } else { - // open Lslidar input device - input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), - config_.packet_size())); //数据包 - positioning_input_.reset(new InputSocket(config_.difop_port(), config_.device_ip(), - config_.packet_size())); //设备包 - } - } - difop_thread_ = std::thread(&LslidarDriver::difopPoll, this); - } + input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), + config_.packet_size(), packets_rate, + config_.pcap_path())); // 数据包 + positioning_input_.reset(new InputPCAP(config_.difop_port(), + config_.device_ip(), 1206, 1, + config_.pcap_path())); // 设备包 + } else { + // open Lslidar input device + input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), + config_.packet_size(), packets_rate, + config_.pcap_path())); // 数据包 + positioning_input_.reset(new InputPCAP( + config_.difop_port(), config_.device_ip(), config_.packet_size(), 1, + config_.pcap_path())); // 设备包 + } + } else { + if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + // open Lslidar input device + input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), + config_.packet_size())); // 数据包 + positioning_input_.reset(new InputSocket( + config_.difop_port(), config_.device_ip(), 1206)); // 设备包 + } else { + // open Lslidar input device + input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), + config_.packet_size())); // 数据包 + positioning_input_.reset( + new InputSocket(config_.difop_port(), config_.device_ip(), + config_.packet_size())); // 设备包 + } + } + difop_thread_ = std::thread(&LslidarDriver::difopPoll, this); +} /** poll the device * * @returns true unless end of file reached */ - bool LslidarDriver::Poll(const std::shared_ptr &scan) { - // Allocate a new shared pointer for zero-copy sharing with other nodelets. - int poll_result = PollStandard(scan); - if (poll_result < 0) return false; - - if (scan->firing_pkts().empty()) { - AINFO << "Get an empty scan from port: " << config_.msop_port(); - return false; - } - // publish message using time of last packet read - ADEBUG << "Publishing a full Lslidar scan."; - LslidarPacket *packet = scan->add_difop_pkts(); - std::unique_lock lock(mutex_); - packet->set_data(bytes, FIRING_DATA_PACKET_SIZE); - scan->mutable_header()->set_timestamp_sec(gps_time); - ADEBUG << "**************************************************************GPS time: " << gps_time; - scan->mutable_header()->set_frame_id(config_.frame_id()); - scan->set_model(config_.model()); - scan->set_basetime(gps_time); - scan->mutable_header()->set_timestamp_sec(gps_time); - return true; - } +bool LslidarDriver::Poll( + const std::shared_ptr &scan) { + // Allocate a new shared pointer for zero-copy sharing with other nodelets. + int poll_result = PollStandard(scan); + if (poll_result < 0) return false; - int LslidarDriver::PollStandard(std::shared_ptr scan) { - // Since the lslidar delivers data at a very high rate, keep reading and - // publishing scans as fast as possible. + if (scan->firing_pkts().empty()) { + AINFO << "Get an empty scan from port: " << config_.msop_port(); + return false; + } + // publish message using time of last packet read + ADEBUG << "Publishing a full Lslidar scan."; + LslidarPacket *packet = scan->add_difop_pkts(); + std::unique_lock lock(mutex_); + packet->set_data(bytes, FIRING_DATA_PACKET_SIZE); + scan->mutable_header()->set_timestamp_sec(gps_time); + ADEBUG << "**************************************************************GPS " + "time: " + << gps_time; + scan->mutable_header()->set_frame_id(config_.frame_id()); + scan->set_model(config_.model()); + scan->set_basetime(gps_time); + scan->mutable_header()->set_timestamp_sec(gps_time); + return true; +} - if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || config_.model() == LSLIDAR_C32_V4 - || config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || - config_.model() == LSLIDAR_C1_V4) { - // Find the 0 degree packet, in order to fix the 0 degree angle position - if (scan_fill) { - LslidarPacket *packet = scan->add_firing_pkts(); - int PKT_DATA_LENGTH = 1212; - void *data_ptr = malloc(PKT_DATA_LENGTH); - memcpy(data_ptr, reinterpret_cast(const_cast(scan_start.data().c_str())), - PKT_DATA_LENGTH); - packet->set_data(data_ptr, PKT_DATA_LENGTH); - packet->set_stamp((scan_start.stamp())); - AINFO << "scan->firing_pkts_size(): " << scan->firing_pkts_size(); - } +int LslidarDriver::PollStandard( + std::shared_ptr scan) { + // Since the lslidar delivers data at a very high rate, keep reading and + // publishing scans as fast as possible. - scan_fill = false; - int i = 1; - while (scan->firing_pkts_size() < config_.npackets()) { - //config_.npackets() - LslidarPacket *packet = scan->add_firing_pkts(); - while (true) { - // keep reading until full packet received - int rc = input_->GetPacket(packet); + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) { + // Find the 0 degree packet, in order to fix the 0 degree angle position + if (scan_fill) { + LslidarPacket *packet = scan->add_firing_pkts(); + int PKT_DATA_LENGTH = 1212; + void *data_ptr = malloc(PKT_DATA_LENGTH); + memcpy(data_ptr, + reinterpret_cast( + const_cast(scan_start.data().c_str())), + PKT_DATA_LENGTH); + packet->set_data(data_ptr, PKT_DATA_LENGTH); + packet->set_stamp((scan_start.stamp())); + AINFO << "scan->firing_pkts_size(): " << scan->firing_pkts_size(); + } - if (rc == 0) { - if (!config_.time_synchronization()) { - time_t t = time(NULL); - localtime_r(&t, ¤t_time); - current_time.tm_hour = current_time.tm_hour - 8; - packet_time_ns_ = 0; - gps_time = apollo::cyber::Time().Now().ToNanosecond(); - } else { - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - if (config_.packet_size() == 1212) { //1212字节雷达 - if (0xff == data[1200]) { //ptp授时 - //std::cout << "ptp"; - basetime_ = (data[1201] * pow(2, 32) + data[1202] * pow(2, 24) + - data[1203] * pow(2, 16) + - data[1204] * pow(2, 8) + data[1205] * pow(2, 0)); - packet_time_ns_ = (static_cast(data[1206]) + - static_cast(data[1207]) * pow(2, 8) + - static_cast(data[1208]) * pow(2, 16) + - static_cast(data[1209]) * pow(2, 24)); - } else { //gps授时 - current_time.tm_year = static_cast(data[1200]) + 2000 - 1900; - current_time.tm_mon = static_cast(data[1201]) - 1; - current_time.tm_mday = static_cast(data[1202]); - current_time.tm_hour = static_cast(data[1203]); - current_time.tm_min = static_cast(data[1204]); - - if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P) { - current_time.tm_sec = static_cast(data[1205]) + 1; - packet_time_ns_ = (static_cast(data[1206]) + - static_cast(data[1207]) * pow(2, 8) + - static_cast(data[1208]) * pow(2, 16) + - static_cast(data[1209]) * pow(2, 24)) * - 1e3; //ns - } else if (config_.model() == LSLIDAR_C32_V4 || - config_.model() == LSLIDAR_C16_V4 - || config_.model() == LSLIDAR_C8_V4 || - config_.model() == LSLIDAR_C1_V4) { - current_time.tm_sec = static_cast(data[1205]); - packet_time_ns_ = (static_cast(data[1206]) + - static_cast(data[1207]) * pow(2, 8) + - static_cast(data[1208]) * pow(2, 16) + - static_cast(data[1209]) * pow(2, 24)); - } - basetime_ = static_cast(timegm(¤t_time)); - } - } else { //1206字节雷达，V3.0 - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - packet_time_ns_ = (static_cast(data[1200]) + - static_cast(data[1201]) * pow(2, 8) + - static_cast(data[1202]) * pow(2, 16) + - static_cast(data[1203]) * pow(2, 24)) * 1e3; //ns - basetime_ = static_cast(timegm(¤t_time)); + scan_fill = false; + int i = 1; + while (scan->firing_pkts_size() < config_.npackets()) { + // config_.npackets() + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); - } - gps_time = basetime_ * 1000000000 + packet_time_ns_; - } - packet->set_stamp(gps_time); - break; - } else if (rc < 0) { - return rc; - } - } - packet->set_stamp(gps_time); //設置包的時間 + if (rc == 0) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + packet_time_ns_ = 0; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + if (config_.packet_size() == 1212) { // 1212字节雷达 + if (0xff == data[1200]) { // ptp授时 + // std::cout << "ptp"; + basetime_ = (data[1201] * pow(2, 32) + data[1202] * pow(2, 24) + + data[1203] * pow(2, 16) + data[1204] * pow(2, 8) + + data[1205] * pow(2, 0)); + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)); + } else { // gps授时 + current_time.tm_year = + static_cast(data[1200]) + 2000 - 1900; + current_time.tm_mon = static_cast(data[1201]) - 1; + current_time.tm_mday = static_cast(data[1202]); + current_time.tm_hour = static_cast(data[1203]); + current_time.tm_min = static_cast(data[1204]); - uint8_t *data = - reinterpret_cast(const_cast(packet->data().c_str())); - int azi1 = - 256 * static_cast(data[3]) + static_cast(data[2]); - int azi2 = 256 * static_cast(data[1103]) + - static_cast(data[1102]); - - if (((azi1 > 35000 && azi2 < 1000) || - (azi1 < 500 && i > config_.npackets() / 2))) { - scan_fill = true; - scan_start = *packet; - break; - } - i++; - } - } else if (config_.model() == LSLIDAR_LS128S2) { //LS128S2 - // Find the 0 degree packet, in order to fix the 0 degree angle position - scan_fill = false; - int i = 1; - bool is_found_frame_header = false; - while (scan->firing_pkts_size() < config_.npackets() && !is_found_frame_header) { - LslidarPacket *packet = scan->add_firing_pkts(); - while (true) { - // keep reading until full packet received - int rc = input_->GetPacket(packet); - AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; - if (rc == 0) { - if (!config_.time_synchronization()) { - time_t t = time(NULL); - localtime_r(&t, ¤t_time); - current_time.tm_hour = current_time.tm_hour - 8; - gps_time = apollo::cyber::Time().Now().ToNanosecond(); - } else { - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - if (0xff == static_cast(data[1194])) { //ptp授时 - basetime_ = (static_cast(data[1195]) * 0 + (static_cast(data[1196]) << 24) + - (static_cast(data[1197]) << 16) + (static_cast(data[1198]) << 8) + static_cast(data[1199]) * pow(2, 0)); - packet_time_ns_ = (static_cast(data[1200]) << 24) + (static_cast(data[1201]) << 16) + - (static_cast(data[1202]) << 8) + (static_cast(data[1203])); - gps_time = basetime_ * 1000000000 + packet_time_ns_; - } else { //gps授时 - struct tm cur_time{}; - memset(&cur_time, 0, sizeof(cur_time)); - cur_time.tm_sec = static_cast(data[1199]); - cur_time.tm_min = static_cast(data[1198]); - cur_time.tm_hour = static_cast(data[1197]); - cur_time.tm_mday = static_cast(data[1196]); - cur_time.tm_mon = static_cast(data[1195]) - 1; - cur_time.tm_year = static_cast(data[1194]) + 2000 - 1900; - basetime_ = static_cast(timegm(&cur_time)); - packet_time_ns_ = static_cast(data[1203]) + - (static_cast(data[1202]) << 8) + - (static_cast(data[1201]) << 16) + - (static_cast(data[1200]) << 24); //ns - gps_time = basetime_ * 1000000000 + packet_time_ns_; - } - } - break; - } else if (rc < 0) { - return rc; - } - } - packet->set_stamp(gps_time); //設置包的時間 - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - int return_mode = static_cast(data[1205]); - - if(return_mode == 1) { - for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_SINGLE_ECHO;point_idx += 8) { //一圈 - if ((static_cast(data[point_idx]) == 0xff) && (static_cast(data[point_idx + 1]) == 0xaa) && - (static_cast(data[point_idx + 2]) == 0xbb) && (static_cast(data[point_idx + 3]) == 0xcc) - && (static_cast(data[point_idx + 4]) == 0xdd)) { - scan_fill = false; - is_found_frame_header = true; - - break; - } - } - } - else { - for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_DOUBLE_ECHO;point_idx += 12) { //一圈 - if ((static_cast(data[point_idx]) == 0xff) && (static_cast(data[point_idx + 1]) == 0xaa) && - (static_cast(data[point_idx + 2]) == 0xbb) && (static_cast(data[point_idx + 3]) == 0xcc) - && (static_cast(data[point_idx + 4]) == 0xdd)) { - scan_fill = false; - is_found_frame_header = true; - AERROR << "\none circle! scan->firing_pkts_size(): " << scan->firing_pkts_size(); - break; - } - } - } - i++; - } - } else { // ch系列 - // Find the 0 degree packet, in order to fix the 0 degree angle position - scan_fill = false; - int i = 1; - bool is_found_frame_header = false; - while (scan->firing_pkts_size() < config_.npackets() && !is_found_frame_header) { - LslidarPacket *packet = scan->add_firing_pkts(); - while (true) { - // keep reading until full packet received - int rc = input_->GetPacket(packet); - AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; - if (rc == 0) { - if (config_.model() == LSLIDAR_CH64w || - config_.model() == LSLIDAR_CH120 || - config_.model() == LSLIDAR_CH128X1 || config_.model() == LSLIDAR_CH32) { - if (!config_.time_synchronization()) { - time_t t = time(NULL); - localtime_r(&t, ¤t_time); - current_time.tm_hour = current_time.tm_hour - 8; - gps_time = apollo::cyber::Time().Now().ToNanosecond(); - } else { - uint8_t *data = reinterpret_cast( - const_cast(packet->data().c_str())); - current_time.tm_hour = static_cast(data[1197]); - current_time.tm_min = static_cast(data[1198]); - current_time.tm_sec = static_cast(data[1199]); - basetime_ = static_cast(timegm(¤t_time)); - if (time_service_mode == "gps") { - packet_time_ns_ = - (static_cast(data[1203]) + - static_cast(data[1202]) * pow(2, 8) + - static_cast(data[1201]) * pow(2, 16) + - static_cast(data[1200]) * pow(2, 24)) * - 1e3; // ns - } else if (time_service_mode == "gptp") { - packet_time_ns_ = - (static_cast(data[1203]) + - static_cast(data[1202]) * pow(2, 8) + - static_cast(data[1201]) * pow(2, 16) + - static_cast(data[1200]) * pow(2, 24)); // ns - } - gps_time = basetime_ * 1000000000 + packet_time_ns_; - } - } - break; - } else if (rc < 0) { - return rc; - } - } - packet->set_stamp(gps_time); //設置包的時間 - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; - point_idx += 7) { //一圈 - if ((static_cast(data[point_idx]) == 0xff) && (static_cast(data[point_idx + 1]) == 0xaa) && - (static_cast(data[point_idx + 2]) == 0xbb) && (static_cast(data[point_idx + 3]) == 0xcc)) { - scan_fill = false; - is_found_frame_header = true; - AERROR << "\none circle! scan->firing_pkts_size(): " << scan->firing_pkts_size(); - break; - } - } - i++; - } + if (config_.model() == LSLIDAR16P || + config_.model() == LSLIDAR32P) { + current_time.tm_sec = static_cast(data[1205]) + 1; + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)) * + 1e3; // ns + } else if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + current_time.tm_sec = static_cast(data[1205]); + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)); } - return 0; + basetime_ = static_cast(timegm(¤t_time)); + } + } else { // 1206字节雷达，V3.0 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + packet_time_ns_ = + (static_cast(data[1200]) + + static_cast(data[1201]) * pow(2, 8) + + static_cast(data[1202]) * pow(2, 16) + + static_cast(data[1203]) * pow(2, 24)) * + 1e3; // ns + basetime_ = static_cast(timegm(¤t_time)); } + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } + packet->set_stamp(gps_time); + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 - void LslidarDriver::difopPoll(void) { - while (!cyber::IsShutdown()) { - std::shared_ptr scans = std::make_shared(); - std::shared_ptr packet = std::make_shared(); - while (!cyber::IsShutdown()) { - int rc = positioning_input_->GetPacket(packet.get()); - if (rc == 0) { - break; // got a full packet - } - if (rc < 0) { - return; - } - } - uint8_t *data = reinterpret_cast(const_cast(packet->data().c_str())); - { - std::unique_lock lock(mutex_); - memcpy(bytes, data, FIRING_DATA_PACKET_SIZE); - } - if (!config_.time_synchronization()) { - time_t t = time(NULL); - localtime_r(&t, ¤t_time); - current_time.tm_hour = current_time.tm_hour - 8; - } else { - if (data[0] == 0xA5 && data[1] == 0xFF && data[2] == 0x00 && data[3] == 0x5A) { - if (config_.model() == LSLIDAR_CH16 || config_.model() == LSLIDAR_CH32 - || config_.model() == LSLIDAR_CH128 || config_.model() == LSLIDAR_CH64) { - current_time.tm_year = static_cast(data[36] + 100); - current_time.tm_mon = static_cast(data[37] - 1); - current_time.tm_mday = static_cast(data[38]); - current_time.tm_hour = static_cast(data[39]); - current_time.tm_min = static_cast(data[40]); - current_time.tm_sec = static_cast(data[41]); - } else if (config_.model() == LSLIDAR_CH64w) { - current_time.tm_year = static_cast(data[52] + 100); - current_time.tm_mon = static_cast(data[53] - 1); - current_time.tm_mday = static_cast(data[54]); - if (data[44] == 0x00) { //gps授时 - time_service_mode = "gps"; - } else if (data[44] == 0x01) { //ptp授时 - time_service_mode = "gptp"; - } - } else if (config_.model() == LSLIDAR_CH120) { - current_time.tm_year = static_cast(data[36] + 100); - current_time.tm_mon = static_cast(data[37] - 1); - current_time.tm_mday = static_cast(data[38]); - } else if (config_.model() == LSLIDAR_CH128X1) { - current_time.tm_year = static_cast(data[52] + 100); - current_time.tm_mon = static_cast(data[53] - 1); - current_time.tm_mday = static_cast(data[54]); - if (data[44] == 0x00) { //gps授时 - time_service_mode = "gps"; - } else if (data[44] == 0x01) { //ptp授时 - time_service_mode = "gptp"; - } - } else if (config_.packet_size() == 1206 && - (config_.model() == LSLIDAR32P || config_.model() == LSLIDAR16P)) { - current_time.tm_year = static_cast(data[52] + 100); - current_time.tm_mon = static_cast(data[53] - 1); - current_time.tm_mday = static_cast(data[54]); - current_time.tm_hour = static_cast(data[55]); - current_time.tm_min = static_cast(data[56]); - current_time.tm_sec = static_cast(data[57]); - } - } - } - } + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + int azi1 = + 256 * static_cast(data[3]) + static_cast(data[2]); + int azi2 = 256 * static_cast(data[1103]) + + static_cast(data[1102]); + + if (((azi1 > 35000 && azi2 < 1000) || + (azi1 < 500 && i > config_.npackets() / 2))) { + scan_fill = true; + scan_start = *packet; + break; + } + i++; + } + } else if (config_.model() == LSLIDAR_LS128S2) { // LS128S2 + // Find the 0 degree packet, in order to fix the 0 degree angle position + scan_fill = false; + int i = 1; + bool is_found_frame_header = false; + while (scan->firing_pkts_size() < config_.npackets() && + !is_found_frame_header) { + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); + AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; + if (rc == 0) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + if (0xff == static_cast(data[1194])) { // ptp授时 + basetime_ = (static_cast(data[1195]) * 0 + + (static_cast(data[1196]) << 24) + + (static_cast(data[1197]) << 16) + + (static_cast(data[1198]) << 8) + + static_cast(data[1199]) * pow(2, 0)); + packet_time_ns_ = (static_cast(data[1200]) << 24) + + (static_cast(data[1201]) << 16) + + (static_cast(data[1202]) << 8) + + (static_cast(data[1203])); + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } else { // gps授时 + struct tm cur_time {}; + memset(&cur_time, 0, sizeof(cur_time)); + cur_time.tm_sec = static_cast(data[1199]); + cur_time.tm_min = static_cast(data[1198]); + cur_time.tm_hour = static_cast(data[1197]); + cur_time.tm_mday = static_cast(data[1196]); + cur_time.tm_mon = static_cast(data[1195]) - 1; + cur_time.tm_year = + static_cast(data[1194]) + 2000 - 1900; + basetime_ = static_cast(timegm(&cur_time)); + packet_time_ns_ = + static_cast(data[1203]) + + (static_cast(data[1202]) << 8) + + (static_cast(data[1201]) << 16) + + (static_cast(data[1200]) << 24); // ns + gps_time = basetime_ * 1000000000 + packet_time_ns_; } + } + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + int return_mode = static_cast(data[1205]); - LslidarDriver *LslidarDriverFactory::CreateDriver(const Config &config) { - LslidarDriver *driver = nullptr; - driver = new LslidarDriver(config); - return driver; + if (return_mode == 1) { + for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_SINGLE_ECHO; + point_idx += 8) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc) && + (static_cast(data[point_idx + 4]) == 0xdd)) { + scan_fill = false; + is_found_frame_header = true; + + break; + } + } + } else { + for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_DOUBLE_ECHO; + point_idx += 12) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc) && + (static_cast(data[point_idx + 4]) == 0xdd)) { + scan_fill = false; + is_found_frame_header = true; + AERROR << "\none circle! scan->firing_pkts_size(): " + << scan->firing_pkts_size(); + break; + } + } + } + i++; + } + } else { // ch系列 + // Find the 0 degree packet, in order to fix the 0 degree angle position + scan_fill = false; + int i = 1; + bool is_found_frame_header = false; + while (scan->firing_pkts_size() < config_.npackets() && + !is_found_frame_header) { + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); + AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; + if (rc == 0) { + if (config_.model() == LSLIDAR_CH64w || + config_.model() == LSLIDAR_CH120 || + config_.model() == LSLIDAR_CH128X1 || + config_.model() == LSLIDAR_CH32) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + current_time.tm_hour = static_cast(data[1197]); + current_time.tm_min = static_cast(data[1198]); + current_time.tm_sec = static_cast(data[1199]); + basetime_ = static_cast(timegm(¤t_time)); + if (time_service_mode == "gps") { + packet_time_ns_ = + (static_cast(data[1203]) + + static_cast(data[1202]) * pow(2, 8) + + static_cast(data[1201]) * pow(2, 16) + + static_cast(data[1200]) * pow(2, 24)) * + 1e3; // ns + } else if (time_service_mode == "gptp") { + packet_time_ns_ = + (static_cast(data[1203]) + + static_cast(data[1202]) * pow(2, 8) + + static_cast(data[1201]) * pow(2, 16) + + static_cast(data[1200]) * pow(2, 24)); // ns + } + gps_time = basetime_ * 1000000000 + packet_time_ns_; } + } + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; + point_idx += 7) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc)) { + scan_fill = false; + is_found_frame_header = true; + AERROR << "\none circle! scan->firing_pkts_size(): " + << scan->firing_pkts_size(); + break; + } + } + i++; + } + } + return 0; +} + +void LslidarDriver::difopPoll(void) { + while (!cyber::IsShutdown()) { + std::shared_ptr scans = + std::make_shared(); + std::shared_ptr packet = std::make_shared(); + while (!cyber::IsShutdown()) { + int rc = positioning_input_->GetPacket(packet.get()); + if (rc == 0) { + break; // got a full packet + } + if (rc < 0) { + return; + } + } + uint8_t *data = + reinterpret_cast(const_cast(packet->data().c_str())); + { + std::unique_lock lock(mutex_); + memcpy(bytes, data, FIRING_DATA_PACKET_SIZE); + } + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + } else { + if (data[0] == 0xA5 && data[1] == 0xFF && data[2] == 0x00 && + data[3] == 0x5A) { + if (config_.model() == LSLIDAR_CH16 || + config_.model() == LSLIDAR_CH32 || + config_.model() == LSLIDAR_CH128 || + config_.model() == LSLIDAR_CH64) { + current_time.tm_year = static_cast(data[36] + 100); + current_time.tm_mon = static_cast(data[37] - 1); + current_time.tm_mday = static_cast(data[38]); + current_time.tm_hour = static_cast(data[39]); + current_time.tm_min = static_cast(data[40]); + current_time.tm_sec = static_cast(data[41]); + } else if (config_.model() == LSLIDAR_CH64w) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + if (data[44] == 0x00) { // gps授时 + time_service_mode = "gps"; + } else if (data[44] == 0x01) { // ptp授时 + time_service_mode = "gptp"; + } + } else if (config_.model() == LSLIDAR_CH120) { + current_time.tm_year = static_cast(data[36] + 100); + current_time.tm_mon = static_cast(data[37] - 1); + current_time.tm_mday = static_cast(data[38]); + } else if (config_.model() == LSLIDAR_CH128X1) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + if (data[44] == 0x00) { // gps授时 + time_service_mode = "gps"; + } else if (data[44] == 0x01) { // ptp授时 + time_service_mode = "gptp"; + } + } else if (config_.packet_size() == 1206 && + (config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR16P)) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + current_time.tm_hour = static_cast(data[55]); + current_time.tm_min = static_cast(data[56]); + current_time.tm_sec = static_cast(data[57]); + } + } + } + } +} + +LslidarDriver *LslidarDriverFactory::CreateDriver(const Config &config) { + LslidarDriver *driver = nullptr; + driver = new LslidarDriver(config); + return driver; +} - } // namespace lslidar - } // namespace drivers +} // namespace lslidar +} // namespace drivers } // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/driver.h b/modules/drivers/lidar/lslidar/driver/driver.h index 3f72d6261bd..2badd457b01 100755 --- a/modules/drivers/lidar/lslidar/driver/driver.h +++ b/modules/drivers/lidar/lslidar/driver/driver.h @@ -16,81 +16,82 @@ #pragma once +#include +#include +#include #include -#include #include -#include -#include "modules/drivers/lidar/lslidar/driver/input.h" +#include + #include "modules/drivers/lidar/lslidar/proto/config.pb.h" #include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - constexpr int BLOCKS_PER_PACKET = 12; - constexpr int BLOCK_SIZE = 100; - - static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000; - static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12; - static const int POINTS_PER_PACKET = 171 * 7; // ch系列 - static const int LS_POINTS_PER_PACKET_SINGLE_ECHO = 149 * 8; // LS 1550nm系列 单回波 - static const int LS_POINTS_PER_PACKET_DOUBLE_ECHO = 99 * 12; // LS 1550nm系列 单回波 - - class LslidarDriver - { - public: - explicit LslidarDriver(const Config &config) : config_(config) - { - // scan_start = new LslidarPacket(); - } - ~LslidarDriver(); - - bool Poll(const std::shared_ptr &scan); - void Init(); - void difopPoll(); - void SetPacketRate(const double packet_rate) { packet_rate_ = packet_rate; } - int npackets; - struct tm current_time; - - protected: - Config config_; - std::unique_ptr input_ = nullptr; - std::unique_ptr positioning_input_ = nullptr; - std::string topic_; - double packet_rate_ = 0.0; - bool scan_fill = false; - uint64_t gps_time = 0; - uint64_t last_gps_time = 0; - - uint64_t basetime_ = 0; - uint64_t packet_time_ns_ = 0; - - uint32_t last_gps_time_ = 0; - uint64_t last_count_ = 0; - static uint64_t sync_counter; - - std::thread difop_thread_; - int PollStandard(std::shared_ptr scan); - LslidarPacket scan_start; - LslidarPacket last_scan_start; - - LslidarPacket scan_start1; - LslidarPacket scan_start2; - std::mutex mutex_; - uint8_t bytes[FIRING_DATA_PACKET_SIZE] = {0x00}; - std::string time_service_mode = {"gps"}; - }; - - class LslidarDriverFactory - { - public: - static LslidarDriver *CreateDriver(const Config &config); - }; - - } // namespace lslidar - } // namespace drivers -} // namespace apollo +#include "cyber/cyber.h" +#include "modules/drivers/lidar/lslidar/driver/input.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +constexpr int BLOCKS_PER_PACKET = 12; +constexpr int BLOCK_SIZE = 100; + +static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000; +static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12; +static const int POINTS_PER_PACKET = 171 * 7; // ch系列 +static const int LS_POINTS_PER_PACKET_SINGLE_ECHO = + 149 * 8; // LS 1550nm系列 单回波 +static const int LS_POINTS_PER_PACKET_DOUBLE_ECHO = + 99 * 12; // LS 1550nm系列 单回波 + +class LslidarDriver { + public: + explicit LslidarDriver(const Config &config) : config_(config) { + // scan_start = new LslidarPacket(); + } + ~LslidarDriver(); + + bool Poll(const std::shared_ptr &scan); + void Init(); + void difopPoll(); + void SetPacketRate(const double packet_rate) { packet_rate_ = packet_rate; } + int npackets; + struct tm current_time; + + protected: + Config config_; + std::unique_ptr input_ = nullptr; + std::unique_ptr positioning_input_ = nullptr; + std::string topic_; + double packet_rate_ = 0.0; + bool scan_fill = false; + uint64_t gps_time = 0; + uint64_t last_gps_time = 0; + + uint64_t basetime_ = 0; + uint64_t packet_time_ns_ = 0; + + uint32_t last_gps_time_ = 0; + uint64_t last_count_ = 0; + static uint64_t sync_counter; + + std::thread difop_thread_; + int PollStandard(std::shared_ptr scan); + LslidarPacket scan_start; + LslidarPacket last_scan_start; + + LslidarPacket scan_start1; + LslidarPacket scan_start2; + std::mutex mutex_; + uint8_t bytes[FIRING_DATA_PACKET_SIZE] = {0x00}; + std::string time_service_mode = {"gps"}; +}; + +class LslidarDriverFactory { + public: + static LslidarDriver *CreateDriver(const Config &config); +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/input.cc b/modules/drivers/lidar/lslidar/driver/input.cc index 4b286870357..3c37dc9680e 100755 --- a/modules/drivers/lidar/lslidar/driver/input.cc +++ b/modules/drivers/lidar/lslidar/driver/input.cc @@ -15,250 +15,220 @@ *****************************************************************************/ #include "modules/drivers/lidar/lslidar/driver/input.h" + #include #include #include #include #include -#include -#include -#include -#include "cyber/cyber.h" - -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - Input::Input(uint16_t port, std::string lidar_ip, int packet_size) - { - packet_size_ = packet_size; - inet_aton(lidar_ip.c_str(), &devip_); - } - int Input::GetPacket(LslidarPacket *pkt) - { - return 0; - } - - Input::~Input(void) - { - (void)close(sockfd_); - } +#include +#include +#include +#include - InputSocket::InputSocket(uint16_t port, std::string lidar_ip, int packet_size) : Input(port, lidar_ip, packet_size) - { - port_ = port; - sockfd_ = -1; - sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); - if (-1 == sockfd_) - { - AERROR << "socket open error"; - return; - } - - inet_aton(lidar_ip.c_str(), &devip_); - sockaddr_in myAddress; // my address information - memset(&myAddress, 0, sizeof(myAddress)); // initialize to zeros - myAddress.sin_family = AF_INET; // host byte order - myAddress.sin_port = htons(port); // port in network byte order - myAddress.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP - - if (bind(sockfd_, reinterpret_cast(&myAddress), - sizeof(sockaddr)) == -1) - { - AERROR << "bind error, port:" << port; - return; - } - - if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) - { - AERROR << "fcntl error"; - return; - } +namespace apollo { +namespace drivers { +namespace lslidar { + +Input::Input(uint16_t port, std::string lidar_ip, int packet_size) { + packet_size_ = packet_size; + inet_aton(lidar_ip.c_str(), &devip_); +} + +int Input::GetPacket(LslidarPacket *pkt) { return 0; } + +Input::~Input(void) { (void)close(sockfd_); } + +InputSocket::InputSocket(uint16_t port, std::string lidar_ip, int packet_size) + : Input(port, lidar_ip, packet_size) { + port_ = port; + sockfd_ = -1; + sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); + if (-1 == sockfd_) { + AERROR << "socket open error"; + return; + } + + inet_aton(lidar_ip.c_str(), &devip_); + sockaddr_in myAddress; // my address information + memset(&myAddress, 0, sizeof(myAddress)); // initialize to zeros + myAddress.sin_family = AF_INET; // host byte order + myAddress.sin_port = htons(port); // port in network byte order + myAddress.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP + + if (bind(sockfd_, reinterpret_cast(&myAddress), + sizeof(sockaddr)) == -1) { + AERROR << "bind error, port:" << port; + return; + } + + if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) { + AERROR << "fcntl error"; + return; + } +} + +InputSocket::~InputSocket(void) { (void)close(sockfd_); } + +int InputSocket::GetPacket(LslidarPacket *pkt) { + double time1 = apollo::cyber::Time().Now().ToSecond(); + struct pollfd fds[1]; + fds[0].fd = sockfd_; + fds[0].events = POLLIN; + static const int POLL_TIMEOUT = 3000; // one second (in msec) + + sockaddr_in sender_address; + socklen_t sender_address_len = sizeof(sender_address); + + while (true) { + // Receive packets that should now be available from the + // socket using a blocking read. + // poll() until input available + do { + int retval = poll(fds, 1, POLL_TIMEOUT); + if (retval < 0) { + if (errno != EINTR) AERROR << "poll() error: " << strerror(errno); + + return 1; } - - InputSocket::~InputSocket(void) - { - (void)close(sockfd_); + if (retval == 0) { + AERROR << "lslidar poll() timeout, port: " << port_; + return 1; } - - int InputSocket::GetPacket(LslidarPacket *pkt) - { - double time1 = apollo::cyber::Time().Now().ToSecond(); - struct pollfd fds[1]; - fds[0].fd = sockfd_; - fds[0].events = POLLIN; - static const int POLL_TIMEOUT = 3000; // one second (in msec) - - sockaddr_in sender_address; - socklen_t sender_address_len = sizeof(sender_address); - - while (true) - { - // Receive packets that should now be available from the - // socket using a blocking read. - // poll() until input available - do - { - int retval = poll(fds, 1, POLL_TIMEOUT); - if (retval < 0) // poll() error? - { - if (errno != EINTR) - AERROR << "poll() error: " << strerror(errno); - - return 1; - } - if (retval == 0) // poll() timeout? - { - AERROR << "lslidar poll() timeout, port: " << port_; - return 1; - } - if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error? - { - AWARN << "poll() reports lslidar error"; - return 1; - } - } while ((fds[0].revents & POLLIN) == 0); - - uint8_t bytes[packet_size_]; - ssize_t nbytes = recvfrom(sockfd_, bytes, packet_size_, 0, (sockaddr *)&sender_address, &sender_address_len); - - if (nbytes < 0) - { - if (errno != EWOULDBLOCK) - { - AERROR << "recvfail"; - return 1; - } - } - else if ((size_t)nbytes == size_t(packet_size_)) - { - if (sender_address.sin_addr.s_addr != devip_.s_addr) - { - AERROR << "lidar IP parameter set error,please reset in config file"; - continue; - } - else - { - pkt->set_data(bytes, packet_size_); - break; - } - } - - AERROR << "incomplete lslidar packet read: " << nbytes << " bytes"; - } - - // Average the times at which we begin and end reading. Use that to - // estimate when the scan occurred. - double time2 = apollo::cyber::Time().Now().ToSecond(); - AINFO << apollo::cyber::Time((time2 + time1) / 2.0).ToNanosecond(); - return 0; + if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || + (fds[0].revents & POLLNVAL)) { + AWARN << "poll() reports lslidar error"; + return 1; } - - InputPCAP::InputPCAP(uint16_t port, std::string lidar_ip, int packet_size, double packet_rate, std::string filename, - bool read_once, bool read_fast, double repeat_delay) - : Input(port, lidar_ip, packet_size), packet_rate_(packet_rate), filename_(filename) - { - pcap_ = NULL; - empty_ = true; - packet_rate_ = packet_rate; - lidar_ip_ = lidar_ip; - // get parameters using private node handle - read_once_ = read_once; - read_fast_ = read_fast; - repeat_delay_ = repeat_delay; - - if (read_once_) - AINFO << "Read input file only once."; - if (read_fast_) - AINFO << "Read input file as quickly as possible."; - if (repeat_delay_ > 0.0) - AINFO << "Delay %.3f seconds before repeating input file." << repeat_delay_; - - // Open the PCAP dump file - AERROR << "Opening PCAP file " << filename_; - if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) - { - AERROR << "Error opening lslidar socket dump file."; - return; - } - - std::stringstream filter; - if (lidar_ip != "") // using specific IP? - { - filter << "src host " << lidar_ip << " && "; - } - filter << "udp dst port " << port; - pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN); + } while ((fds[0].revents & POLLIN) == 0); + uint8_t bytes[1212]; + ssize_t nbytes = recvfrom(sockfd_, bytes, packet_size_, 0, + reinterpret_cast(&sender_address), + &sender_address_len); + if (nbytes < 0) { + if (errno != EWOULDBLOCK) { + AERROR << "recvfail"; + return 1; } - - /** destructor */ - InputPCAP::~InputPCAP(void) - { - pcap_close(pcap_); + } else if ((size_t)nbytes == size_t(packet_size_)) { + if (sender_address.sin_addr.s_addr != devip_.s_addr) { + AERROR << "lidar IP parameter set error,please reset in config file"; + continue; + } else { + pkt->set_data(bytes, packet_size_); + break; } - - /** @brief Get one lslidar packet. */ - int InputPCAP::GetPacket(LslidarPacket *pkt) - { - struct pcap_pkthdr *header; - const u_char *pkt_data; - - // while (flag == 1) - while (!apollo::cyber::IsShutdown()) - { - int res; - if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) - { - // Skip packets not for the correct port and from the - // selected IP address. - if (!lidar_ip_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) - continue; - - // Keep the reader from blowing through the file. - if (read_fast_ == false) - usleep(int(1000 * 1000 / packet_rate_ / 1.1)); - - uint8_t bytes[packet_size_]; - memcpy(bytes, pkt_data + 42, packet_size_); - pkt->set_data(bytes, packet_size_); - empty_ = false; - return 0; // success - } - - if (empty_) // no data in file? - { - AINFO << "Error " << res << " reading lslidar packet: " << pcap_geterr(pcap_); - return -1; - } - - if (read_once_) - { - AINFO << "end of file reached -- done reading."; - return -1; - } - - if (repeat_delay_ > 0.0) - { - AINFO << "end of file reached -- delaying" << repeat_delay_ << "seconds."; - usleep(rint(repeat_delay_ * 1000000.0)); - } - - AINFO << "replaying lslidar dump file"; - - // I can't figure out how to rewind the file, because it - // starts with some kind of header. So, close the file - // and reopen it with pcap. - pcap_close(pcap_); - pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); - empty_ = true; // maybe the file disappeared? - } // loop back and try again - return 0; + } + AERROR << "incomplete lslidar packet read: " << nbytes << " bytes"; + } + + // Average the times at which we begin and end reading. Use that to + // estimate when the scan occurred. + double time2 = apollo::cyber::Time().Now().ToSecond(); + AINFO << apollo::cyber::Time((time2 + time1) / 2.0).ToNanosecond(); + return 0; +} + +InputPCAP::InputPCAP(uint16_t port, std::string lidar_ip, int packet_size, + double packet_rate, std::string filename, bool read_once, + bool read_fast, double repeat_delay) + : Input(port, lidar_ip, packet_size), + packet_rate_(packet_rate), + filename_(filename) { + pcap_ = NULL; + empty_ = true; + packet_rate_ = packet_rate; + lidar_ip_ = lidar_ip; + // get parameters using private node handle + read_once_ = read_once; + read_fast_ = read_fast; + repeat_delay_ = repeat_delay; + + if (read_once_) AINFO << "Read input file only once."; + if (read_fast_) AINFO << "Read input file as quickly as possible."; + if (repeat_delay_ > 0.0) + AINFO << "Delay %.3f seconds before repeating input file." << repeat_delay_; + + // Open the PCAP dump file + AERROR << "Opening PCAP file " << filename_; + if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) { + AERROR << "Error opening lslidar socket dump file."; + return; + } + + std::stringstream filter; + if (lidar_ip != "") { + filter << "src host " << lidar_ip << " && "; + } + filter << "udp dst port " << port; + pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, + PCAP_NETMASK_UNKNOWN); +} + +/** destructor */ +InputPCAP::~InputPCAP(void) { pcap_close(pcap_); } + +/** @brief Get one lslidar packet. */ +int InputPCAP::GetPacket(LslidarPacket *pkt) { + struct pcap_pkthdr *header; + const u_char *pkt_data; + + // while (flag == 1) + while (!apollo::cyber::IsShutdown()) { + int res; + if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) { + // Skip packets not for the correct port and from the + // selected IP address. + if (!lidar_ip_.empty() && + (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) + continue; + + // Keep the reader from blowing through the file. + if (read_fast_ == false) + usleep(static_cast(1000 * 1000 / packet_rate_ / 1.1)); + if (1206 == packet_size_) { + uint8_t bytes[1206]; + memcpy(bytes, pkt_data + 42, packet_size_); + pkt->set_data(bytes, packet_size_); + } else if (1212 == packet_size_) { + uint8_t bytes[1212]; + memcpy(bytes, pkt_data + 42, packet_size_); + pkt->set_data(bytes, packet_size_); } - } // namespace lslidar - } // namespace drivers -} // namespace apollo + empty_ = false; + return 0; // success + } + + if (empty_) { + AINFO << "Error " << res + << " reading lslidar packet: " << pcap_geterr(pcap_); + return -1; + } + + if (read_once_) { + AINFO << "end of file reached -- done reading."; + return -1; + } + + if (repeat_delay_ > 0.0) { + AINFO << "end of file reached -- delaying" << repeat_delay_ << "seconds."; + usleep(rint(repeat_delay_ * 1000000.0)); + } + + AINFO << "replaying lslidar dump file"; + + // I can't figure out how to rewind the file, because it + // starts with some kind of header. So, close the file + // and reopen it with pcap. + pcap_close(pcap_); + pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); + empty_ = true; // maybe the file disappeared? + } // loop back and try again + return 0; +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/input.h b/modules/drivers/lidar/lslidar/driver/input.h index e0c09a3686d..99c87f196be 100755 --- a/modules/drivers/lidar/lslidar/driver/input.h +++ b/modules/drivers/lidar/lslidar/driver/input.h @@ -14,84 +14,81 @@ * limitations under the License. *****************************************************************************/ -#ifndef LIDAR_LSLIDAR_SRC_INPUT_H_ -#define LIDAR_LSLIDAR_SRC_INPUT_H_ +#pragma once -#include #include + + #include -#include #include -#include "cyber/cyber.h" - -#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" #include +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/cyber.h" -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - static const size_t FIRING_DATA_PACKET_SIZE = 1212; - static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC - - class Input - { - public: - Input(uint16_t portport = MSOP_DATA_PORT_NUMBER, std::string lidar_ip = "192.168.1.200", int packet_size = 1212); - virtual ~Input(); - virtual int GetPacket(LslidarPacket *pkt); - - protected: - int port_; - int sockfd_; - uint64_t pointcloudTimeStamp; - in_addr devip_; - int packet_size_; - }; - - /** @brief Live lslidar input from socket. */ - class InputSocket : public Input - { - public: - InputSocket(uint16_t port = MSOP_DATA_PORT_NUMBER, std::string lidar_ip = "192.168.1.200", int packet_size = 1212); - - virtual ~InputSocket(); - - virtual int GetPacket(LslidarPacket *pkt); - }; - - /** @brief lslidar input from PCAP dump file. - * - * Dump files can be grabbed by libpcap - */ - class InputPCAP : public Input - { - public: - InputPCAP(uint16_t port = MSOP_DATA_PORT_NUMBER, std::string lidar_ip = "192.168.1.200", int packet_size = 1212, double packet_rate = 0.0, - std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0); - - virtual ~InputPCAP(); - - virtual int GetPacket(LslidarPacket *pkt); - - private: - double packet_rate_; - std::string filename_; - pcap_t *pcap_; - bpf_program pcap_packet_filter_; - char errbuf_[PCAP_ERRBUF_SIZE]; - bool empty_; - bool read_once_; - bool read_fast_; - double repeat_delay_; - std::string lidar_ip_; - }; - - } // namespace lslidar - } // namespace drivers -} // namespace apollo - -#endif // SRC_INPUT_H_ +namespace apollo { +namespace drivers { +namespace lslidar { + +static const size_t FIRING_DATA_PACKET_SIZE = 1212; +static uint16_t MSOP_DATA_PORT_NUMBER = + 2368; // lslidar default data port on PC + +class Input { + public: + Input(uint16_t portport = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212); + virtual ~Input(); + virtual int GetPacket(LslidarPacket *pkt); + + protected: + int port_; + int sockfd_; + uint64_t pointcloudTimeStamp; + in_addr devip_; + int packet_size_; +}; + +/** @brief Live lslidar input from socket. */ +class InputSocket : public Input { + public: + InputSocket(uint16_t port = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212); + + virtual ~InputSocket(); + + virtual int GetPacket(LslidarPacket *pkt); +}; + +/** @brief lslidar input from PCAP dump file. + * + * Dump files can be grabbed by libpcap + */ +class InputPCAP : public Input { + public: + InputPCAP(uint16_t port = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212, + double packet_rate = 0.0, std::string filename = "", + bool read_once = false, bool read_fast = false, + double repeat_delay = 0.0); + + virtual ~InputPCAP(); + + virtual int GetPacket(LslidarPacket *pkt); + + private: + double packet_rate_; + std::string filename_; + pcap_t *pcap_; + bpf_program pcap_packet_filter_; + char errbuf_[PCAP_ERRBUF_SIZE]; + bool empty_; + bool read_once_; + bool read_fast_; + double repeat_delay_; + std::string lidar_ip_; +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc index 3ee5cc0a6c4..612a2369367 100755 --- a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc +++ b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc @@ -14,83 +14,66 @@ * limitations under the License. *****************************************************************************/ -#include -#include -#include - -#include "cyber/cyber.h" -#include "modules/common/util/message_util.h" #include "modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h" -namespace apollo -{ - namespace drivers - { - namespace lslidar - { +namespace apollo { +namespace drivers { +namespace lslidar { - static void my_handler(int sig) - { - exit(-1); - } +static void my_handler(int sig) { exit(-1); } - bool LslidarDriverComponent::Init() - { - signal(SIGINT, my_handler); - AINFO << "Lslidar driver component init"; - Config lslidar_config; - if (!GetProtoConfig(&lslidar_config)) - { - return false; - } +bool LslidarDriverComponent::Init() { + signal(SIGINT, my_handler); + AINFO << "Lslidar driver component init"; + Config lslidar_config; + if (!GetProtoConfig(&lslidar_config)) { + return false; + } - // start the driver - writer_ = node_->CreateWriter(lslidar_config.scan_channel()); - LslidarDriver *driver = LslidarDriverFactory::CreateDriver(lslidar_config); - if (driver == nullptr) - { - return false; - } - dvr_.reset(driver); - dvr_->Init(); - // spawn device poll thread - runing_ = true; - device_thread_ = std::shared_ptr( - new std::thread(std::bind(&LslidarDriverComponent::device_poll, this))); - device_thread_->detach(); + // start the driver + writer_ = node_->CreateWriter( + lslidar_config.scan_channel()); + LslidarDriver *driver = LslidarDriverFactory::CreateDriver(lslidar_config); + if (driver == nullptr) { + return false; + } + dvr_.reset(driver); + dvr_->Init(); + // spawn device poll thread + runing_ = true; + device_thread_ = std::shared_ptr( + new std::thread(std::bind(&LslidarDriverComponent::device_poll, this))); + device_thread_->detach(); - return true; - } + return true; +} - /** @brief Device poll thread main loop. */ - void LslidarDriverComponent::device_poll() - { - signal(SIGINT, my_handler); - while (!apollo::cyber::IsShutdown()) - { - // poll device until end of file - std::shared_ptr scan = std::make_shared(); +/** @brief Device poll thread main loop. */ +void LslidarDriverComponent::device_poll() { + signal(SIGINT, my_handler); + while (!apollo::cyber::IsShutdown()) { + // poll device until end of file + std::shared_ptr scan = + std::make_shared(); - bool ret = dvr_->Poll(scan); - if (ret) - { - common::util::FillHeader("lslidar", scan.get()); - AINFO << "publish scan!"; - double time1 = apollo::cyber::Time().Now().ToSecond(); - writer_->Write(scan); - double time2 = apollo::cyber::Time().Now().ToSecond(); - AINFO << "apollo::cyber::Time((time2 - time1)" << apollo::cyber::Time((time2 - time1) / 2.0).ToNanosecond(); - } - else - { - AWARN << "device poll failed"; - } - } + bool ret = dvr_->Poll(scan); + if (ret) { + common::util::FillHeader("lslidar", scan.get()); + AINFO << "publish scan!"; + double time1 = apollo::cyber::Time().Now().ToSecond(); + writer_->Write(scan); + double time2 = apollo::cyber::Time().Now().ToSecond(); + AINFO << "apollo::cyber::Time((time2 - time1)" + << apollo::cyber::Time((time2 - time1) / 2.0).ToNanosecond(); + } else { + AWARN << "device poll failed"; + } + } - AERROR << "CompLslidarDriver thread exit"; - runing_ = false; - } + AERROR << "CompLslidarDriver thread exit"; + runing_ = false; +} - } // namespace lslidar - } // namespace drivers -} // namespace apollo +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h index c905bcdc657..8648104fe86 100755 --- a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h +++ b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h @@ -17,13 +17,13 @@ #include #include -#include +#include +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" #include "cyber/cyber.h" - +#include "modules/common/util/message_util.h" #include "modules/drivers/lidar/lslidar/driver/driver.h" -#include "modules/drivers/lidar/lslidar/proto/config.pb.h" -#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" namespace apollo { namespace drivers { @@ -44,13 +44,13 @@ class LslidarDriverComponent : public Component<> { bool Init() override; private: - void device_poll(); volatile bool runing_; ///< device thread is running uint32_t seq_ = 0; std::shared_ptr device_thread_; std::shared_ptr dvr_; ///< driver implementation class - std::shared_ptr> writer_; + std::shared_ptr> + writer_; }; CYBER_REGISTER_COMPONENT(LslidarDriverComponent) diff --git a/modules/drivers/lidar/lslidar/parser/calibration.cc b/modules/drivers/lidar/lslidar/parser/calibration.cc index 60f0c3c0398..0a34b37cc05 100644 --- a/modules/drivers/lidar/lslidar/parser/calibration.cc +++ b/modules/drivers/lidar/lslidar/parser/calibration.cc @@ -29,235 +29,201 @@ #include "modules/drivers/lidar/lslidar/parser/calibration.h" -#include -#include -#include -#include -#include -#include -#include "yaml-cpp/yaml.h" - -namespace YAML -{ - // The >> operator disappeared in yaml-cpp 0.5, so this function is - // added to provide support for code written under the yaml-cpp 0.3 API. - template - void operator>>(const YAML::Node &node, T &i) - { - i = node.as(); - } -} // namespace YAML - -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - const char *NUM_LASERS = "num_lasers"; - const char *LASERS = "lasers"; - const char *LASER_ID = "laser_id"; - const char *ROT_CORRECTION = "rot_correction"; - const char *VERT_CORRECTION = "vert_correction"; - const char *DIST_CORRECTION = "dist_correction"; - const char *TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available"; - const char *DIST_CORRECTION_X = "dist_correction_x"; - const char *DIST_CORRECTION_Y = "dist_correction_y"; - const char *VERT_OFFSET_CORRECTION = "vert_offset_correction"; - const char *HORIZ_OFFSET_CORRECTION = "horiz_offset_correction"; - const char *MAX_INTENSITY = "max_intensity"; - const char *MIN_INTENSITY = "min_intensity"; - const char *FOCAL_DISTANCE = "focal_distance"; - const char *FOCAL_SLOPE = "focal_slope"; - - void operator>>(const YAML::Node &node, - std::pair &correction) - { - node[LASER_ID] >> correction.first; - node[ROT_CORRECTION] >> correction.second.rot_correction; - node[VERT_CORRECTION] >> correction.second.vert_correction; - node[DIST_CORRECTION] >> correction.second.dist_correction; - node[DIST_CORRECTION_X] >> correction.second.dist_correction_x; - node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y; - node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; - if (node[HORIZ_OFFSET_CORRECTION]) - { - node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; - } - else - { - correction.second.horiz_offset_correction = 0.0; - } - if (node[MAX_INTENSITY]) - { - node[MAX_INTENSITY] >> correction.second.max_intensity; - } - else - { - correction.second.max_intensity = 255; - } - if (node[MIN_INTENSITY]) - { - node[MIN_INTENSITY] >> correction.second.min_intensity; - } - else - { - correction.second.min_intensity = 0; - } - - node[FOCAL_DISTANCE] >> correction.second.focal_distance; - node[FOCAL_SLOPE] >> correction.second.focal_slope; - - // Calculate cached values - correction.second.cos_rot_correction = cosf(correction.second.rot_correction); - correction.second.sin_rot_correction = sinf(correction.second.rot_correction); - correction.second.cos_vert_correction = - cosf(correction.second.vert_correction); - correction.second.sin_vert_correction = - sinf(correction.second.vert_correction); - correction.second.focal_offset = - 256.0f * static_cast(std::pow( - 1 - correction.second.focal_distance / 13100.0f, 2)); - correction.second.laser_ring = 0; // clear initially (set later) - } +#include "yaml-cpp/yaml.h" - void operator>>(const YAML::Node &node, Calibration &calibration) - { - int num_lasers = 0; - node[NUM_LASERS] >> num_lasers; - const YAML::Node &lasers = node[LASERS]; - calibration.laser_corrections_.clear(); - calibration.num_lasers_ = num_lasers; +namespace YAML { +// The >> operator disappeared in yaml-cpp 0.5, so this function is +// added to provide support for code written under the yaml-cpp 0.3 API. +template +void operator>>(const YAML::Node &node, T &i) { + i = node.as(); +} +} // namespace YAML + +namespace apollo { +namespace drivers { +namespace lslidar { + +const char *NUM_LASERS = "num_lasers"; +const char *LASERS = "lasers"; +const char *LASER_ID = "laser_id"; +const char *ROT_CORRECTION = "rot_correction"; +const char *VERT_CORRECTION = "vert_correction"; +const char *DIST_CORRECTION = "dist_correction"; +const char *TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available"; +const char *DIST_CORRECTION_X = "dist_correction_x"; +const char *DIST_CORRECTION_Y = "dist_correction_y"; +const char *VERT_OFFSET_CORRECTION = "vert_offset_correction"; +const char *HORIZ_OFFSET_CORRECTION = "horiz_offset_correction"; +const char *MAX_INTENSITY = "max_intensity"; +const char *MIN_INTENSITY = "min_intensity"; +const char *FOCAL_DISTANCE = "focal_distance"; +const char *FOCAL_SLOPE = "focal_slope"; + +void operator>>(const YAML::Node &node, + std::pair &correction) { + node[LASER_ID] >> correction.first; + node[ROT_CORRECTION] >> correction.second.rot_correction; + node[VERT_CORRECTION] >> correction.second.vert_correction; + node[DIST_CORRECTION] >> correction.second.dist_correction; + node[DIST_CORRECTION_X] >> correction.second.dist_correction_x; + node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y; + node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; + if (node[HORIZ_OFFSET_CORRECTION]) { + node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; + } else { + correction.second.horiz_offset_correction = 0.0; + } - for (int i = 0; i < num_lasers; i++) - { - std::pair correction; - lasers[i] >> correction; - calibration.laser_corrections_.insert(correction); - } + if (node[MAX_INTENSITY]) { + node[MAX_INTENSITY] >> correction.second.max_intensity; + } else { + correction.second.max_intensity = 255; + } - // For each laser ring, find the next-smallest vertical angle. - // - // This implementation is simple, but not efficient. That is OK, - // since it only runs while starting up. - double next_angle = -std::numeric_limits::infinity(); + if (node[MIN_INTENSITY]) { + node[MIN_INTENSITY] >> correction.second.min_intensity; + } else { + correction.second.min_intensity = 0; + } - for (int ring = 0; ring < num_lasers; ++ring) - { - // find minimum remaining vertical offset correction - double min_seen = std::numeric_limits::infinity(); - int next_index = num_lasers; + node[FOCAL_DISTANCE] >> correction.second.focal_distance; + node[FOCAL_SLOPE] >> correction.second.focal_slope; + + // Calculate cached values + correction.second.cos_rot_correction = cosf(correction.second.rot_correction); + correction.second.sin_rot_correction = sinf(correction.second.rot_correction); + correction.second.cos_vert_correction = + cosf(correction.second.vert_correction); + correction.second.sin_vert_correction = + sinf(correction.second.vert_correction); + correction.second.focal_offset = + 256.0f * static_cast(std::pow( + 1 - correction.second.focal_distance / 13100.0f, 2)); + correction.second.laser_ring = 0; // clear initially (set later) +} + +void operator>>(const YAML::Node &node, Calibration &calibration) { + int num_lasers = 0; + node[NUM_LASERS] >> num_lasers; + const YAML::Node &lasers = node[LASERS]; + calibration.laser_corrections_.clear(); + calibration.num_lasers_ = num_lasers; + + for (int i = 0; i < num_lasers; i++) { + std::pair correction; + lasers[i] >> correction; + calibration.laser_corrections_.insert(correction); + } - for (int j = 0; j < num_lasers; ++j) - { - double angle = calibration.laser_corrections_[j].vert_correction; + // For each laser ring, find the next-smallest vertical angle. + // + // This implementation is simple, but not efficient. That is OK, + // since it only runs while starting up. + double next_angle = -std::numeric_limits::infinity(); - if (next_angle < angle && angle < min_seen) - { - min_seen = angle; - next_index = j; - } - } + for (int ring = 0; ring < num_lasers; ++ring) { + // find minimum remaining vertical offset correction + double min_seen = std::numeric_limits::infinity(); + int next_index = num_lasers; - if (next_index < num_lasers) - { // anything found in this ring? - // store this ring number with its corresponding laser number - calibration.laser_corrections_[next_index].laser_ring = ring; - next_angle = min_seen; - } - } - } + for (int j = 0; j < num_lasers; ++j) { + double angle = calibration.laser_corrections_[j].vert_correction; - YAML::Emitter &operator<<(YAML::Emitter &out, - const std::pair &correction) - { - out << YAML::BeginMap; - out << YAML::Key << LASER_ID << YAML::Value << correction.first; - out << YAML::Key << ROT_CORRECTION << YAML::Value - << correction.second.rot_correction; - out << YAML::Key << VERT_CORRECTION << YAML::Value - << correction.second.vert_correction; - out << YAML::Key << DIST_CORRECTION << YAML::Value - << correction.second.dist_correction; - out << YAML::Key << DIST_CORRECTION_X << YAML::Value - << correction.second.dist_correction_x; - out << YAML::Key << DIST_CORRECTION_Y << YAML::Value - << correction.second.dist_correction_y; - out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value - << correction.second.vert_offset_correction; - out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value - << correction.second.horiz_offset_correction; - out << YAML::Key << MAX_INTENSITY << YAML::Value - << correction.second.max_intensity; - out << YAML::Key << MIN_INTENSITY << YAML::Value - << correction.second.min_intensity; - out << YAML::Key << FOCAL_DISTANCE << YAML::Value - << correction.second.focal_distance; - out << YAML::Key << FOCAL_SLOPE << YAML::Value - << correction.second.focal_slope; - out << YAML::EndMap; - return out; + if (next_angle < angle && angle < min_seen) { + min_seen = angle; + next_index = j; } + } - YAML::Emitter &operator<<(YAML::Emitter &out, const Calibration &calibration) - { - out << YAML::BeginMap; - out << YAML::Key << NUM_LASERS << YAML::Value - << calibration.laser_corrections_.size(); - out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq; + if (next_index < num_lasers) { // anything found in this ring? + // store this ring number with its corresponding laser number + calibration.laser_corrections_[next_index].laser_ring = ring; + next_angle = min_seen; + } + } +} + +YAML::Emitter &operator<<(YAML::Emitter &out, + const std::pair &correction) { + out << YAML::BeginMap; + out << YAML::Key << LASER_ID << YAML::Value << correction.first; + out << YAML::Key << ROT_CORRECTION << YAML::Value + << correction.second.rot_correction; + out << YAML::Key << VERT_CORRECTION << YAML::Value + << correction.second.vert_correction; + out << YAML::Key << DIST_CORRECTION << YAML::Value + << correction.second.dist_correction; + out << YAML::Key << DIST_CORRECTION_X << YAML::Value + << correction.second.dist_correction_x; + out << YAML::Key << DIST_CORRECTION_Y << YAML::Value + << correction.second.dist_correction_y; + out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value + << correction.second.vert_offset_correction; + out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value + << correction.second.horiz_offset_correction; + out << YAML::Key << MAX_INTENSITY << YAML::Value + << correction.second.max_intensity; + out << YAML::Key << MIN_INTENSITY << YAML::Value + << correction.second.min_intensity; + out << YAML::Key << FOCAL_DISTANCE << YAML::Value + << correction.second.focal_distance; + out << YAML::Key << FOCAL_SLOPE << YAML::Value + << correction.second.focal_slope; + out << YAML::EndMap; + return out; +} + +YAML::Emitter &operator<<(YAML::Emitter &out, const Calibration &calibration) { + out << YAML::BeginMap; + out << YAML::Key << NUM_LASERS << YAML::Value + << calibration.laser_corrections_.size(); + out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq; + + for (std::map::const_iterator it = + calibration.laser_corrections_.begin(); + it != calibration.laser_corrections_.end(); ++it) { + out << *it; + } - for (std::map::const_iterator it = - calibration.laser_corrections_.begin(); - it != calibration.laser_corrections_.end(); ++it) - { - out << *it; - } + out << YAML::EndSeq; + out << YAML::EndMap; + return out; +} - out << YAML::EndSeq; - out << YAML::EndMap; - return out; - } +void Calibration::read(const std::string &calibration_file) { + std::ifstream fin(calibration_file.c_str()); - void Calibration::read(const std::string &calibration_file) - { - std::ifstream fin(calibration_file.c_str()); + if (!fin.is_open()) { + initialized_ = false; + return; + } - if (!fin.is_open()) - { - initialized_ = false; - return; - } + initialized_ = true; - initialized_ = true; + try { + YAML::Node doc; + fin.close(); + doc = YAML::LoadFile(calibration_file); + doc >> *this; + } catch (YAML::Exception &e) { + std::cerr << "YAML Exception: " << e.what() << std::endl; + initialized_ = false; + } - try - { - YAML::Node doc; - fin.close(); - doc = YAML::LoadFile(calibration_file); - doc >> *this; - } - catch (YAML::Exception &e) - { - std::cerr << "YAML Exception: " << e.what() << std::endl; - initialized_ = false; - } + fin.close(); +} - fin.close(); - } - - void Calibration::write(const std::string &calibration_file) - { - std::ofstream fout(calibration_file.c_str()); - YAML::Emitter out; - out << *this; - fout << out.c_str(); - fout.close(); - } +void Calibration::write(const std::string &calibration_file) { + std::ofstream fout(calibration_file.c_str()); + YAML::Emitter out; + out << *this; + fout << out.c_str(); + fout.close(); +} - } // namespace velodyne - } // namespace drivers -} // namespace apollo +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/calibration.h b/modules/drivers/lidar/lslidar/parser/calibration.h index 8e1bd027651..d789d9874d4 100644 --- a/modules/drivers/lidar/lslidar/parser/calibration.h +++ b/modules/drivers/lidar/lslidar/parser/calibration.h @@ -27,8 +27,13 @@ #pragma once +#include +#include +#include +#include #include #include +#include namespace apollo { namespace drivers { diff --git a/modules/drivers/lidar/lslidar/parser/convert.cc b/modules/drivers/lidar/lslidar/parser/convert.cc index 015ebc32d1d..a34a74062ff 100644 --- a/modules/drivers/lidar/lslidar/parser/convert.cc +++ b/modules/drivers/lidar/lslidar/parser/convert.cc @@ -40,11 +40,11 @@ void Convert::init(const Config& lslidar_config) { void Convert::ConvertPacketsToPointcloud( const std::shared_ptr& scan_msg, std::shared_ptr point_cloud) { - - AINFO_EVERY(100) << "Converting scan msg seq " << scan_msg->header().sequence_num(); - + AINFO_EVERY(100) << "Converting scan msg seq " + << scan_msg->header().sequence_num(); + parser_->GeneratePointcloud(scan_msg, point_cloud); - + if (point_cloud == nullptr || point_cloud->point().empty()) { AERROR << "point cloud has no point"; return; diff --git a/modules/drivers/lidar/lslidar/parser/convert.h b/modules/drivers/lidar/lslidar/parser/convert.h index eeddaafa182..17a108c9d6d 100644 --- a/modules/drivers/lidar/lslidar/parser/convert.h +++ b/modules/drivers/lidar/lslidar/parser/convert.h @@ -19,12 +19,13 @@ #include #include -//#include "modules/drivers/proto/pointcloud.pb.h" +// #include "modules/drivers/proto/pointcloud.pb.h" #include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" -#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" #include "modules/drivers/lidar/lslidar/proto/config.pb.h" #include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + namespace apollo { namespace drivers { namespace lslidar { @@ -42,8 +43,9 @@ class Convert { void init(const Config& lslidar_config); // convert lslidar data to pointcloud and public - void ConvertPacketsToPointcloud(const std::shared_ptr& scan_msg, - std::shared_ptr point_cloud_out); + void ConvertPacketsToPointcloud( + const std::shared_ptr& scan_msg, + std::shared_ptr point_cloud_out); private: // RawData class for converting data to point cloud diff --git a/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc index ccc31fb0a16..73411d89393 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc @@ -53,7 +53,7 @@ Lslidar16Parser::Lslidar16Parser(const Config &config) void Lslidar16Parser::GeneratePointcloud( const std::shared_ptr &scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr &out_msg) { // 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); @@ -79,7 +79,8 @@ void Lslidar16Parser::GeneratePointcloud( vert_angle = vert_angle - 65535; } - scan_altitude[i] = ((float)vert_angle / 100.f) * DEG_TO_RAD; + scan_altitude[i] = + (static_cast(vert_angle) / 100.f) * DEG_TO_RAD; if (2 == config_.degree_mode()) { if (scan_altitude[i] != 0) { if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * @@ -112,7 +113,8 @@ void Lslidar16Parser::GeneratePointcloud( vert_angle = vert_angle - 65535; } - scan_altitude[i] = ((float)vert_angle / 100.f) * DEG_TO_RAD; + scan_altitude[i] = + (static_cast(vert_angle) / 100.f) * DEG_TO_RAD; if (2 == config_.degree_mode()) { if (scan_altitude[i] != 0) { @@ -144,8 +146,6 @@ void Lslidar16Parser::GeneratePointcloud( packet_number_ = packets_size; block_num = 0; - AERROR << "packets_size:" << packets_size; - for (size_t i = 0; i < packets_size; ++i) { Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); last_time_stamp_ = out_msg->measurement_time(); @@ -186,7 +186,6 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, const raw_packet_t *raw = (const raw_packet_t *)pkt.data().c_str(); if (!config_.time_synchronization()) { - packet_end_time = pkt.stamp(); } else { packet_end_time = pkt.stamp(); @@ -200,8 +199,7 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, raw->blocks[block].rotation_1); if (2 == config_.return_mode()) { - if (block < (BLOCKS_PER_PACKET - 2)) // 12 - { + if (block < (BLOCKS_PER_PACKET - 2)) { int azi1, azi2; azi1 = 256 * raw->blocks[block + 2].rotation_2 + raw->blocks[block + 2].rotation_1; @@ -217,8 +215,7 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); } } else { - if (block < (BLOCKS_PER_PACKET - 1)) // 12 - { + if (block < (BLOCKS_PER_PACKET - 1)) { int azi1, azi2; azi1 = 256 * raw->blocks[block + 1].rotation_2 + raw->blocks[block + 1].rotation_1; @@ -272,7 +269,8 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, distance2 = distance2 + corrections.dist_correction; // The offset calibration - float arg_horiz = (float)azimuth_corrected_f * ROTATION_RESOLUTION; + float arg_horiz = + static_cast(azimuth_corrected_f * ROTATION_RESOLUTION); arg_horiz = arg_horiz > 360 ? (arg_horiz - 360) : arg_horiz; float arg_horiz_orginal = (14.67 - arg_horiz) * M_PI / 180; @@ -286,7 +284,7 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, } else { // fist packet point_time = current_packet_time; } - + } else { // modify if (previous_packet_time > 1e-6) { @@ -325,11 +323,11 @@ void Lslidar16Parser::Unpack(const LslidarPacket &pkt, } else { PointXYZIT *point = pc->add_point(); point->set_timestamp(point_time); - + point->set_intensity(intensity); if (config_.calibration()) { - ComputeCoords(distance2, corrections, + ComputeCoords(distance2, &corrections, static_cast(azimuth_corrected), point); } else { diff --git a/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc index ef39a7b70c3..badbfe22261 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc @@ -16,11 +16,10 @@ #include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" - namespace apollo { namespace drivers { namespace lslidar { - + Lslidar32Parser::Lslidar32Parser(const Config& config) : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { adjust_angle = 0; @@ -34,142 +33,169 @@ Lslidar32Parser::Lslidar32Parser(const Config& config) void Lslidar32Parser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); + 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(); double scan_altitude_original_degree33[32]; - double scan_altitude_original_degree1[32]; + double scan_altitude_original_degree1[32]; int startpos = 0; - - 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){ - if((difop_ptr[1202] >= 0x02 && difop_ptr[1203] >= 0x80) || difop_ptr[1202] == 0x03){ - lslidar_type = 3; - startpos = 245; - //Horizontal correction Angle - adjust_angle = (difop_ptr[186]*256 + difop_ptr[187]); //Angle correction A1 - if(adjust_angle > 32767){ - adjust_angle = adjust_angle - 65535; - } - adjust_angle_two = (difop_ptr[190]*256 + difop_ptr[191]); //Angle correction A2 - if(adjust_angle_two > 32767){ - adjust_angle_two = adjust_angle_two - 65535; - } - adjust_angle_three = (difop_ptr[188]*256 + difop_ptr[189]); //Angle correction A3 - if(adjust_angle_three > 32767){ - adjust_angle_three = adjust_angle_three - 65535; - } - adjust_angle_four = (difop_ptr[192]*256 + difop_ptr[193]); //Angle correction A4 - if(adjust_angle_four > 32767){ - adjust_angle_four = adjust_angle_four - 65535; - } - memcpy(scan_altitude_original_degree1,scan_altitude_original_A3,32*8); - memcpy(scan_altitude_original_degree33,scan_altitude_original_C3,32*8); - - if(difop_ptr[185] == 0 || difop_ptr[185] == 1) - { - int return_mode = difop_ptr[185]+1; - config_.set_return_mode(return_mode); - - if(difop_ptr[1195] == 0x21 ) - config_.set_degree_mode(2); - else - config_.set_degree_mode(1); - - config_.set_distance_unit(0.4f); - - for(int i = 0; i < LSC32_SCANS_PER_FIRING; i++) - { - //均匀1度校准两列 - if(1 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_original_A3[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_original_A3[i] * M_PI) / 180.0f); - scan_altitude_A[i] = scan_altitude_original_A3[i]; - } - //0.33度校准四列 - if(2 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_original_C3[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_original_C3[i] * M_PI) / 180.0f); - scan_altitude_C[i] = scan_altitude_original_C3[i]; - } + + 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) { + if ((difop_ptr[1202] >= 0x02 && difop_ptr[1203] >= 0x80) || + difop_ptr[1202] == 0x03) { + lslidar_type = 3; + startpos = 245; + // Horizontal correction Angle + adjust_angle = + (difop_ptr[186] * 256 + difop_ptr[187]); // Angle correction A1 + if (adjust_angle > 32767) { + adjust_angle = adjust_angle - 65535; + } + adjust_angle_two = + (difop_ptr[190] * 256 + difop_ptr[191]); // Angle correction A2 + if (adjust_angle_two > 32767) { + adjust_angle_two = adjust_angle_two - 65535; + } + adjust_angle_three = + (difop_ptr[188] * 256 + difop_ptr[189]); // Angle correction A3 + if (adjust_angle_three > 32767) { + adjust_angle_three = adjust_angle_three - 65535; + } + adjust_angle_four = + (difop_ptr[192] * 256 + difop_ptr[193]); // Angle correction A4 + if (adjust_angle_four > 32767) { + adjust_angle_four = adjust_angle_four - 65535; + } + memcpy(scan_altitude_original_degree1, scan_altitude_original_A3, 32 * 8); + memcpy(scan_altitude_original_degree33, scan_altitude_original_C3, + 32 * 8); + + if (difop_ptr[185] == 0 || difop_ptr[185] == 1) { + int return_mode = difop_ptr[185] + 1; + config_.set_return_mode(return_mode); + + if (difop_ptr[1195] == 0x21) + config_.set_degree_mode(2); + else + config_.set_degree_mode(1); + + config_.set_distance_unit(0.4f); + + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_A3[i] * M_PI) / + 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_A3[i] * M_PI) / + 180.0f); + scan_altitude_A[i] = scan_altitude_original_A3[i]; + } + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_C3[i] * M_PI) / + 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_C3[i] * M_PI) / + 180.0f); + scan_altitude_C[i] = scan_altitude_original_C3[i]; } } - }else{ - lslidar_type = 2; - startpos = 882; - //Horizontal correction Angle - adjust_angle = (difop_ptr[34]*256 + difop_ptr[35]); /*Angle correction A1*/ - if(adjust_angle > 32767){ - adjust_angle = adjust_angle - 65535; - } - adjust_angle_two = (difop_ptr[42]*256 + difop_ptr[43]); /*Angle correction A2*/ - if(adjust_angle_two > 32767){ - adjust_angle_two = adjust_angle_two - 65535; + } + } else { + lslidar_type = 2; + startpos = 882; + // Horizontal correction Angle + adjust_angle = + (difop_ptr[34] * 256 + difop_ptr[35]); /*Angle correction A1*/ + if (adjust_angle > 32767) { + adjust_angle = adjust_angle - 65535; + } + adjust_angle_two = + (difop_ptr[42] * 256 + difop_ptr[43]); /*Angle correction A2*/ + if (adjust_angle_two > 32767) { + adjust_angle_two = adjust_angle_two - 65535; + } + adjust_angle_three = + (difop_ptr[66] * 256 + difop_ptr[67]); /*Angle correction A3*/ + if (adjust_angle_three > 32767) { + adjust_angle_three = adjust_angle_three - 65535; + } + adjust_angle_four = + (difop_ptr[68] * 256 + difop_ptr[69]); /*Angle correction A4*/ + if (adjust_angle_four > 32767) { + adjust_angle_four = adjust_angle_four - 65535; + } + AWARN << "Load config failed, config file"; + // Vertical Angle Calibration for device package + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); + scan_altitude_A[i] = scan_altitude_original_A[i]; } - adjust_angle_three = (difop_ptr[66]*256 + difop_ptr[67]); /*Angle correction A3*/ - if(adjust_angle_three > 32767){ - adjust_angle_three = adjust_angle_three - 65535; + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); + scan_altitude_C[i] = scan_altitude_original_C[i]; } - adjust_angle_four = (difop_ptr[68]*256 + difop_ptr[69]); /*Angle correction A4*/ - if(adjust_angle_four > 32767){ - adjust_angle_four = adjust_angle_four - 65535; + } + memcpy(scan_altitude_original_degree1, scan_altitude_original_A, 32 * 8); + memcpy(scan_altitude_original_degree33, scan_altitude_original_C, 32 * 8); + } + } + + // Vertical Angle parse + if (config_.config_vert()) { + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + uint8_t data1 = difop_ptr[startpos + 2 * i]; + uint8_t data2 = difop_ptr[startpos + 2 * i + 1]; + int vert_angle = data1 * 256 + data2; + if (vert_angle > 32767) { + vert_angle = vert_angle - 65535; + } + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + scan_altitude_A[i] = + static_cast(vert_angle * ROTATION_RESOLUTION); + if (fabs(scan_altitude_original_degree1[i] - scan_altitude_A[i]) > + 1.5) { + scan_altitude_A[i] = scan_altitude_original_degree1[i]; } - AWARN << "Load config failed, config file"; - //Vertical Angle Calibration for device package - for(int i = 0; i < LSC32_SCANS_PER_FIRING; i++) - { - //均匀1度校准两列 - if(1 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); - scan_altitude_A[i] = scan_altitude_original_A[i]; - } - //0.33度校准四列 - if(2 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); - scan_altitude_C[i] = scan_altitude_original_C[i]; - } + config_vert_angle = true; + } + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + scan_altitude_C[i] = + static_cast(vert_angle * ROTATION_RESOLUTION); + if (fabs(scan_altitude_original_degree33[i] - scan_altitude_C[i]) > + 1.5) { + scan_altitude_C[i] = scan_altitude_original_degree33[i]; } - memcpy(scan_altitude_original_degree1,scan_altitude_original_A,32*8); - memcpy(scan_altitude_original_degree33,scan_altitude_original_C,32*8); + config_vert_angle = true; } + } } - - //Vertical Angle parse - if(config_.config_vert()){ - for(int i = 0; i < LSC32_SCANS_PER_FIRING; i++){ - uint8_t data1 = difop_ptr[startpos + 2*i]; - uint8_t data2 = difop_ptr[startpos + 2*i+1]; - int vert_angle = data1*256 + data2; - if(vert_angle > 32767){ - vert_angle = vert_angle - 65535; - } - //均匀1度校准两列 - if(1 == config_.degree_mode()){ - scan_altitude_A[i] = (double)(vert_angle * ROTATION_RESOLUTION); - if(fabs(scan_altitude_original_degree1[i] - scan_altitude_A[i]) > 1.5){ - scan_altitude_A[i] = scan_altitude_original_degree1[i]; - } - config_vert_angle = true; - } - //0.33度校准四列 - if(2 == config_.degree_mode()){ - scan_altitude_C[i] = (double)(vert_angle * ROTATION_RESOLUTION); - if(fabs(scan_altitude_original_degree33[i] - scan_altitude_C[i]) > 1.5){ - scan_altitude_C[i] = scan_altitude_original_degree33[i]; - } - config_vert_angle = true; - } - } - } size_t packets_size = scan_msg->firing_pkts_size(); block_num = 0; @@ -178,7 +204,7 @@ void Lslidar32Parser::GeneratePointcloud( AINFO << "packets_size :" << packets_size; for (size_t i = 0; i < packets_size; ++i) { - Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg,i); + Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -192,265 +218,313 @@ void Lslidar32Parser::GeneratePointcloud( out_msg->set_width(out_msg->point_size()); } - /** @brief convert raw packet to point cloud * @param pkt raw packet to Unpack * @param pc shared pointer to point cloud (points are appended) */ void Lslidar32Parser::Unpack(const LslidarPacket& pkt, - std::shared_ptr pc ,int packet_number) { + std::shared_ptr pc, + int packet_number) { float x, y, z; float azimuth = 0.0f; - uint32_t intensity = 0; + uint32_t intensity = 0; float azimuth_diff = 0.0f; float azimuth_corrected_f = 0.0f; float azimuth_corrected_offset_f = 0.0f; uint64_t point_time; - uint64_t packet_end_time; - - - if (config_vert_angle) { - for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { - //均匀1度校准两列 - if(1 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); - } - - //0.33度校准四列 - if(2 == config_.degree_mode()){ - cos_scan_altitude_caliration[i] = std::cos(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); - sin_scan_altitude_caliration[i] = std::sin(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); - } - } - config_vert_angle = false; - } + uint64_t packet_end_time; + + if (config_vert_angle) { + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = + std::cos(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = + std::sin(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); + } + + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = + std::cos(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = + std::sin(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); + } + } + config_vert_angle = false; + } const raw_packet_t* raw = (const raw_packet_t*)pkt.data().c_str(); - if (!config_.time_synchronization()){ + if (!config_.time_synchronization()) { packet_end_time = pkt.stamp(); - } - else{ + } else { packet_end_time = pkt.stamp(); } current_packet_time = packet_end_time; - int point_count=-1; + int point_count = -1; for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_num++) { - if (UPPER_BANK != raw->blocks[block].header) break; - - azimuth =static_cast(256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1); - - if(2 == config_.return_mode()){ - if (block < (BLOCKS_PER_PACKET - 2)) // 12 - { - int azi1, azi2; - azi1 = 256 * raw->blocks[block + 2].rotation_2 + raw->blocks[block + 2].rotation_1; - azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; - azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); - } else { - int azi1, azi2; - azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; - azi2 = 256 * raw->blocks[block - 2].rotation_2 + raw->blocks[block - 2].rotation_1; - azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); - } - azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; - azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; - } else { - if (block < (BLOCKS_PER_PACKET - 1)) // 12 - { - int azi1, azi2; - azi1 = 256 * raw->blocks[block + 1].rotation_2 + raw->blocks[block + 1].rotation_1; - azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; - azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); - } else { - int azi1, azi2; - azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; - azi2 = 256 * raw->blocks[block - 1].rotation_2 + raw->blocks[block - 1].rotation_1; - azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); - } - azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; - azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; - } - + if (UPPER_BANK != raw->blocks[block].header) break; + + azimuth = static_cast(256 * raw->blocks[block].rotation_2 + + raw->blocks[block].rotation_1); + + if (2 == config_.return_mode()) { + if (block < (BLOCKS_PER_PACKET - 2)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 2].rotation_2 + + raw->blocks[block + 2].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 2].rotation_2 + + raw->blocks[block - 2].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; + azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; + } else { + if (block < (BLOCKS_PER_PACKET - 1)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 1].rotation_2 + + raw->blocks[block + 1].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 1].rotation_2 + + raw->blocks[block - 1].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; + azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; + } + for (int firing = 0, k = 0; firing < LSC32_FIRINGS_PER_BLOCK; ++firing) { - for (int dsr = 0; dsr < LSC32_SCANS_PER_FIRING; ++dsr, k += RAW_SCAN_SIZE) { + for (int dsr = 0; dsr < LSC32_SCANS_PER_FIRING; + ++dsr, k += RAW_SCAN_SIZE) { point_count++; - /** correct for the laser rotation as a function of timing during the firings **/ + /** correct for the laser rotation as a function of timing during the + * firings **/ LaserCorrection& corrections = calibration_.laser_corrections_[dsr]; - - azimuth_corrected_f = azimuth + azimuth_diff / LSC32_SCANS_PER_FIRING * dsr; - azimuth_corrected_f = azimuth_corrected_f < 0 ? azimuth_corrected_f + 36000 : azimuth_corrected_f; - azimuth_corrected_f = azimuth_corrected_f >36000 ? azimuth_corrected_f - 36000 : azimuth_corrected_f; - - //distance + + azimuth_corrected_f = + azimuth + azimuth_diff / LSC32_SCANS_PER_FIRING * dsr; + azimuth_corrected_f = azimuth_corrected_f < 0 + ? azimuth_corrected_f + 36000 + : azimuth_corrected_f; + azimuth_corrected_f = azimuth_corrected_f > 36000 + ? azimuth_corrected_f - 36000 + : azimuth_corrected_f; + + // distance union two_bytes tmp; tmp.bytes[0] = raw->blocks[block].data[k]; tmp.bytes[1] = raw->blocks[block].data[k + 1]; int distance = tmp.uint; - //read intensity + // read intensity intensity = raw->blocks[block].data[k + 2]; - - float distance2 = (distance * DISTANCE_RESOLUTION) * config_.distance_unit(); - - if (config_.calibration()) + + float distance2 = + (distance * DISTANCE_RESOLUTION) * config_.distance_unit(); + + if (config_.calibration()) distance2 = distance2 + corrections.dist_correction; - if (2 == config_.return_mode()){ - if (previous_packet_time >1e-6) { - point_time = current_packet_time - (SCANS_PER_PACKET - point_count - 1) * (current_packet_time - previous_packet_time)/(SCANS_PER_PACKET); - } else { //fist packet + if (2 == config_.return_mode()) { + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet point_time = current_packet_time; } } else { - if(previous_packet_time >1e-6) { - point_time = current_packet_time - (SCANS_PER_PACKET - point_count - 1) * (current_packet_time - previous_packet_time)/(SCANS_PER_PACKET); - } else{ //fist packet + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet point_time = current_packet_time; } } - - if (distance2 > config_.max_range() || distance2 < config_.min_range()) { - PointXYZIT* point = pc->add_point(); - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - } else { - //均匀1度校准两列 - if(1 == config_.degree_mode()) - { - double adjust_diff = adjust_angle_two - adjust_angle; - if(adjust_diff > 300 && adjust_diff < 460){ - //v2.7 calibrtation - if(lslidar_type == 3) - { - if(1 >= dsr % 4){ - azimuth_corrected_f += adjust_angle_two; - }else{ - azimuth_corrected_f += adjust_angle; - } - }else{ - if(0 == dsr % 2){ - azimuth_corrected_f += adjust_angle_two; - }else{ - azimuth_corrected_f += adjust_angle; - } - } - }else{ - //v2.6 calibrtation - if(0 == dsr % 2){ - azimuth_corrected_f += adjust_angle; - }else{ - azimuth_corrected_f -= adjust_angle; - } - } - } - //0.33度校准四列 - if(2 == config_.degree_mode()) - { - double adjust_diff_one = adjust_angle_two - adjust_angle; - double adjust_diff_two = adjust_angle_four - adjust_angle_three; - if(lslidar_type == 3) - { - //v3.0 calibrtation - if(0 == dsr || 1 == dsr || 4 == dsr || 8 == dsr || 9 == dsr || 12 == dsr || 16 == dsr || 17 == dsr || 21 == dsr || 24 == dsr || 25 == dsr || 29 == dsr) - azimuth_corrected_f += adjust_angle_four; //A4 - - if(2 == dsr || 3 == dsr || 6 == dsr || 10 == dsr || 11 == dsr || 14 == dsr || 18 == dsr || 19 == dsr || 23 == dsr || 26 == dsr || 27 == dsr || 31 == dsr) - azimuth_corrected_f += adjust_angle_three; //A3 - - if(5== dsr || 13 == dsr || 20 == dsr || 28 == dsr) - azimuth_corrected_f += adjust_angle_two; //A2 - - if(7 == dsr || 15 == dsr || 22 == dsr || 30 == dsr) - azimuth_corrected_f += adjust_angle; //A1 - } - else if(adjust_diff_one > 500 && adjust_diff_one < 660 && adjust_diff_two > 150 && adjust_diff_two < 350) - { - //v2.7 calibrtation - if(10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) - azimuth_corrected_f += adjust_angle_four; //A4 - - if(11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) - azimuth_corrected_f += adjust_angle_three; //A3 - - if(0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || 26 == dsr || 28 == dsr || 30 == dsr) - azimuth_corrected_f += adjust_angle_two; //A2 - - if(1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || 27 == dsr || 29 == dsr || 31 == dsr) - azimuth_corrected_f += adjust_angle; //A1 - }else{ - //v2.6 calibrtation - if(10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) + if (distance2 > config_.max_range() || + distance2 < config_.min_range()) { + PointXYZIT* point = pc->add_point(); + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + double adjust_diff = adjust_angle_two - adjust_angle; + if (adjust_diff > 300 && adjust_diff < 460) { + // v2.7 calibrtation + if (lslidar_type == 3) { + if (1 >= dsr % 4) { + azimuth_corrected_f += adjust_angle_two; + } else { azimuth_corrected_f += adjust_angle; - - if(11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) - azimuth_corrected_f -= adjust_angle; - - if(0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || 26 == dsr || 28 == dsr || 30 == dsr) + } + } else { + if (0 == dsr % 2) { azimuth_corrected_f += adjust_angle_two; - - if(1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || 27 == dsr || 29 == dsr || 31 == dsr) - azimuth_corrected_f -= adjust_angle_two; + } else { + azimuth_corrected_f += adjust_angle; + } + } + } else { + // v2.6 calibrtation + if (0 == dsr % 2) { + azimuth_corrected_f += adjust_angle; + } else { + azimuth_corrected_f -= adjust_angle; } } + } - azimuth_corrected_f = azimuth_corrected_f < 0 ? azimuth_corrected_f + 36000 : azimuth_corrected_f; - azimuth_corrected_f = azimuth_corrected_f >36000 ? azimuth_corrected_f - 36000 : azimuth_corrected_f; + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + double adjust_diff_one = adjust_angle_two - adjust_angle; + double adjust_diff_two = adjust_angle_four - adjust_angle_three; + if (lslidar_type == 3) { + // v3.0 calibrtation + if (0 == dsr || 1 == dsr || 4 == dsr || 8 == dsr || 9 == dsr || + 12 == dsr || 16 == dsr || 17 == dsr || 21 == dsr || + 24 == dsr || 25 == dsr || 29 == dsr) + azimuth_corrected_f += adjust_angle_four; // A4 + + if (2 == dsr || 3 == dsr || 6 == dsr || 10 == dsr || 11 == dsr || + 14 == dsr || 18 == dsr || 19 == dsr || 23 == dsr || + 26 == dsr || 27 == dsr || 31 == dsr) + azimuth_corrected_f += adjust_angle_three; // A3 + + if (5 == dsr || 13 == dsr || 20 == dsr || 28 == dsr) + azimuth_corrected_f += adjust_angle_two; // A2 + + if (7 == dsr || 15 == dsr || 22 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle; // A1 + } else if (adjust_diff_one > 500 && adjust_diff_one < 660 && + adjust_diff_two > 150 && adjust_diff_two < 350) { + // v2.7 calibrtation + if (10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) + azimuth_corrected_f += adjust_angle_four; // A4 + + if (11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) + azimuth_corrected_f += adjust_angle_three; // A3 + + if (0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || + 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || + 26 == dsr || 28 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle_two; // A2 + + if (1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || + 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || + 27 == dsr || 29 == dsr || 31 == dsr) + azimuth_corrected_f += adjust_angle; // A1 + } else { + // v2.6 calibrtation + if (10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) + azimuth_corrected_f += adjust_angle; + + if (11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) + azimuth_corrected_f -= adjust_angle; + + if (0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || + 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || + 26 == dsr || 28 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle_two; + + if (1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || + 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || + 27 == dsr || 29 == dsr || 31 == dsr) + azimuth_corrected_f -= adjust_angle_two; + } + } - if ((azimuth_corrected_f < config_.scan_start_angle()) || (azimuth_corrected_f > config_.scan_end_angle())) continue; - if (packet_number == 0) { - if (azimuth_corrected_f > 30000) { - continue; - } - } + azimuth_corrected_f = azimuth_corrected_f < 0 + ? azimuth_corrected_f + 36000 + : azimuth_corrected_f; + azimuth_corrected_f = azimuth_corrected_f > 36000 + ? azimuth_corrected_f - 36000 + : azimuth_corrected_f; + + if ((azimuth_corrected_f < config_.scan_start_angle()) || + (azimuth_corrected_f > config_.scan_end_angle())) + continue; + if (packet_number == 0) { + if (azimuth_corrected_f > 30000) { + continue; + } + } - if (packet_number == packet_number_ - 1) { - if (azimuth_corrected_f < 10000) { - continue; - } - } + if (packet_number == packet_number_ - 1) { + if (azimuth_corrected_f < 10000) { + continue; + } + } - //以结构为中心 - float rotation_azimuth = ROTATION_RESOLUTION * static_cast(azimuth_corrected_f * M_PI) / 180.0f; - azimuth_corrected_offset_f = azimuth_corrected_f*ROTATION_RESOLUTION - LSC32_AZIMUTH_TOFFSET; - float rotation_azimuth_offset = static_cast(azimuth_corrected_offset_f * M_PI) / 180.0f; - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(intensity); - point->set_intensity(intensity); - - if (config_.calibration()) { - ComputeCoords(distance2, corrections, - static_cast(azimuth_corrected_f), point); - }else{ - x = distance2 * cos_scan_altitude_caliration[dsr] * cosf(rotation_azimuth) + (LSC32_DISTANCE_TOFFSET * cosf(rotation_azimuth_offset)) * DISTANCE_RESOLUTION; - y = -(distance2 * cos_scan_altitude_caliration[dsr] * sinf(rotation_azimuth) + (LSC32_DISTANCE_TOFFSET * sinf(rotation_azimuth_offset)) * DISTANCE_RESOLUTION); - z = distance2 * sin_scan_altitude_caliration[dsr]; - - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } + // 以结构为中心 + float rotation_azimuth = + ROTATION_RESOLUTION * + static_cast(azimuth_corrected_f * M_PI) / 180.0f; + azimuth_corrected_offset_f = + azimuth_corrected_f * ROTATION_RESOLUTION - LSC32_AZIMUTH_TOFFSET; + float rotation_azimuth_offset = + static_cast(azimuth_corrected_offset_f * M_PI) / 180.0f; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(intensity); + point->set_intensity(intensity); + + if (config_.calibration()) { + ComputeCoords(distance2, &corrections, + static_cast(azimuth_corrected_f), point); + } else { + x = distance2 * cos_scan_altitude_caliration[dsr] * + cosf(rotation_azimuth) + + (LSC32_DISTANCE_TOFFSET * cosf(rotation_azimuth_offset)) * + DISTANCE_RESOLUTION; + y = -(distance2 * cos_scan_altitude_caliration[dsr] * + sinf(rotation_azimuth) + + (LSC32_DISTANCE_TOFFSET * sinf(rotation_azimuth_offset)) * + DISTANCE_RESOLUTION); + z = distance2 * sin_scan_altitude_caliration[dsr]; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && + -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); } } } } } + } previous_packet_time = current_packet_time; } diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc index eb3f1047d8a..10abf3507a1 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc @@ -19,28 +19,31 @@ namespace apollo { namespace drivers { namespace lslidar { - + LslidarCH120Parser::LslidarCH120Parser(const Config& config) : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { - last_packet_time = 0; + last_packet_time = 0; } void LslidarCH120Parser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); + 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(); - packets_size = scan_msg->firing_pkts_size(); + packets_size = scan_msg->firing_pkts_size(); for (size_t i = 0; i < packets_size; ++i) { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -58,84 +61,90 @@ void LslidarCH120Parser::GeneratePointcloud( * @param pc shared pointer to point cloud (points are appended) */ void LslidarCH120Parser::Unpack(int num, const LslidarPacket& pkt, - std::shared_ptr pc) { + std::shared_ptr pc) { float x, y, z; - uint64_t packet_end_time; + uint64_t packet_end_time; double z_sin_altitude = 0.0; - double z_cos_altitude = 0.0; + double z_cos_altitude = 0.0; const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); time_last = 0; - + packet_end_time = pkt.stamp(); - for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) - { - firings[point_idx1].vertical_line=raw->points[point_idx1].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; - firings[point_idx1].azimuth=static_cast(point_amuzith.uint*0.01*DEG_TO_RAD); - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx1].distance_3; - point_distance.bytes[1] = raw->points[point_idx1].distance_2; - point_distance.bytes[2] = raw->points[point_idx1].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx1].distance=static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx1].intensity=raw->points[point_idx1].intensity; - } - - for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection& corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - z_sin_altitude = sin((-13 + 0.167 * firings[point_idx].vertical_line)*DEG_TO_RAD); - z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); - x = firings[point_idx].distance * z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance * z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance * z_sin_altitude; - - // Compute the time of the point - uint64_t point_time = packet_end_time - 1282*(POINTS_PER_PACKET - point_idx - 1); - if(time_last < point_time && time_last > 0){ - point_time = time_last + 1282; - } - time_last = point_time; - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) { - - ComputeCoords2(firings[point_idx].vertical_line, CH120, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - - } else { - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } -} + for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) { + firings[point_idx1].vertical_line = raw->points[point_idx1].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; + firings[point_idx1].azimuth = + static_cast(point_amuzith.uint * 0.01 * DEG_TO_RAD); + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx1].distance_3; + point_distance.bytes[1] = raw->points[point_idx1].distance_2; + point_distance.bytes[2] = raw->points[point_idx1].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx1].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx1].intensity = raw->points[point_idx1].intensity; + } + + for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; -void LslidarCH120Parser::Order(std::shared_ptr cloud) { + z_sin_altitude = + sin((-13 + 0.167 * firings[point_idx].vertical_line) * DEG_TO_RAD); + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1282 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1282; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH120, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } } +void LslidarCH120Parser::Order(std::shared_ptr cloud) {} + } // namespace lslidar } // namespace drivers } // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc index 1a6636bd1d2..c35c66398c3 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc @@ -19,39 +19,40 @@ namespace apollo { namespace drivers { namespace lslidar { - + LslidarCH128X1Parser::LslidarCH128X1Parser(const Config& config) - : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { - -} + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} void LslidarCH128X1Parser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); - + 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(); + two_bytes angle_1, angle_2, angle_3, angle_4; angle_1.bytes[0] = difop_ptr[243]; angle_1.bytes[1] = difop_ptr[242]; prism_angle[0] = angle_1.uint * 0.01; - + angle_2.bytes[0] = difop_ptr[245]; angle_2.bytes[1] = difop_ptr[244]; prism_angle[1] = angle_2.uint * 0.01; - + angle_3.bytes[0] = difop_ptr[247]; angle_3.bytes[1] = difop_ptr[246]; prism_angle[2] = angle_3.uint * 0.01; - + angle_4.bytes[0] = difop_ptr[249]; angle_4.bytes[1] = difop_ptr[248]; prism_angle[3] = angle_4.uint * 0.01; @@ -59,7 +60,8 @@ void LslidarCH128X1Parser::GeneratePointcloud( packets_size = scan_msg->firing_pkts_size(); for (size_t i = 0; i < packets_size; ++i) { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -74,120 +76,129 @@ void LslidarCH128X1Parser::GeneratePointcloud( out_msg->set_width(out_msg->point_size()); } - /** @brief convert raw packet to point cloud * @param pkt raw packet to Unpack * @param pc shared pointer to point cloud (points are appended) */ void LslidarCH128X1Parser::Unpack(int num, const LslidarPacket& pkt, - std::shared_ptr pc) { - float x, y, z; - uint64_t point_time; - uint64_t packet_end_time; - double z_sin_altitude = 0.0; - double z_cos_altitude = 0.0; - double sinTheta_1[128] = {0}; - double cosTheta_1[128] = {0}; - double sinTheta_2[128] = {0}; - double cosTheta_2[128] = {0}; - - for (int i = 0; i < 128; i++) { - sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); - cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); - - if(abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) - { - sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); - cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); - }else{ - sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); - cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); - } - } - + std::shared_ptr pc) { + float x, y, z; + uint64_t point_time; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + double sinTheta_1[128] = {0}; + double cosTheta_1[128] = {0}; + double sinTheta_2[128] = {0}; + double cosTheta_2[128] = {0}; + + for (int i = 0; i < 128; i++) { + sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); + cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); + cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); + } else { + sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); + cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); + } + } + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); packet_end_time = pkt.stamp(); current_packet_time = packet_end_time; - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - firings[point_idx].vertical_line=raw->points[point_idx].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; - firings[point_idx].azimuth=static_cast(point_amuzith.uint)*0.01*DEG_TO_RAD; - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx].distance_3; - point_distance.bytes[1] = raw->points[point_idx].distance_2; - point_distance.bytes[2] = raw->points[point_idx].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx].distance=static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx].intensity=raw->points[point_idx].intensity; - } - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection& corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - double _R_ = cosTheta_2[firings[point_idx].vertical_line] * cosTheta_1[firings[point_idx].vertical_line] * + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + double _R_ = cosTheta_2[firings[point_idx].vertical_line] * + cosTheta_1[firings[point_idx].vertical_line] * cos(firings[point_idx].azimuth / 2.0) - - sinTheta_2[firings[point_idx].vertical_line] * sinTheta_1[firings[point_idx].vertical_line]; - - z_sin_altitude = sinTheta_1[firings[point_idx].vertical_line] + - 2 * _R_ * sinTheta_2[firings[point_idx].vertical_line]; - - z_cos_altitude = sqrt(1 - pow(z_sin_altitude, 2)); - x = firings[point_idx].distance * z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance * z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance * z_sin_altitude; - - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) return; - - // Compute the time of the point - // modify - if(previous_packet_time >1e-6) { - point_time = current_packet_time - (POINTS_PER_PACKET - point_idx - 1) * (current_packet_time - previous_packet_time)/(POINTS_PER_PACKET); - } else{ //fist packet - point_time = current_packet_time; - } - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) { - - ComputeCoords2(firings[point_idx].vertical_line, CH128X1, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - - } else { - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } + sinTheta_2[firings[point_idx].vertical_line] * + sinTheta_1[firings[point_idx].vertical_line]; + + z_sin_altitude = sinTheta_1[firings[point_idx].vertical_line] + + 2 * _R_ * sinTheta_2[firings[point_idx].vertical_line]; + + z_cos_altitude = sqrt(1 - pow(z_sin_altitude, 2)); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) + return; + + // Compute the time of the point + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (POINTS_PER_PACKET - point_idx - 1) * + (current_packet_time - previous_packet_time) / + (POINTS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH128X1, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } previous_packet_time = current_packet_time; } -void LslidarCH128X1Parser::Order(std::shared_ptr cloud) { -} +void LslidarCH128X1Parser::Order(std::shared_ptr cloud) {} } // namespace lslidar } // namespace drivers diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc index 70360866147..e5dc64c14f1 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc @@ -16,182 +16,172 @@ #include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - LslidarCH128Parser::LslidarCH128Parser(const Config &config) - : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) - { - last_packet_time = 0; +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH128Parser::LslidarCH128Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + last_packet_time = 0; +} + +void LslidarCH128Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // 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(); + + two_bytes angle_1, angle_2, angle_3, angle_4; + angle_1.bytes[0] = difop_ptr[87]; + angle_1.bytes[1] = difop_ptr[86]; + prism_angle128[0] = angle_1.uint * 0.01; + + angle_2.bytes[0] = difop_ptr[89]; + angle_2.bytes[1] = difop_ptr[88]; + prism_angle128[1] = angle_2.uint * 0.01; + + angle_3.bytes[0] = difop_ptr[91]; + angle_3.bytes[1] = difop_ptr[90]; + prism_angle128[2] = angle_3.uint * 0.01; + + angle_4.bytes[0] = difop_ptr[93]; + angle_4.bytes[1] = difop_ptr[92]; + prism_angle128[3] = angle_4.uint * 0.01; + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + 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()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH128Parser::Unpack(int num, const LslidarPacket &pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + double cos_azimuth_half = 0.0; + const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); + time_last = 0; + + packet_end_time = pkt.stamp(); + + for (int i = 0; i < 128; i++) { + if (abs(prism_angle128[0]) < 1e-6 && abs(prism_angle128[1]) < 1e-6 && + abs(prism_angle128[2]) < 1e-6 && abs(prism_angle128[3]) < 1e-6) { + sinTheta_2[i] = sin_scan_mirror_altitude_ch128[i % 4]; + } else { + sinTheta_2[i] = std::sin(prism_angle128[i % 4] * M_PI / 180); + } + } + for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) { + firings[point_idx1].vertical_line = raw->points[point_idx1].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; + firings[point_idx1].azimuth = + static_cast(point_amuzith.uint * 0.01 * DEG_TO_RAD); + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx1].distance_3; + point_distance.bytes[1] = raw->points[point_idx1].distance_2; + point_distance.bytes[2] = raw->points[point_idx1].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx1].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx1].intensity = raw->points[point_idx1].intensity; + } + + for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection &corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + if (firings[point_idx].vertical_line / 4 % 2 == 0) { + cos_azimuth_half = + sin((firings[point_idx].azimuth - 15 * DEG_TO_RAD) * 0.5); + } else { + cos_azimuth_half = + cos((firings[point_idx].azimuth + 15 * DEG_TO_RAD) * 0.5); + } + + z_sin_altitude = + sin_scan_laser_altitude_ch128[firings[point_idx].vertical_line / 4] + + 2 * cos_azimuth_half * sinTheta_2[firings[point_idx].vertical_line]; + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1562.5 * ((POINTS_PER_PACKET - point_idx) / 7 - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1562.5; + } + time_last = point_time; + + PointXYZIT *point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH128, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); } + } + } +} - void LslidarCH128Parser::GeneratePointcloud( - const std::shared_ptr &scan_msg, - std::shared_ptr &out_msg) - { - // 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(); - - two_bytes angle_1, angle_2, angle_3, angle_4; - angle_1.bytes[0] = difop_ptr[87]; - angle_1.bytes[1] = difop_ptr[86]; - prism_angle128[0] = angle_1.uint * 0.01; - - angle_2.bytes[0] = difop_ptr[89]; - angle_2.bytes[1] = difop_ptr[88]; - prism_angle128[1] = angle_2.uint * 0.01; - - angle_3.bytes[0] = difop_ptr[91]; - angle_3.bytes[1] = difop_ptr[90]; - prism_angle128[2] = angle_3.uint * 0.01; - - angle_4.bytes[0] = difop_ptr[93]; - angle_4.bytes[1] = difop_ptr[92]; - prism_angle128[3] = angle_4.uint * 0.01; - - packets_size = scan_msg->firing_pkts_size(); - - for (size_t i = 0; i < packets_size; ++i) - { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); - 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()); - } - - /** @brief convert raw packet to point cloud - * @param pkt raw packet to Unpack - * @param pc shared pointer to point cloud (points are appended) - */ - void LslidarCH128Parser::Unpack(int num, const LslidarPacket &pkt, - std::shared_ptr pc) - { - float x, y, z; - uint64_t packet_end_time; - double z_sin_altitude = 0.0; - double z_cos_altitude = 0.0; - double cos_azimuth_half = 0.0; - const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); - time_last = 0; - - packet_end_time = pkt.stamp(); - - for (int i = 0; i < 128; i++) - { - if (abs(prism_angle128[0]) < 1e-6 && abs(prism_angle128[1]) < 1e-6 && abs(prism_angle128[2]) < 1e-6 && - abs(prism_angle128[3]) < 1e-6) - { - sinTheta_2[i] = sin_scan_mirror_altitude_ch128[i % 4]; - } - else - { - sinTheta_2[i] = std::sin(prism_angle128[i % 4] * M_PI / 180); - } - } - for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) - { - firings[point_idx1].vertical_line = raw->points[point_idx1].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; - firings[point_idx1].azimuth = static_cast(point_amuzith.uint * 0.01 * DEG_TO_RAD); - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx1].distance_3; - point_distance.bytes[1] = raw->points[point_idx1].distance_2; - point_distance.bytes[2] = raw->points[point_idx1].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx1].distance = static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx1].intensity = raw->points[point_idx1].intensity; - } - - for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection &corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - if (firings[point_idx].vertical_line / 4 % 2 == 0) - { - cos_azimuth_half = sin((firings[point_idx].azimuth - 15 * DEG_TO_RAD) * 0.5); - } - else - { - cos_azimuth_half = cos((firings[point_idx].azimuth + 15 * DEG_TO_RAD) * 0.5); - } - - z_sin_altitude = sin_scan_laser_altitude_ch128[firings[point_idx].vertical_line / 4] + - 2 * cos_azimuth_half * sinTheta_2[firings[point_idx].vertical_line]; - z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); - x = firings[point_idx].distance * z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance * z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance * z_sin_altitude; - - // Compute the time of the point - uint64_t point_time = packet_end_time - 1562.5 * ((POINTS_PER_PACKET - point_idx) / 7 - 1); - if (time_last < point_time && time_last > 0) - { - point_time = time_last + 1562.5; - } - time_last = point_time; - - PointXYZIT *point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) - { - - ComputeCoords2(firings[point_idx].vertical_line, CH128, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - } - else - { - if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) - { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - } - else - { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } - } - - void LslidarCH128Parser::Order(std::shared_ptr cloud) - { - } +void LslidarCH128Parser::Order(std::shared_ptr cloud) {} - } // namespace lslidar - } // namespace drivers -} // namespace apollo +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc index bd228d55f3f..dc1df379ab1 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc @@ -19,46 +19,48 @@ namespace apollo { namespace drivers { namespace lslidar { - + LslidarCH16Parser::LslidarCH16Parser(const Config& config) - : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { - -} + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} void LslidarCH16Parser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); - + 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(); + two_bytes angle_1, angle_2, angle_3, angle_4; angle_1.bytes[0] = difop_ptr[663]; angle_1.bytes[1] = difop_ptr[662]; prism_angle[0] = angle_1.uint * 0.01; - + angle_2.bytes[0] = difop_ptr[665]; angle_2.bytes[1] = difop_ptr[664]; prism_angle[1] = angle_2.uint * 0.01; - + angle_3.bytes[0] = difop_ptr[667]; angle_3.bytes[1] = difop_ptr[666]; prism_angle[2] = angle_3.uint * 0.01; - + angle_4.bytes[0] = difop_ptr[669]; angle_4.bytes[1] = difop_ptr[668]; prism_angle[3] = angle_4.uint * 0.01; packets_size = scan_msg->firing_pkts_size(); for (size_t i = 0; i < packets_size; ++i) { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -72,95 +74,106 @@ void LslidarCH16Parser::GeneratePointcloud( out_msg->set_width(out_msg->point_size()); } - /** @brief convert raw packet to point cloud * @param pkt raw packet to Unpack * @param pc shared pointer to point cloud (points are appended) */ void LslidarCH16Parser::Unpack(int num, const LslidarPacket& pkt, - std::shared_ptr pc) { + std::shared_ptr pc) { float x, y, z; - uint64_t packet_end_time; + uint64_t packet_end_time; double z_sin_altitude = 0.0; double z_cos_altitude = 0.0; time_last = 0; const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); - + packet_end_time = pkt.stamp(); - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - firings[point_idx].vertical_line=raw->points[point_idx].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; - firings[point_idx].azimuth=static_cast(point_amuzith.uint)*0.01*DEG_TO_RAD; - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx].distance_3; - point_distance.bytes[1] = raw->points[point_idx].distance_2; - point_distance.bytes[2] = raw->points[point_idx].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx].distance=static_cast(point_distance.uint) / 256.0 * DISTANCE_RESOLUTION; - firings[point_idx].intensity=raw->points[point_idx].intensity; - } - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection& corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - // Convert the point to xyz coordinate - double cos_azimuth_half=cos(firings[point_idx].azimuth*0.5); - - if(abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && - abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6 ){ - z_sin_altitude = sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + - 2 * cos_azimuth_half * sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; - }else{ - z_sin_altitude = sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + - 2 * cos_azimuth_half * sin(prism_angle[firings[point_idx].vertical_line % 4] * M_PI / 180); - } - - z_cos_altitude=sqrt(1-z_sin_altitude*z_sin_altitude); - - x = firings[point_idx].distance *z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance *z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance *z_sin_altitude; - - // Compute the time of the point - uint64_t point_time = packet_end_time - 1665*(POINTS_PER_PACKET - point_idx - 1); - if(time_last < point_time && time_last > 0){ - point_time = time_last + 1665; - } - time_last = point_time; - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time/1000000000.0); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) { - ComputeCoords2(firings[point_idx].vertical_line, CH16, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - }else{ - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) / 256.0 * DISTANCE_RESOLUTION; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + // Convert the point to xyz coordinate + double cos_azimuth_half = cos(firings[point_idx].azimuth * 0.5); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + z_sin_altitude = + sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + + 2 * cos_azimuth_half * + sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; + } else { + z_sin_altitude = + sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + + 2 * cos_azimuth_half * + sin(prism_angle[firings[point_idx].vertical_line % 4] * M_PI / + 180); + } + + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1665 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1665; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time / 1000000000.0); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH16, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); } - } - } + } + } } void LslidarCH16Parser::Order(std::shared_ptr cloud) { diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc index 3ef2915744c..d5e6cc4f044 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc @@ -16,175 +16,167 @@ #include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" -namespace apollo -{ - namespace drivers - { - namespace lslidar - { - - LslidarCH32Parser::LslidarCH32Parser(const Config &config) - : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) - { +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH32Parser::LslidarCH32Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} + +void LslidarCH32Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // 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(); + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + 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()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH32Parser::Unpack(int num, const LslidarPacket &pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + time_last = 0; + const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + double scan_laser_altitude_degree[8]; + if (1 == config_.degree_mode()) { + for (int i = 0; i < 8; i++) + scan_laser_altitude_degree[i] = sin_scan_laser_altitude[i]; + } else { + for (int i = 0; i < 8; i++) + scan_laser_altitude_degree[i] = sin_scan_laser_altitude_ch32[i]; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection &corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + // Convert the point to xyz coordinate + double cos_azimuth_half = cos(firings[point_idx].azimuth * 0.5); + z_sin_altitude = + scan_laser_altitude_degree[firings[point_idx].vertical_line / 4] + + 2 * cos_azimuth_half * + sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1665 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1665; + } + time_last = point_time; + + PointXYZIT *point = pc->add_point(); + (point_time / 1000000000.0); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH32, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); } - - void LslidarCH32Parser::GeneratePointcloud( - const std::shared_ptr &scan_msg, - std::shared_ptr &out_msg) - { - // 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(); - - packets_size = scan_msg->firing_pkts_size(); - - for (size_t i = 0; i < packets_size; ++i) - { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); - 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()); - } - - /** @brief convert raw packet to point cloud - * @param pkt raw packet to Unpack - * @param pc shared pointer to point cloud (points are appended) - */ - void LslidarCH32Parser::Unpack(int num, const LslidarPacket &pkt, - std::shared_ptr pc) - { - float x, y, z; - uint64_t packet_end_time; - double z_sin_altitude = 0.0; - double z_cos_altitude = 0.0; - time_last = 0; - const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); - - packet_end_time = pkt.stamp(); - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; - firings[point_idx].azimuth = static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx].distance_3; - point_distance.bytes[1] = raw->points[point_idx].distance_2; - point_distance.bytes[2] = raw->points[point_idx].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx].distance = static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx].intensity = raw->points[point_idx].intensity; - } - - double scan_laser_altitude_degree[8]; - if (1 == config_.degree_mode()) - { - for (int i = 0; i < 8; i++) - scan_laser_altitude_degree[i] = sin_scan_laser_altitude[i]; - } - else - { - for (int i = 0; i < 8; i++) - scan_laser_altitude_degree[i] = sin_scan_laser_altitude_ch32[i]; - } - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection &corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - // Convert the point to xyz coordinate - double cos_azimuth_half = cos(firings[point_idx].azimuth * 0.5); - z_sin_altitude = scan_laser_altitude_degree[firings[point_idx].vertical_line / 4] + 2 * cos_azimuth_half * sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; - z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); - - x = firings[point_idx].distance * z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance * z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance * z_sin_altitude; - - // Compute the time of the point - uint64_t point_time = packet_end_time - 1665 * (POINTS_PER_PACKET - point_idx - 1); - if (time_last < point_time && time_last > 0) - { - point_time = time_last + 1665; - } - time_last = point_time; - - PointXYZIT *point = pc->add_point(); - (point_time / 1000000000.0); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) - { - ComputeCoords2(firings[point_idx].vertical_line, CH32, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - } - else - { - if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) - { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - } - else - { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } - } - - void LslidarCH32Parser::Order(std::shared_ptr cloud) - { - int width = 32; - cloud->set_width(width); - int height = cloud->point_size() / cloud->width(); - cloud->set_height(height); - - std::shared_ptr cloud_origin = std::make_shared(); - cloud_origin->CopyFrom(*cloud); - - for (int i = 0; i < width; ++i) - { - int col = lslidar::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)); - } - } - } - - } // namespace lslidar - } // namespace drivers -} // namespace apollo + } + } +} + +void LslidarCH32Parser::Order(std::shared_ptr cloud) { + int width = 32; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::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)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc index 367a13c414f..a040cdd0ef5 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc @@ -19,27 +19,29 @@ namespace apollo { namespace drivers { namespace lslidar { - + LslidarCH64Parser::LslidarCH64Parser(const Config& config) - : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { -} + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} void LslidarCH64Parser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); - - packets_size = scan_msg->firing_pkts_size(); + 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(); + + packets_size = scan_msg->firing_pkts_size(); for (size_t i = 0; i < packets_size; ++i) { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -53,106 +55,111 @@ void LslidarCH64Parser::GeneratePointcloud( out_msg->set_width(out_msg->point_size()); } - /** @brief convert raw packet to point cloud * @param pkt raw packet to Unpack * @param pc shared pointer to point cloud (points are appended) */ void LslidarCH64Parser::Unpack(int num, const LslidarPacket& pkt, - std::shared_ptr pc) { + std::shared_ptr pc) { float x, y, z; uint64_t point_time; - uint64_t packet_end_time; + uint64_t packet_end_time; double z_sin_altitude = 0.0; double z_cos_altitude = 0.0; time_last = 0; const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); - - packet_end_time = pkt.stamp(); - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - firings[point_idx].vertical_line=raw->points[point_idx].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; - firings[point_idx].azimuth=static_cast(point_amuzith.uint)*0.01*DEG_TO_RAD; - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx].distance_3; - point_distance.bytes[1] = raw->points[point_idx].distance_2; - point_distance.bytes[2] = raw->points[point_idx].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx].distance=static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx].intensity=raw->points[point_idx].intensity; - } - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection& corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - int line_num = firings[point_idx].vertical_line; - - // Convert the point to xyz coordinate - if (line_num % 8 == 0 || line_num % 8 == 1 || line_num % 8 == 2 || line_num % 8 == 3) { - - z_sin_altitude = sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33* DEG_TO_RAD) + - 2 * cos(firings[point_idx].azimuth / 2 + 1.05* DEG_TO_RAD ) * - sin((line_num % 4) * 0.33* DEG_TO_RAD); - - } else if (line_num % 8 == 4 || line_num % 8 == 5 || line_num % 8 == 6 || line_num % 8 == 7) { - z_sin_altitude = sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33* DEG_TO_RAD) + - 2 * cos(firings[point_idx].azimuth / 2 - 1.05* DEG_TO_RAD ) * - sin((line_num % 4) * 0.33* DEG_TO_RAD); - } - - z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); - x = firings[point_idx].distance * z_cos_altitude * cos(firings[point_idx].azimuth); - y = firings[point_idx].distance * z_cos_altitude * sin(firings[point_idx].azimuth); - z = firings[point_idx].distance * z_sin_altitude; - - // Compute the time of the point - point_time = packet_end_time - 1726*(POINTS_PER_PACKET - point_idx - 1); - if(time_last < point_time && time_last > 0){ - point_time = time_last + 1726; - } - time_last = point_time; - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) { - - ComputeCoords2(firings[point_idx].vertical_line, CH64, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - - } else { - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } -} + packet_end_time = pkt.stamp(); + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } -void LslidarCH64Parser::Order(std::shared_ptr cloud) { + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + int line_num = firings[point_idx].vertical_line; + + // Convert the point to xyz coordinate + if (line_num % 8 == 0 || line_num % 8 == 1 || line_num % 8 == 2 || + line_num % 8 == 3) { + z_sin_altitude = + sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(firings[point_idx].azimuth / 2 + 1.05 * DEG_TO_RAD) * + sin((line_num % 4) * 0.33 * DEG_TO_RAD); + + } else if (line_num % 8 == 4 || line_num % 8 == 5 || line_num % 8 == 6 || + line_num % 8 == 7) { + z_sin_altitude = + sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(firings[point_idx].azimuth / 2 - 1.05 * DEG_TO_RAD) * + sin((line_num % 4) * 0.33 * DEG_TO_RAD); + } + + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + point_time = packet_end_time - 1726 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1726; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH64, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } } +void LslidarCH64Parser::Order(std::shared_ptr cloud) {} + } // namespace lslidar } // namespace drivers } // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc index a88706d5d7e..10b5b9d584f 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc @@ -19,53 +19,53 @@ namespace apollo { namespace drivers { namespace lslidar { - + LslidarCH64wParser::LslidarCH64wParser(const Config& config) : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { - point_time_diff = 1 / (60.0 * 1000 * 32); // s - for (int i = 0; i < 4; ++i) { - prism_angle64[i] = i * 0.35; - } - for (int j = 0; j < 128; ++j) { - //右边 - if (j / 4 % 2 == 0) { - theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); - theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); - } - //左边 - else { - theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); - theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); - } - } + point_time_diff = 1 / (60.0 * 1000 * 32); // s + for (int i = 0; i < 4; ++i) { + prism_angle64[i] = i * 0.35; + } + for (int j = 0; j < 128; ++j) { + if (j / 4 % 2 == 0) { + theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } else { + theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } + } } void LslidarCH64wParser::GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr& out_msg) { // 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(); - - packets_size = scan_msg->firing_pkts_size(); + 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(); + + packets_size = scan_msg->firing_pkts_size(); AINFO << "packets_size :" << packets_size; for (size_t i = 0; i < packets_size; ++i) { - Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), out_msg); + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); 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(); + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); } // set default width @@ -77,116 +77,127 @@ void LslidarCH64wParser::GeneratePointcloud( * @param pc shared pointer to point cloud (points are appended) */ void LslidarCH64wParser::Unpack(int num, const LslidarPacket& pkt, - std::shared_ptr pc) { - float x, y, z; - uint64_t point_time; - uint64_t packet_end_time; - time_last = 0; - const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); - double cos_xita; - // 垂直角度 - double sin_theat; - double cos_theat; - double _R_; - // 水平角度 - double cos_H_xita; - double sin_H_xita; - double cos_xita_F; - double sin_xita_F; - + std::shared_ptr pc) { + float x, y, z; + uint64_t point_time; + uint64_t packet_end_time; + time_last = 0; + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + double cos_xita; + // 垂直角度 + double sin_theat; + double cos_theat; + double _R_; + // 水平角度 + double cos_H_xita; + double sin_H_xita; + double cos_xita_F; + double sin_xita_F; + packet_end_time = pkt.stamp(); current_packet_time = packet_end_time; - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - firings[point_idx].vertical_line=raw->points[point_idx].vertical_line; - two_bytes point_amuzith; - point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; - point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; - firings[point_idx].azimuth=static_cast(point_amuzith.uint)*0.01*DEG_TO_RAD; - four_bytes point_distance; - point_distance.bytes[0] = raw->points[point_idx].distance_3; - point_distance.bytes[1] = raw->points[point_idx].distance_2; - point_distance.bytes[2] = raw->points[point_idx].distance_1; - point_distance.bytes[3] = 0; - firings[point_idx].distance=static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; - firings[point_idx].intensity=raw->points[point_idx].intensity; - } - - for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) - { - LaserCorrection& corrections = calibration_.laser_corrections_[firings[point_idx].vertical_line]; - - if (config_.calibration()) - firings[point_idx].distance = firings[point_idx].distance + corrections.dist_correction; - - if (firings[point_idx].distance > config_.max_range() || firings[point_idx].distance < config_.min_range()) - continue; - - int line_num = firings[point_idx].vertical_line; - if (line_num / 4 % 2 == 0) { - cos_xita = cos((firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 22.5) * M_PI / 180); - }else{ - cos_xita = cos((-firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 112.5) * M_PI / 180); - } - - _R_ = theat2_c[line_num] * theat1_c[line_num] * cos_xita - theat2_s[line_num] * theat1_s[line_num]; - sin_theat = theat1_s[line_num] + 2 * _R_ * theat2_s[line_num]; - cos_theat = sqrt(1 - pow(sin_theat, 2)); - cos_H_xita = (2 * _R_ * theat2_c[line_num] * cos_xita - theat1_c[line_num]) / cos_theat; - sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); - - if(line_num / 4 % 2 == 0){ - cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); - sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); - } else{ - cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); - sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); - } - - x = firings[point_idx].distance * cos_theat * cos_xita_F; - y = firings[point_idx].distance * cos_theat * sin_xita_F; - z = firings[point_idx].distance * sin_theat; - - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) return; - // modify - if(previous_packet_time >1e-6) { - point_time = current_packet_time - (POINTS_PER_PACKET - point_idx - 1) * (current_packet_time - previous_packet_time)/(POINTS_PER_PACKET); - } else{ //fist packet - point_time = current_packet_time; - } - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - point->set_intensity(firings[point_idx].intensity); - - if (config_.calibration()) { - - ComputeCoords2(firings[point_idx].vertical_line, CH64w, firings[point_idx].distance, corrections, - firings[point_idx].azimuth, point); - - } else { - if((y >= config_.bottom_left_x() && y <= config_.top_right_x()) - && (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y); - point->set_y(-x); - point->set_z(z); - } - } - } + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + int line_num = firings[point_idx].vertical_line; + if (line_num / 4 % 2 == 0) { + cos_xita = cos((firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 22.5) * + M_PI / 180); + } else { + cos_xita = cos((-firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 112.5) * + M_PI / 180); + } + + _R_ = theat2_c[line_num] * theat1_c[line_num] * cos_xita - + theat2_s[line_num] * theat1_s[line_num]; + sin_theat = theat1_s[line_num] + 2 * _R_ * theat2_s[line_num]; + cos_theat = sqrt(1 - pow(sin_theat, 2)); + cos_H_xita = + (2 * _R_ * theat2_c[line_num] * cos_xita - theat1_c[line_num]) / + cos_theat; + sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); + + if (line_num / 4 % 2 == 0) { + cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); + sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); + } else { + cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); + sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); + } + + x = firings[point_idx].distance * cos_theat * cos_xita_F; + y = firings[point_idx].distance * cos_theat * sin_xita_F; + z = firings[point_idx].distance * sin_theat; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) + return; + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (POINTS_PER_PACKET - point_idx - 1) * + (current_packet_time - previous_packet_time) / + (POINTS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH64w, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } previous_packet_time = current_packet_time; } -void LslidarCH64wParser::Order(std::shared_ptr cloud) { -} +void LslidarCH64wParser::Order(std::shared_ptr cloud) {} } // namespace lslidar } // namespace drivers diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc index b3ee4fb1639..25da56b8cb3 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc @@ -16,151 +16,166 @@ #include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" - namespace apollo { namespace drivers { namespace lslidar { -LslidarCXV4Parser::LslidarCXV4Parser(const Config& config) +LslidarCXV4Parser::LslidarCXV4Parser(const Config &config) : 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_ " <(j) / 100.0 * DEG_TO_RAD; - sin_azimuth_table[j] = sin(angle); - cos_azimuth_table[j] = cos(angle); + } 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(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; } void LslidarCXV4Parser::decodePacket(const RawPacket_C32 *packet) { - //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; + // 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; + } } - 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(raw_distance.distance) * DISTANCE_RESOLUTION * distance_unit_; - - //intensity - firings.intensity[block_idx * 32 + scan_fir_idx] = static_cast( - raw_block.data[byte_idx + 2]); - } + // 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(raw_distance.distance) * DISTANCE_RESOLUTION * + distance_unit_; + + // intensity + firings.intensity[block_idx * 32 + scan_fir_idx] = + static_cast(raw_block.data[byte_idx + 2]); } - return; -}; + } + return; +} bool LslidarCXV4Parser::checkPacketValidity(const RawPacket_C32 *packet) { - 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; + 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; +} void LslidarCXV4Parser::GeneratePointcloud( - const std::shared_ptr& scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { // 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 暂时不用设备包 + 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(); @@ -170,7 +185,7 @@ void LslidarCXV4Parser::GeneratePointcloud( AINFO << "packets_size :" << packets_size; for (size_t i = 0; i < packets_size; ++i) { - Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg,i); + Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); last_time_stamp_ = out_msg->measurement_time(); ADEBUG << "stamp: " << std::fixed << last_time_stamp_; } @@ -184,127 +199,142 @@ void LslidarCXV4Parser::GeneratePointcloud( out_msg->set_width(out_msg->point_size()); } - /** @brief convert raw packet to point cloud * @param pkt raw packet to Unpack * @param pc shared pointer to point cloud (points are appended) */ -void LslidarCXV4Parser::Unpack(const LslidarPacket& pkt, - std::shared_ptr pc ,int packet_number) { - double point_time; - const RawPacket_C32 *raw_packet = (const RawPacket_C32 *) (pkt.data().c_str()); - uint8_t *data = - reinterpret_cast(const_cast(pkt.data().c_str())); - - //check if the packet is valid - if (!checkPacketValidity(raw_packet)) return ; - - //decode the packet - decodePacket(raw_packet); - - if (!is_get_scan_altitude_ && static_cast(data[1211]) == 0x46) { //old C32w - AERROR << "byte[1211] == 0x46: old C32W"; - is_new_c32w_ = false; - for (int k = 0; k < 32; ++k) { - sin_scan_altitude[k] = sin(c32_70_vertical_angle[k] * DEG_TO_RAD); - cos_scan_altitude[k] = cos(c32_70_vertical_angle[k] * DEG_TO_RAD); - } - is_get_scan_altitude_ = true; +void LslidarCXV4Parser::Unpack(const LslidarPacket &pkt, + std::shared_ptr pc, + int packet_number) { + double point_time; + const RawPacket_C32 *raw_packet = (const RawPacket_C32 *)(pkt.data().c_str()); + uint8_t *data = + reinterpret_cast(const_cast(pkt.data().c_str())); + + // check if the packet is valid + if (!checkPacketValidity(raw_packet)) return; + + // decode the packet + decodePacket(raw_packet); + + if (!is_get_scan_altitude_ && + static_cast(data[1211]) == 0x46) { // old C32w + AERROR << "byte[1211] == 0x46: old C32W"; + is_new_c32w_ = false; + for (int k = 0; k < 32; ++k) { + sin_scan_altitude[k] = sin(c32_70_vertical_angle[k] * DEG_TO_RAD); + cos_scan_altitude[k] = cos(c32_70_vertical_angle[k] * DEG_TO_RAD); } - + is_get_scan_altitude_ = true; + } for (size_t fir_idx = 0; fir_idx < SCANS_PER_PACKET; ++fir_idx) { - if(!is_new_c32w_ && vertical_angle_ == 70){ - if (fir_idx % 32 == 29 || fir_idx % 32 == 6 || fir_idx % 32 == 14 || fir_idx % 32 == 22 || - fir_idx % 32 == 30 || fir_idx % 32 == 7 || fir_idx % 32 == 15 || fir_idx % 32 == 23) - { - firings.azimuth[fir_idx] += 389; - } - if (firings.azimuth[fir_idx] > 36000) firings.azimuth[fir_idx]-=36000; - } - //convert the point to xyz coordinate - size_t table_idx = firings.azimuth[fir_idx]; - double cos_azimuth = cos_azimuth_table[table_idx]; - double sin_azimuth = sin_azimuth_table[table_idx]; - double x_coord, y_coord, z_coord; - bool coordinate_opt = true; - double R1 = 0.0; - if (vertical_angle_ == 90) { // ch32r - R1 = CX4_R1_90; - } else if (is_new_c32w_) { //new C32w - R1 = CX4_C32W; - } else { //others - R1 = CX4_R1_; - } - double conversionAngle = (vertical_angle_ == 90) ? CX4_conversionAngle_90 : CX4_conversionAngle_; - if (coordinate_opt) { - x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + - R1 * cos((conversionAngle - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD); - y_coord = -firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + - R1 * sin((conversionAngle - firings.azimuth[fir_idx] * 0.01) * DEG_TO_RAD); - z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_]; - - - } else { - //Y-axis correspondence 0 degree - x_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + - R1 * sin((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * DEG_TO_RAD); - y_coord = firings.distance[fir_idx] * cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + - R1 * cos((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * DEG_TO_RAD); - z_coord = firings.distance[fir_idx] * sin_scan_altitude[fir_idx % lidar_number_]; + if (!is_new_c32w_ && vertical_angle_ == 70) { + if (fir_idx % 32 == 29 || fir_idx % 32 == 6 || fir_idx % 32 == 14 || + fir_idx % 32 == 22 || fir_idx % 32 == 30 || fir_idx % 32 == 7 || + fir_idx % 32 == 15 || fir_idx % 32 == 23) { + firings.azimuth[fir_idx] += 389; + } + if (firings.azimuth[fir_idx] > 36000) firings.azimuth[fir_idx] -= 36000; + } + // convert the point to xyz coordinate + size_t table_idx = firings.azimuth[fir_idx]; + double cos_azimuth = cos_azimuth_table[table_idx]; + double sin_azimuth = sin_azimuth_table[table_idx]; + double x_coord, y_coord, z_coord; + bool coordinate_opt = true; + double R1 = 0.0; + if (vertical_angle_ == 90) { // ch32r + R1 = CX4_R1_90; + } else if (is_new_c32w_) { // new C32w + R1 = CX4_C32W; + } else { // others + R1 = CX4_R1_; + } + double conversionAngle = + (vertical_angle_ == 90) ? CX4_conversionAngle_90 : CX4_conversionAngle_; + if (coordinate_opt) { + x_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + + R1 * cos((conversionAngle - firings.azimuth[fir_idx] * 0.01) * + DEG_TO_RAD); + y_coord = -firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + + R1 * sin((conversionAngle - firings.azimuth[fir_idx] * 0.01) * + DEG_TO_RAD); + z_coord = firings.distance[fir_idx] * + sin_scan_altitude[fir_idx % lidar_number_]; + + } else { + // Y-axis correspondence 0 degree + x_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + + R1 * sin((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * + DEG_TO_RAD); + y_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + + R1 * cos((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * + DEG_TO_RAD); + z_coord = firings.distance[fir_idx] * + sin_scan_altitude[fir_idx % lidar_number_]; + } + + // computer the time of the point + if (last_packet_time > 1e-6) { + point_time = current_packet_time - + (current_packet_time - previous_packet_time) * + (SCANS_PER_PACKET - fir_idx - 1) / SCANS_PER_PACKET; + } else { + point_time = current_packet_time; + } + + if (firings.distance[fir_idx] > config_.max_range() || + firings.distance[fir_idx] < config_.min_range()) { + PointXYZIT *point = pc->add_point(); + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + if ((firings.azimuth[fir_idx] < config_.scan_start_angle()) || + (firings.azimuth[fir_idx] > config_.scan_end_angle())) + continue; + if (packet_number == 0) { + if (firings.azimuth[fir_idx] > 30000) { + continue; } + } - // computer the time of the point - if (last_packet_time > 1e-6) { - point_time = - current_packet_time - (current_packet_time - previous_packet_time) * (SCANS_PER_PACKET - fir_idx - 1) / SCANS_PER_PACKET; - } else { - point_time = current_packet_time; + if (packet_number == packet_number_ - 1) { + if (firings.azimuth[fir_idx] < 10000) { + continue; } + } - if (firings.distance[fir_idx] > config_.max_range() || firings.distance[fir_idx] < config_.min_range()) { - PointXYZIT* point = pc->add_point(); + PointXYZIT *point = pc->add_point(); + point->set_timestamp(point_time); + + point->set_intensity(firings.intensity[fir_idx]); + if ((y_coord >= config_.bottom_left_x() && + y_coord <= config_.top_right_x()) && + (-x_coord >= config_.bottom_left_y() && + -x_coord <= config_.top_right_y())) { point->set_x(nan); point->set_y(nan); point->set_z(nan); point->set_timestamp(point_time); point->set_intensity(0); - }else { - if ((firings.azimuth[fir_idx] < config_.scan_start_angle()) || (firings.azimuth[fir_idx] > config_.scan_end_angle())) continue; - if (packet_number == 0) { - if (firings.azimuth[fir_idx] > 30000) { - continue; - } - } - - if (packet_number == packet_number_ - 1) { - if (firings.azimuth[fir_idx] < 10000) { - continue; - } - } - - PointXYZIT* point = pc->add_point(); - point->set_timestamp(point_time); - - point->set_intensity(firings.intensity[fir_idx]); - if((y_coord >= config_.bottom_left_x() && y_coord <= config_.top_right_x()) - && (-x_coord >= config_.bottom_left_y() && -x_coord <= config_.top_right_y())) { - point->set_x(nan); - point->set_y(nan); - point->set_z(nan); - point->set_timestamp(point_time); - point->set_intensity(0); - }else { - point->set_x(y_coord); - point->set_y(-x_coord); - point->set_z(z_coord); - } - } + } else { + point->set_x(y_coord); + point->set_y(-x_coord); + point->set_z(z_coord); + } } - previous_packet_time = current_packet_time; + } + previous_packet_time = current_packet_time; } - void LslidarCXV4Parser::Order(std::shared_ptr cloud) { int width = 32; cloud->set_width(width); diff --git a/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc index 25c58f4bb89..f0ced67767c 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc @@ -45,7 +45,7 @@ LslidarLS128S2Parser::LslidarLS128S2Parser(const Config &config) void LslidarLS128S2Parser::GeneratePointcloud( const std::shared_ptr &scan_msg, - std::shared_ptr &out_msg) { + const std::shared_ptr &out_msg) { // 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); @@ -200,8 +200,7 @@ void LslidarLS128S2Parser::Unpack(int num, const LslidarPacket &pkt, int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号 int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位 double fAngle_V = 0.0; - if (1 == iSymmbol) // 符号位 0：正数 1：负数 - { + if (1 == iSymmbol) { // 符号位 0：正数 1：负数 int iAngle_V = msop_ptr[point_idx + 3] + (msop_ptr[point_idx + 2] << 8); @@ -259,8 +258,7 @@ void LslidarLS128S2Parser::Unpack(int num, const LslidarPacket &pkt, int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号 int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位 double fAngle_V = 0.0; - if (1 == iSymmbol) // 符号位 0：正数 1：负数 - { + if (1 == iSymmbol) { // 符号位 0：正数 1：负数 int iAngle_V = msop_ptr[point_idx + 3] + (msop_ptr[point_idx + 2] << 8); @@ -326,8 +324,8 @@ int LslidarLS128S2Parser::convertCoordinate( fAngle_H += 360.0; } - int table_index_V = int(fGalvanometrtAngle * 100) % 36000; - int table_index_H = int(fAngle_H * 100) % 36000; + int table_index_V = static_cast(fGalvanometrtAngle * 100) % 36000; + int table_index_H = static_cast(fAngle_H * 100) % 36000; double fAngle_R0 = cos30 * cos_mirror_angle[lidardata.channel_number % 4] * @@ -408,8 +406,8 @@ int LslidarLS128S2Parser::convertCoordinate( fAngle_H += 360.0; } - int table_index_V = int(fGalvanometrtAngle * 100) % 36000; - int table_index_H = int(fAngle_H * 100) % 36000; + int table_index_V = static_cast(fGalvanometrtAngle * 100) % 36000; + int table_index_H = static_cast(fAngle_H * 100) % 36000; double fAngle_R0 = cos30 * cos_mirror_angle[lidardata.channel_number % 4] * @@ -464,8 +462,7 @@ int LslidarLS128S2Parser::convertCoordinate( return 0; } -void LslidarLS128S2Parser::Order(std::shared_ptr cloud) { -} +void LslidarLS128S2Parser::Order(std::shared_ptr cloud) {} } // namespace lslidar } // namespace drivers diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc index cb09901d53b..fdcd57ab7de 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc @@ -14,20 +14,13 @@ * limitations under the License. *****************************************************************************/ -#include -#include -#include - -#include "cyber/cyber.h" #include "modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h" namespace apollo { namespace drivers { namespace lslidar { -static void my_handler(int sig) { - exit(-1); -} +static void my_handler(int sig) { exit(-1); } bool LslidarConvertComponent::Init() { signal(SIGINT, my_handler); @@ -39,11 +32,13 @@ bool LslidarConvertComponent::Init() { conv_.reset(new Convert()); conv_->init(lslidar_config); - - writer_ = node_->CreateWriter(lslidar_config.convert_channel_name()); - point_cloud_pool_.reset(new CCObjectPool(pool_size_)); + + writer_ = node_->CreateWriter( + lslidar_config.convert_channel_name()); + point_cloud_pool_.reset( + new CCObjectPool(pool_size_)); point_cloud_pool_->ConstructAll(); - + for (int i = 0; i < pool_size_; i++) { auto point_cloud = point_cloud_pool_->GetObject(); if (point_cloud == nullptr) { @@ -59,8 +54,8 @@ bool LslidarConvertComponent::Init() { bool LslidarConvertComponent::Proc( const std::shared_ptr& scan_msg) { - - std::shared_ptr point_cloud_out = point_cloud_pool_->GetObject(); + std::shared_ptr point_cloud_out = + point_cloud_pool_->GetObject(); if (point_cloud_out == nullptr) { AWARN << "poin cloud pool return nullptr, will be create new."; point_cloud_out = std::make_shared(); @@ -76,8 +71,7 @@ bool LslidarConvertComponent::Proc( point_cloud_out->Clear(); conv_->ConvertPacketsToPointcloud(scan_msg, point_cloud_out); uint64_t time2 = apollo::cyber::Time().Now().ToNanosecond(); - AINFO << "process pointcloud time: " <<(time2-time1)/1000000000.0; - + AINFO << "process pointcloud time: " << (time2 - time1) / 1000000000.0; if (point_cloud_out == nullptr || point_cloud_out->point().empty()) { AWARN << "point_cloud_out convert is empty."; diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h index f5d01ba1a37..10d90939948 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h +++ b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h @@ -19,14 +19,13 @@ #include #include #include -#include + +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" #include "cyber/base/concurrent_object_pool.h" #include "cyber/cyber.h" - #include "modules/drivers/lidar/lslidar/parser/convert.h" -#include "modules/drivers/lidar/lslidar/proto/config.pb.h" -#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" namespace apollo { namespace drivers { @@ -39,15 +38,18 @@ using apollo::cyber::base::CCObjectPool; using apollo::drivers::PointCloud; using apollo::drivers::lslidar::LslidarScan; -class LslidarConvertComponent : public Component { +class LslidarConvertComponent + : public Component { public: bool Init() override; - bool Proc(const std::shared_ptr& scan_msg) override; + bool Proc(const std::shared_ptr& + scan_msg) override; private: std::shared_ptr> writer_; std::unique_ptr conv_ = nullptr; - std::shared_ptr> point_cloud_pool_ = nullptr; + std::shared_ptr> point_cloud_pool_ = + nullptr; int pool_size_ = 8; }; diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc index 8296cf70135..5d45f2119b9 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc +++ b/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc @@ -22,33 +22,31 @@ namespace lslidar { LslidarParser::LslidarParser(const Config &config) : last_time_stamp_(0), config_(config) { - for (int i = 0; i < 4; ++i) { - prism_angle64[i] = i * 0.35; - } - for (int j = 0; j < 128; ++j) { - //右边 - if (j / 4 % 2 == 0) { - theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); - theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); - } - //左边 - else { - theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); - theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); - theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); - } - } + for (int i = 0; i < 4; ++i) { + prism_angle64[i] = i * 0.35; + } + for (int j = 0; j < 128; ++j) { + // 右边 + if (j / 4 % 2 == 0) { + theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } else { // 左边 + theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } + } } bool LslidarParser::is_scan_valid(int rotation, float range) { // check range first - if (range < config_.min_range() || range > config_.max_range()) + if (range < config_.min_range() || range > config_.max_range()) return false; else - return true; + return true; } /** Set up for on-line operation. */ @@ -62,240 +60,242 @@ void LslidarParser::setup() { } } for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { - float rotation_f = ROTATION_RESOLUTION * static_cast(rot_index * M_PI) / 180.0f; - cos_azimuth_table[rot_index] = cosf(rotation_f); - sin_azimuth_table[rot_index] = sinf(rotation_f); - } + float rotation_f = + ROTATION_RESOLUTION * static_cast(rot_index * M_PI) / 180.0f; + cos_azimuth_table[rot_index] = cosf(rotation_f); + sin_azimuth_table[rot_index] = sinf(rotation_f); + } } void LslidarParser::ComputeCoords(const float &raw_distance, - LaserCorrection &corrections, - const uint16_t rotation, PointXYZIT *point) { + LaserCorrection *corrections, + const uint16_t rotation, PointXYZIT *point) { double x = 0.0; double y = 0.0; double z = 0.0; double distance_corr_x = 0; double distance_corr_y = 0; - corrections.cos_rot_correction = cosf(corrections.rot_correction); - corrections.sin_rot_correction = sinf(corrections.rot_correction); - corrections.cos_vert_correction = cosf(corrections.vert_correction); - corrections.sin_vert_correction = sinf(corrections.vert_correction); - - double distance = raw_distance + corrections.dist_correction; + corrections->cos_rot_correction = cosf(corrections->rot_correction); + corrections->sin_rot_correction = sinf(corrections->rot_correction); + corrections->cos_vert_correction = cosf(corrections->vert_correction); + corrections->sin_vert_correction = sinf(corrections->vert_correction); + + double distance = raw_distance + corrections->dist_correction; - double cos_rot_angle = cos_azimuth_table[rotation] * corrections.cos_rot_correction + - sin_azimuth_table[rotation] * corrections.sin_rot_correction; - double sin_rot_angle = sin_azimuth_table[rotation] * corrections.cos_rot_correction - - cos_azimuth_table[rotation] * corrections.sin_rot_correction; + double cos_rot_angle = + cos_azimuth_table[rotation] * corrections->cos_rot_correction + + sin_azimuth_table[rotation] * corrections->sin_rot_correction; + double sin_rot_angle = + sin_azimuth_table[rotation] * corrections->cos_rot_correction - + cos_azimuth_table[rotation] * corrections->sin_rot_correction; // Compute the distance in the xy plane (w/o accounting for rotation) - double xy_distance = distance * corrections.cos_vert_correction; + double xy_distance = distance * corrections->cos_vert_correction; // Get 2points calibration values,Linear interpolation to get distance // correction for X and Y, that means distance correction use // different value at different distance - distance_corr_x = distance_corr_y = corrections.dist_correction; - + distance_corr_x = distance_corr_y = corrections->dist_correction; + double distance_x = raw_distance + distance_corr_x; - xy_distance = distance_x * corrections.cos_vert_correction; - - x = xy_distance * sin_rot_angle - corrections.horiz_offset_correction * cos_rot_angle; + xy_distance = distance_x * corrections->cos_vert_correction; + + x = xy_distance * sin_rot_angle - + corrections->horiz_offset_correction * cos_rot_angle; double distance_y = raw_distance + distance_corr_y; - xy_distance = distance_y * corrections.cos_vert_correction; + xy_distance = distance_y * corrections->cos_vert_correction; + + y = xy_distance * cos_rot_angle + + corrections->horiz_offset_correction * sin_rot_angle; + z = distance * corrections->sin_vert_correction + + corrections->vert_offset_correction; - y = xy_distance * cos_rot_angle + corrections.horiz_offset_correction * sin_rot_angle; - z = distance * corrections.sin_vert_correction + corrections.vert_offset_correction; - /** Use standard ROS coordinate system (right-hand rule) */ - point->set_x(static_cast(-y)); - point->set_y(static_cast(-x)); - point->set_z(static_cast(z)); + point->set_x(static_cast(-y)); + point->set_y(static_cast(-x)); + point->set_z(static_cast(z)); } -void LslidarParser::ComputeCoords2(int Laser_ring, int Type, const float &raw_distance, - LaserCorrection &corrections, +void LslidarParser::ComputeCoords2(int Laser_ring, int Type, + const float &raw_distance, + LaserCorrection* corrections, const double rotation, PointXYZIT *point) { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double distance_corr_x = 0; - double distance_corr_y = 0; - double sinTheta_1[128] = {0}; - double cosTheta_1[128] = {0}; - double sinTheta_2[128] = {0}; - double cosTheta_2[128] = {0}; - double cos_xita; - // 垂直角度 - double sin_theat; - double cos_theat; - double _R_; - // 水平角度 - double cos_H_xita; - double sin_H_xita; - double cos_xita_F; - - for (int i = 0; i < 128; i++) { - sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); - cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); - - if(abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) - { - sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); - cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); - }else{ - sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); - cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); - } - } - - corrections.cos_rot_correction = cosf(corrections.rot_correction); - corrections.sin_rot_correction = sinf(corrections.rot_correction); - - double cos_azimuth_half=cos(rotation*0.5); - - if(Type == CH16) - { - if(abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && - abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6 ){ - corrections.sin_vert_correction = sin_scan_laser_altitude[Laser_ring / 4 + 2] + - 2 * cos_azimuth_half * sin_scan_mirror_altitude[Laser_ring % 4]; - }else{ - corrections.sin_vert_correction = sin_scan_laser_altitude[Laser_ring / 4 + 2] + - 2 * cos_azimuth_half * sin(prism_angle[Laser_ring % 4] * M_PI / 180); - } - } - else if(Type == CH32) - { - corrections.sin_vert_correction=sin_scan_laser_altitude[Laser_ring/4]+2*cos_azimuth_half* sin_scan_mirror_altitude[Laser_ring%4]; - } - else if(Type == CH64) - { - if (Laser_ring % 8 == 0 || Laser_ring % 8 == 1 || Laser_ring % 8 == 2 || Laser_ring % 8 == 3) { - corrections.sin_vert_correction = sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33* DEG_TO_RAD) + - 2 * cos(rotation / 2 + 1.05* DEG_TO_RAD ) * - sin((Laser_ring % 4) * 0.33* DEG_TO_RAD); - } - else if (Laser_ring % 8 == 4 || Laser_ring % 8 == 5 || Laser_ring % 8 == 6 || Laser_ring % 8 == 7) { - corrections.sin_vert_correction = sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33* DEG_TO_RAD) + - 2 * cos(rotation / 2 - 1.05* DEG_TO_RAD ) * - sin((Laser_ring % 4) * 0.33* DEG_TO_RAD); - } - } - else if(Type == CH64w) - { - if (Laser_ring / 4 % 2 == 0) { - cos_xita = cos((rotation / 2.0 + 22.5) * M_PI / 180); - }else{ - cos_xita = cos((-rotation / 2.0 + 112.5) * M_PI / 180); - } - _R_ = theat2_c[Laser_ring] * theat1_c[Laser_ring] * cos_xita - theat2_s[Laser_ring] * theat1_s[Laser_ring]; - sin_theat = theat1_s[Laser_ring] + 2 * _R_ * theat2_s[Laser_ring]; - cos_theat = sqrt(1 - pow(sin_theat, 2)); - cos_H_xita = (2 * _R_ * theat2_c[Laser_ring] * cos_xita - theat1_c[Laser_ring]) / cos_theat; - sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); - - if(Laser_ring / 4 % 2 == 0){ - cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); - corrections.sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); - } else{ - cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); - corrections.sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); - } - } - else if(Type == CH120) - { - corrections.sin_vert_correction = sinf(corrections.vert_correction); - } - else if(Type == CH128) - { - if (Laser_ring / 4 % 2 == 0) { - cos_azimuth_half = sin((rotation - 15 * DEG_TO_RAD) * 0.5); - } else { - cos_azimuth_half = cos((rotation + 15 * DEG_TO_RAD) * 0.5); - } - corrections.sin_vert_correction = sin_scan_laser_altitude_ch128[Laser_ring / 4] + - 2 * cos_azimuth_half * sinTheta_2[Laser_ring]; + double x = 0.0; + double y = 0.0; + double z = 0.0; + double distance_corr_x = 0; + double distance_corr_y = 0; + double sinTheta_1[128] = {0}; + double cosTheta_1[128] = {0}; + double sinTheta_2[128] = {0}; + double cosTheta_2[128] = {0}; + double cos_xita; + // 垂直角度 + double sin_theat; + double cos_theat; + double _R_; + // 水平角度 + double cos_H_xita; + double sin_H_xita; + double cos_xita_F; + + for (int i = 0; i < 128; i++) { + sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); + cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); + cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); + } else { + sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); + cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); + } } - else if(Type == CH128X1) - { - double _R_ = cosTheta_2[Laser_ring] * cosTheta_1[Laser_ring] * - cos_azimuth_half - sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring]; - corrections.sin_vert_correction = sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring]; + + corrections->cos_rot_correction = cosf(corrections->rot_correction); + corrections->sin_rot_correction = sinf(corrections->rot_correction); + + double cos_azimuth_half = cos(rotation * 0.5); + + if (Type == CH16) { + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4 + 2] + + 2 * cos_azimuth_half * sin_scan_mirror_altitude[Laser_ring % 4]; + } else { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4 + 2] + + 2 * cos_azimuth_half * sin(prism_angle[Laser_ring % 4] * M_PI / 180); + } + } else if (Type == CH32) { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4] + + 2 * cos_azimuth_half * sin_scan_mirror_altitude[Laser_ring % 4]; + } else if (Type == CH64) { + if (Laser_ring % 8 == 0 || Laser_ring % 8 == 1 || Laser_ring % 8 == 2 || + Laser_ring % 8 == 3) { + corrections->sin_vert_correction = + sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(rotation / 2 + 1.05 * DEG_TO_RAD) * + sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD); + } else if (Laser_ring % 8 == 4 || Laser_ring % 8 == 5 || + Laser_ring % 8 == 6 || Laser_ring % 8 == 7) { + corrections->sin_vert_correction = + sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(rotation / 2 - 1.05 * DEG_TO_RAD) * + sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD); + } + } else if (Type == CH64w) { + if (Laser_ring / 4 % 2 == 0) { + cos_xita = cos((rotation / 2.0 + 22.5) * M_PI / 180); + } else { + cos_xita = cos((-rotation / 2.0 + 112.5) * M_PI / 180); + } + _R_ = theat2_c[Laser_ring] * theat1_c[Laser_ring] * cos_xita - + theat2_s[Laser_ring] * theat1_s[Laser_ring]; + sin_theat = theat1_s[Laser_ring] + 2 * _R_ * theat2_s[Laser_ring]; + cos_theat = sqrt(1 - pow(sin_theat, 2)); + cos_H_xita = + (2 * _R_ * theat2_c[Laser_ring] * cos_xita - theat1_c[Laser_ring]) / + cos_theat; + sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); + + if (Laser_ring / 4 % 2 == 0) { + cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); + corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); + } else { + cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); + corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); + } + } else if (Type == CH120) { + corrections->sin_vert_correction = sinf(corrections->vert_correction); + } else if (Type == CH128) { + if (Laser_ring / 4 % 2 == 0) { + cos_azimuth_half = sin((rotation - 15 * DEG_TO_RAD) * 0.5); + } else { + cos_azimuth_half = cos((rotation + 15 * DEG_TO_RAD) * 0.5); + } + corrections->sin_vert_correction = + sin_scan_laser_altitude_ch128[Laser_ring / 4] + + 2 * cos_azimuth_half * sinTheta_2[Laser_ring]; + } else if (Type == CH128X1) { + double _R_ = + cosTheta_2[Laser_ring] * cosTheta_1[Laser_ring] * cos_azimuth_half - + sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring]; + corrections->sin_vert_correction = + sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring]; } - corrections.cos_vert_correction = sqrt(1 - pow(corrections.sin_vert_correction, 2)); - - double distance = raw_distance + corrections.dist_correction; + corrections->cos_vert_correction = + sqrt(1 - pow(corrections->sin_vert_correction, 2)); + + double distance = raw_distance + corrections->dist_correction; - double cos_rot_angle = cos(rotation) * corrections.cos_rot_correction + - sin(rotation) * corrections.sin_rot_correction; - double sin_rot_angle = sin(rotation) * corrections.cos_rot_correction - - cos(rotation) * corrections.sin_rot_correction; + double cos_rot_angle = cos(rotation) * corrections->cos_rot_correction + + sin(rotation) * corrections->sin_rot_correction; + double sin_rot_angle = sin(rotation) * corrections->cos_rot_correction - + cos(rotation) * corrections->sin_rot_correction; // Compute the distance in the xy plane (w/o accounting for rotation) - double xy_distance = distance * corrections.cos_vert_correction; + double xy_distance = distance * corrections->cos_vert_correction; // Get 2points calibration values,Linear interpolation to get distance // correction for X and Y, that means distance correction use // different value at different distance - distance_corr_x = distance_corr_y = corrections.dist_correction; - + distance_corr_x = distance_corr_y = corrections->dist_correction; + double distance_x = raw_distance + distance_corr_x; - xy_distance = distance_x * corrections.cos_vert_correction; - - x = xy_distance * sin_rot_angle - corrections.horiz_offset_correction * cos_rot_angle; + xy_distance = distance_x * corrections->cos_vert_correction; + + x = xy_distance * sin_rot_angle - + corrections->horiz_offset_correction * cos_rot_angle; double distance_y = raw_distance + distance_corr_y; - xy_distance = distance_y * corrections.cos_vert_correction; + xy_distance = distance_y * corrections->cos_vert_correction; + + y = xy_distance * cos_rot_angle + + corrections->horiz_offset_correction * sin_rot_angle; + z = distance * corrections->sin_vert_correction + + corrections->vert_offset_correction; - y = xy_distance * cos_rot_angle + corrections.horiz_offset_correction * sin_rot_angle; - z = distance * corrections.sin_vert_correction + corrections.vert_offset_correction; - /** Use standard ROS coordinate system (right-hand rule) */ - point->set_x(static_cast(-y)); - point->set_y(static_cast(-x)); - point->set_z(static_cast(z)); + point->set_x(static_cast(-y)); + point->set_y(static_cast(-x)); + point->set_z(static_cast(z)); } LslidarParser *LslidarParserFactory::CreateParser(Config source_config) { Config config = source_config; - + if (config.model() == LSLIDAR16P) { return new Lslidar16Parser(config); - } - else if(config.model() == LSLIDAR32P){ - return new Lslidar32Parser(config); - } - else if(config.model() == LSLIDAR_C32_V4 ||config.model() == LSLIDAR_C16_V4 - ||config.model() == LSLIDAR_C8_V4 ||config.model() == LSLIDAR_C1_V4){ - return new LslidarCXV4Parser(config); - } - else if(config.model() == LSLIDAR_CH16){ - return new LslidarCH16Parser(config); - } - else if(config.model() == LSLIDAR_CH32){ - return new LslidarCH32Parser(config); - } - else if(config.model() == LSLIDAR_CH64){ - return new LslidarCH64Parser(config); - } - else if(config.model() == LSLIDAR_CH64w){ - return new LslidarCH64wParser(config); - } - else if(config.model() == LSLIDAR_CH120){ - return new LslidarCH120Parser(config); - } - else if(config.model() == LSLIDAR_CH128){ - return new LslidarCH128Parser(config); - } - else if(config.model() == LSLIDAR_CH128X1){ - return new LslidarCH128X1Parser(config); - } - else if(config.model() == LSLIDAR_LS128S2){ - return new LslidarLS128S2Parser(config); - } - else { + } else if (config.model() == LSLIDAR32P) { + return new Lslidar32Parser(config); + } else if (config.model() == LSLIDAR_C32_V4 || + config.model() == LSLIDAR_C16_V4 || + config.model() == LSLIDAR_C8_V4 || + config.model() == LSLIDAR_C1_V4) { + return new LslidarCXV4Parser(config); + } else if (config.model() == LSLIDAR_CH16) { + return new LslidarCH16Parser(config); + } else if (config.model() == LSLIDAR_CH32) { + return new LslidarCH32Parser(config); + } else if (config.model() == LSLIDAR_CH64) { + return new LslidarCH64Parser(config); + } else if (config.model() == LSLIDAR_CH64w) { + return new LslidarCH64wParser(config); + } else if (config.model() == LSLIDAR_CH120) { + return new LslidarCH120Parser(config); + } else if (config.model() == LSLIDAR_CH128) { + return new LslidarCH128Parser(config); + } else if (config.model() == LSLIDAR_CH128X1) { + return new LslidarCH128X1Parser(config); + } else if (config.model() == LSLIDAR_LS128S2) { + return new LslidarLS128S2Parser(config); + } else { AERROR << "invalid model"; return nullptr; } diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_parser.h b/modules/drivers/lidar/lslidar/parser/lslidar_parser.h index 170dd1ca421..50e5c90200d 100644 --- a/modules/drivers/lidar/lslidar/parser/lslidar_parser.h +++ b/modules/drivers/lidar/lslidar/parser/lslidar_parser.h @@ -53,10 +53,11 @@ #include #include #include +#include +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" #include "modules/drivers/lidar/lslidar/proto/config.pb.h" #include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" -#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" #include "cyber/cyber.h" #include "modules/drivers/lidar/lslidar/parser/calibration.h" @@ -173,21 +174,24 @@ const int ORDER_16[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; const int ORDER_32[32] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31}; - -static const double scan_altitude_original_A[32] = { // 1 v2.6 +// 1,v2.6 +static const double scan_altitude_original_A[32] = { -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; -static const double scan_altitude_original_A3[32] = { // 1 v3.0 +// 1,v3.0 +static const double scan_altitude_original_A3[32] = { -16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15}; -static const double scan_altitude_original_C[32] = { // 0.33 v2.6 +// 0.33,v2.6 +static const double scan_altitude_original_C[32] = { -18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66, -3, -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66, 1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14}; -static const double scan_altitude_original_C3[32] = { // 0.33 v3.0 +// 0.33,v3.0 +static const double scan_altitude_original_C3[32] = { -18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7, 0.66, -6, 1, -5, 1.33, -4, 1.66, -3.33, 2, -3, 3, -2.66, 4, -2.33, 6, -2, 8, -1.66, 11, -1.33, 14}; @@ -217,7 +221,6 @@ static const double c32_90_vertical_angle[32] = { 14.324, 36.008, 58.26, 79.938, 17.096, 38.808, 60.87, 82.884, 19.824, 41.603, 63.498, 85.933, 22.513, 44.404, 66.144, 89.105}; - static const double c16_vertical_angle[16] = { -16, 0, -14, 2, -12, 4, -10, 6, -8, 8, -6, 10, -4, 12, -2, 14}; @@ -472,7 +475,7 @@ class LslidarParser { */ virtual void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg) = 0; + const std::shared_ptr& out_msg) = 0; virtual void setup(); // Order point cloud fod IDL by lslidar model @@ -502,14 +505,13 @@ class LslidarParser { bool is_scan_valid(int rotation, float distance); - void ComputeCoords(const float& raw_distance, LaserCorrection& corrections, + 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, + LaserCorrection* corrections, const double rotation, PointXYZIT* point); - - }; // class LslidarParser class Lslidar16Parser : public LslidarParser { @@ -519,7 +521,7 @@ class Lslidar16Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -543,7 +545,7 @@ class Lslidar32Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -579,7 +581,7 @@ class LslidarCXV4Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); bool checkPacketValidity(const RawPacket_C32* packet); @@ -611,7 +613,6 @@ class LslidarCXV4Parser : public LslidarParser { int lidar_number_; bool is_new_c32w_; bool is_get_scan_altitude_; - }; // class LslidarCXV4Parser class Lslidar401Parser : public LslidarParser { @@ -621,7 +622,7 @@ class Lslidar401Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -644,7 +645,7 @@ class LslidarCH16Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -669,7 +670,7 @@ class LslidarCH32Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -694,7 +695,7 @@ class LslidarCH64Parser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -719,7 +720,7 @@ class LslidarCH64wParser : public LslidarParser { void GeneratePointcloud( const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -748,7 +749,7 @@ class LslidarCH120Parser : public LslidarParser { ~LslidarCH120Parser() {} void GeneratePointcloud(const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -772,7 +773,7 @@ class LslidarCH128Parser : public LslidarParser { ~LslidarCH128Parser() {} void GeneratePointcloud(const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -798,7 +799,7 @@ class LslidarCH128X1Parser : public LslidarParser { ~LslidarCH128X1Parser() {} void GeneratePointcloud(const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); private: @@ -821,7 +822,7 @@ class LslidarLS128S2Parser : public LslidarParser { ~LslidarLS128S2Parser() {} void GeneratePointcloud(const std::shared_ptr& scan_msg, - std::shared_ptr& out_msg); + const std::shared_ptr& out_msg); void Order(std::shared_ptr cloud); int convertCoordinate(const struct Firing_LS128S2& lidardata);