diff --git a/demo/demo_online.cpp b/demo/demo_online.cpp index fbea5c52..e6e214fb 100644 --- a/demo/demo_online.cpp +++ b/demo/demo_online.cpp @@ -76,7 +76,7 @@ int main(int argc, char* argv[]) RSDriverParam param; ///< Create a parameter object param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 - param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct param.print(); driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver @@ -95,4 +95,4 @@ int main(int argc, char* argv[]) } return 0; -} +} \ No newline at end of file diff --git a/demo/demo_pcap.cpp b/demo/demo_pcap.cpp index 0eb117ab..35c0f03b 100644 --- a/demo/demo_pcap.cpp +++ b/demo/demo_pcap.cpp @@ -78,7 +78,7 @@ int main(int argc, char* argv[]) param.input_param.pcap_path = "/home/robosense/lidar.pcap"; ///< Set the pcap file directory param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699 param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788 - param.lidar_type = LidarType::RSM1; ///< Set the lidar type. Make sure this type is correct + param.lidar_type = LidarType::RS16; ///< Set the lidar type. Make sure this type is correct param.print(); driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function into the driver @@ -97,4 +97,4 @@ int main(int argc, char* argv[]) } return 0; -} +} \ No newline at end of file diff --git a/src/rs_driver/driver/decoder/decoder_RS128_40.hpp b/src/rs_driver/driver/decoder/decoder_RS128_40.hpp new file mode 100644 index 00000000..5dbca121 --- /dev/null +++ b/src/rs_driver/driver/decoder/decoder_RS128_40.hpp @@ -0,0 +1,199 @@ +/********************************************************************************************************************* +Copyright (c) 2020 RoboSense +All rights reserved + +By downloading, copying, installing or using the software you agree to this license. If you do not agree to this +license, do not download, install, copy or use the software. + +License Agreement +For RoboSense LiDAR SDK Library +(3-clause BSD License) + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used +to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, +INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*********************************************************************************************************************/ + +#pragma once +#include +#include +namespace robosense +{ +namespace lidar +{ + +template +class DecoderRS128_40 : public DecoderRS128 +{ +public: + explicit DecoderRS128_40(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param); + RSDecoderResult decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, int& azimuth); +}; + +template +inline DecoderRS128_40::DecoderRS128_40(const RSDecoderParam& param, const LidarConstantParameter& lidar_const_param) + : DecoderRS128(param, lidar_const_param) +{ +} + +template +inline RSDecoderResult DecoderRS128_40::decodeMsopPkt(const uint8_t* pkt, std::vector& vec, int& height, + int& azimuth) +{ + height = this->lidar_const_param_.LASER_NUM; + + const RS128MsopPkt* mpkt_ptr = reinterpret_cast(pkt); + if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID) + { + return RSDecoderResult::WRONG_PKT_HEADER; + } + + this->protocol_ver_ = RS_SWAP_SHORT(mpkt_ptr->header.protocol_version); + + azimuth = RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth); + this->check_camera_trigger_func_(azimuth, pkt); + + this->current_temperature_ = this->computeTemperature(mpkt_ptr->header.temp_low, mpkt_ptr->header.temp_high); + + double block_timestamp = this->get_point_time_func_(pkt); + + float azi_diff = 0; + for (uint16_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) + { + if (mpkt_ptr->blocks[blk_idx].id != this->lidar_const_param_.BLOCK_ID) + { + break; + } + + int cur_azi = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].azimuth); + + if (this->echo_mode_ == ECHO_DUAL) + { + azi_diff = static_cast( + (RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[2].azimuth) - RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth)) % + RS_ONE_ROUND); + + if (RS_SWAP_SHORT(mpkt_ptr->blocks[0].azimuth) == RS_SWAP_SHORT(mpkt_ptr->blocks[1].azimuth)) ///< AAB + { + if (blk_idx == 2) + { + block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : + (block_timestamp + this->time_duration_between_blocks_); + } + } + else ///< ABB + { + if (blk_idx == 1) + { + block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : + (block_timestamp + this->time_duration_between_blocks_); + } + } + } + else + { + if (blk_idx < (this->lidar_const_param_.BLOCKS_PER_PKT - 1)) + { + azi_diff = static_cast((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % + RS_ONE_ROUND); + } + else + { + azi_diff = static_cast((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % + RS_ONE_ROUND); + } + + if (blk_idx > 0) + { + block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : + (block_timestamp + this->time_duration_between_blocks_); + } + } + + azi_diff = (azi_diff > 100) ? this->azi_diff_between_block_theoretical_ : azi_diff; + + for (uint16_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) + { + static const float tss[128] = + { + 0.24f, 0.24f, 0.24f, 0.24f, 1.084f, 1.084f, 1.084f, 1.084f, + 1.932f, 1.932f, 1.932f, 1.932f, 2.872f, 2.872f, 2.872f, 2.872f, + 3.824f, 3.824f, 3.824f, 3.824f, 4.684f, 4.684f, 4.684f, 4.684f, + 5.540f, 5.540f, 5.540f, 5.540f, 6.496f, 6.496f, 6.496f, 6.496f, + 7.440f, 7.440f, 7.440f, 7.440f, 9.348f, 9.348f, 9.348f, 9.348f, + 11.260f, 11.260f, 11.260f, 11.260f, 13.264f, 13.264f, 13.264f, 13.264f, + 15.280f, 15.280f, 15.280f, 15.280f, 17.204f, 17.204f, 17.204f, 17.204f, + 19.124f, 19.124f, 19.124f, 19.124f, 21.144f, 21.144f, 21.144f, 21.144f, + 23.152f, 23.152f, 23.152f, 23.152f, 25.060f, 25.060f, 25.060f, 25.060f, + 26.972f, 26.972f, 26.972f, 26.972f, 28.976f, 28.976f, 28.976f, 28.976f, + 30.992f, 30.992f, 30.992f, 30.992f, 32.916f, 32.916f, 32.916f, 32.916f, + 34.836f, 34.836f, 34.836f, 34.836f, 36.856f, 36.856f, 36.856f, 36.856f, + 38.864f, 38.864f, 38.864f, 38.864f, 40.272f, 40.272f, 40.272f, 40.272f, + 41.684f, 41.684f, 41.684f, 41.684f, 43.188f, 43.188f, 43.188f, 43.188f, + 44.704f, 44.704f, 44.704f, 44.704f, 46.128f, 46.128f, 46.128f, 46.128f, + 47.548f, 47.548f, 47.548f, 47.548f, 49.068f, 49.068f, 49.068f, 49.068f + }; + static const float blk_ts = 50.984f; + + float azi_channel_ori = cur_azi + azi_diff * tss[channel_idx] / blk_ts; + + int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx); + float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * + this->lidar_const_param_.DIS_RESOLUTION; + //int angle_horiz = static_cast(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND; + int angle_horiz_x = static_cast(azi_channel_ori - this->lidar_alph0_ + RS_ONE_ROUND) % RS_ONE_ROUND; + int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND; + + T_Point point; + if ((distance <= this->param_.max_distance && distance >= this->param_.min_distance) && + ((this->angle_flag_ && azi_channel_final >= this->start_angle_ && azi_channel_final <= this->end_angle_) || + (!this->angle_flag_ && + ((azi_channel_final >= this->start_angle_) || (azi_channel_final <= this->end_angle_))))) + { + float x = distance * this->checkCosTable(angle_vert) * this->checkCosTable(azi_channel_final) + + this->lidar_Rxy_ * this->checkCosTable(angle_horiz_x); + float y = -distance * this->checkCosTable(angle_vert) * this->checkSinTable(azi_channel_final) - + this->lidar_Rxy_ * this->checkSinTable(angle_horiz_x); + float z = distance * this->checkSinTable(angle_vert) + this->lidar_const_param_.RZ; + + uint8_t intensity = mpkt_ptr->blocks[blk_idx].channels[channel_idx].intensity; + this->transformPoint(x, y, z); + setX(point, x); + setY(point, y); + setZ(point, z); + setIntensity(point, intensity); + } + else + { + setX(point, NAN); + setY(point, NAN); + setZ(point, NAN); + setIntensity(point, 0); + } + setRing(point, this->beam_ring_table_[channel_idx]); + setTimestamp(point, block_timestamp); + vec.emplace_back(std::move(point)); + } + } + + return RSDecoderResult::DECODE_OK; +} + +} // namespace lidar +} // namespace robosense diff --git a/src/rs_driver/driver/decoder/decoder_base.hpp b/src/rs_driver/driver/decoder/decoder_base.hpp index b3ea8bea..5a1ce2b1 100644 --- a/src/rs_driver/driver/decoder/decoder_base.hpp +++ b/src/rs_driver/driver/decoder/decoder_base.hpp @@ -852,6 +852,19 @@ inline RSEchoMode DecoderBase::getEchoMode(const LidarType& type, const { switch (type) { + case LidarType::RS128_40: + switch (return_mode) + { + case 0x00: + case 0x01: + case 0x02: + return RSEchoMode::ECHO_SINGLE; + case 0x03: + case 0x04: + case 0x05: + default: + return RSEchoMode::ECHO_DUAL; + } case LidarType::RS128: case LidarType::RS80: case LidarType::RSHELIOS: diff --git a/src/rs_driver/driver/decoder/decoder_factory.hpp b/src/rs_driver/driver/decoder/decoder_factory.hpp index 74170e5e..181408ee 100644 --- a/src/rs_driver/driver/decoder/decoder_factory.hpp +++ b/src/rs_driver/driver/decoder/decoder_factory.hpp @@ -34,6 +34,7 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -59,10 +60,9 @@ class DecoderFactory static const LidarConstantParameter getRSBPConstantParam(); static const LidarConstantParameter getRS80ConstantParam(); static const LidarConstantParameter getRS128ConstantParam(); - static const LidarConstantParameter getRSRUBY_PLUSConstantParam(); + static const LidarConstantParameter getRS128_40ConstantParam(); static const LidarConstantParameter getRSM1ConstantParam(); static const LidarConstantParameter getRSHELIOSConstantParam(); - static const LidarConstantParameter getRSHELIOS_16PConstantParam(); static const LidarConstantParameter getRSROCKConstantParam(); }; @@ -84,6 +84,9 @@ inline std::shared_ptr> DecoderFactory::createDeco case LidarType::RS128: ret_ptr = std::make_shared>(param.decoder_param, getRS128ConstantParam()); break; + case LidarType::RS128_40: + ret_ptr = std::make_shared>(param.decoder_param, getRS128_40ConstantParam()); + break; case LidarType::RS80: ret_ptr = std::make_shared>(param.decoder_param, getRS80ConstantParam()); break; @@ -205,7 +208,7 @@ inline const LidarConstantParameter DecoderFactory::getRS128ConstantPar } template -inline const LidarConstantParameter DecoderFactory::getRSRUBY_PLUSConstantParam() +inline const LidarConstantParameter DecoderFactory::getRS128_40ConstantParam() { LidarConstantParameter ret_param; ret_param.MSOP_ID = 0x5A05AA55; @@ -257,26 +260,6 @@ inline const LidarConstantParameter DecoderFactory::getRSHELIOSConstant return ret_param; } -template -inline const LidarConstantParameter DecoderFactory::getRSHELIOS_16PConstantParam() -{ - LidarConstantParameter ret_param; - ret_param.MSOP_ID = 0x5A05AA55; - ret_param.DIFOP_ID = 0x555511115A00FFA5; - ret_param.BLOCK_ID = 0xEEFF; - ret_param.PKT_RATE = 750; - ret_param.BLOCKS_PER_PKT = 12; - ret_param.CHANNELS_PER_BLOCK = 32; - ret_param.LASER_NUM = 16; - ret_param.DSR_TOFFSET = 1.0; - ret_param.FIRING_FREQUENCY = 0.018; - ret_param.DIS_RESOLUTION = 0.0025; - ret_param.RX = 0.03498; - ret_param.RY = -0.015; - ret_param.RZ = 0.0; - return ret_param; -} - // TODO template inline const LidarConstantParameter DecoderFactory::getRSROCKConstantParam() diff --git a/src/rs_driver/driver/driver_param.h b/src/rs_driver/driver/driver_param.h index ce3d33d8..e5cb7164 100644 --- a/src/rs_driver/driver/driver_param.h +++ b/src/rs_driver/driver/driver_param.h @@ -43,6 +43,7 @@ enum LidarType ///< LiDAR type RS32, RSBP, RS128, + RS128_40, RS80, RSHELIOS, RSROCK, @@ -197,6 +198,9 @@ typedef struct RSDriverParam ///< The LiDAR driver parameter case LidarType::RS128: str = "RS128"; break; + case LidarType::RS128_40: + str = "RS128_40"; + break; case LidarType::RS80: str = "RS80"; break; @@ -233,6 +237,10 @@ typedef struct RSDriverParam ///< The LiDAR driver parameter { return lidar::LidarType::RS128; } + else if (type == "RS128_40") + { + return lidar::LidarType::RS128_40; + } else if (type == "RS80") { return lidar::LidarType::RS80; diff --git a/src/rs_driver/driver/input.hpp b/src/rs_driver/driver/input.hpp index ffd0ac20..fb97385d 100644 --- a/src/rs_driver/driver/input.hpp +++ b/src/rs_driver/driver/input.hpp @@ -505,6 +505,7 @@ inline void Input::getPcapPacket() time2go += std::chrono::microseconds(static_cast(RSBP_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); break; case LidarType::RS128: + case LidarType::RS128_40: time2go += std::chrono::microseconds(static_cast(RS128_PCAP_SLEEP_DURATION / input_param_.pcap_rate)); break; diff --git a/src/rs_driver/driver/lidar_driver_impl.hpp b/src/rs_driver/driver/lidar_driver_impl.hpp index f4d355a2..85d8153d 100644 --- a/src/rs_driver/driver/lidar_driver_impl.hpp +++ b/src/rs_driver/driver/lidar_driver_impl.hpp @@ -417,7 +417,6 @@ inline void LidarDriverImpl::processMsop() msop_pkt_queue_.is_task_finished_.store(true); return; } - while (msop_pkt_queue_.size() > 0) { PacketMsg pkt = msop_pkt_queue_.popFront(); @@ -482,7 +481,6 @@ inline void LidarDriverImpl::processMsop() reportError(Error(ERRCODE_CLOUDOVERFLOW)); } } - msop_pkt_queue_.is_task_finished_.store(true); }