Measure/Modules/DataToCloud/rslidar/rslidar_driver.cpp

#include "rslidar_driver.h"

// 万集雷达封装类构造函数
RslidarDevice::RslidarDevice() {
    m_rs_lidar_device_status = E_UNKNOWN;
    m_decoder_ptr_ = nullptr;
}

// 万集雷达封装类析构函数
RslidarDevice::~RslidarDevice() {
    uninit();
}

Error_manager RslidarDevice::init() {
    return SUCCESS;
}

Error_manager RslidarDevice::init(RSDriverParam &param) {
    m_lidar_params = param;
    m_lidar_params.print();
    m_lidar_driver.regPointCloudCallback(std::bind(&RslidarDevice::driverGetPointCloudFromCallerCallback, this),
                                         std::bind(&RslidarDevice::driverReturnPointCloudToCallerCallback, this,
                                                   std::placeholders::_1));
    m_lidar_driver.regExceptionCallback(std::bind(&RslidarDevice::exceptionCallback, this, std::placeholders::_1));
    m_lidar_driver.regPacketCallback(std::bind(&RslidarDevice::packetCallback, this, std::placeholders::_1));
    if (m_lidar_driver.init(param)) {
        m_decoder_ptr_ = DecoderFactory<PointCloudMsg>::createDecoder(param.lidar_type, param.decoder_param);
        m_decoder_ptr_->regCallback(&decoderexceptionCallback, &decoderCallback);
        m_decoder_ptr_->point_cloud_ = m_point_cloud_;
        m_lidar_driver.start();
    } else {
        LOG(INFO) << param.input_param.msop_port << " init falture";
        return FAILED;
    }

    return SUCCESS;
}

Error_manager RslidarDevice::uninit() {
    m_lidar_driver.stop();
    return Error_code::SUCCESS;
}

Error_manager RslidarDevice::get_last_cloud(std::vector<unsigned char> &data) {
    if (m_lidar_data.empty()) {
        return FAILED;
    }

    {
        std::lock_guard<std::mutex> lck(m_cloud_mutex);
        data.swap(m_lidar_data.front());
        m_lidar_data.pop_front();
    }
    DLOG(INFO) << "M.size " << data.size() / 7 << "  " << m_lidar_data.size();

    return SUCCESS;
}

bool RslidarDevice::updateRSTransformParam(RSTransformParam &transform_param) {
    m_lidar_params.decoder_param.transform_param = transform_param;
    return true;
}

bool RslidarDevice::is_ready() {
    update_status();
    return m_rs_lidar_device_status == E_READY;
}

RslidarDevice::RSLIDAR_STATUS RslidarDevice::status() {
    update_status();
    return m_rs_lidar_device_status;
}

void RslidarDevice::update_status() {
    if (m_rs_protocol_status == E_FAULT) {
        m_rs_lidar_device_status = E_FAULT;
    } else if (m_rs_protocol_status == E_BUSY) {
        m_rs_lidar_device_status = E_BUSY;
    } else if (m_rs_protocol_status == E_READY) {
        m_rs_lidar_device_status = E_READY;
    } else {
        m_rs_lidar_device_status = E_UNKNOWN;
    }
}

std::shared_ptr<PointCloudMsg> RslidarDevice::driverGetPointCloudFromCallerCallback(void) {
    std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
    if (msg.get() != nullptr) {
        return msg;
    }
    return std::make_shared<PointCloudMsg>();
}

int RslidarDevice::id() {
    return m_lidar_params.input_param.msop_port;
}

void RslidarDevice::driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg) {
    // 先将之前队列的首个给释放回调函数
    free_cloud_queue.push(stuffed_cloud_queue.pop());

    // 再存入一个
    stuffed_cloud_queue.push(msg);
    DLOG(INFO) << "Get points form driver, point size: " << msg->points.size();

    // 转换
    if (!msg->points.empty()) {
        std::chrono::steady_clock::time_point ttt = std::chrono::steady_clock::now();
        DLOG(INFO) << m_decoder_ptr_->point_cloud_->points.begin()->x << " " << m_decoder_ptr_->point_cloud_->points.begin()->y;
        Packet packet;
        float x, y, z, x_y, r, roll, yaw;

        // 提前计算部分数据，可以在循环中少计算一点这些
        float trans_roll = (m_lidar_params.decoder_param.transform_param.roll * float(M_PI)) / 180.0f;
        float trans_pitch = (m_lidar_params.decoder_param.transform_param.pitch * float(M_PI)) / 180.0f;
        float trans_yaw = (m_lidar_params.decoder_param.transform_param.yaw * float(M_PI)) / 180.0f;
        float cosYaw = cos(trans_yaw);
        float sinYaw = sin(trans_yaw);
        float cosPitch = cos(trans_pitch);
        float sinPitch = sin(trans_pitch);
        float cosRoll = cos(trans_roll);
        float sinRoll = sin(trans_roll);
        float rotationMatrix[3][3] = {
                {cosYaw * cosPitch, cosYaw * sinPitch * sinRoll - sinYaw * cosRoll, cosYaw * sinPitch * cosRoll + sinYaw * sinRoll},
                {sinYaw * cosPitch, sinYaw * sinPitch * sinRoll + cosYaw * cosRoll, sinYaw * sinPitch * cosRoll - cosYaw * sinRoll},
                {-sinPitch, cosPitch * sinRoll, cosPitch * cosRoll}
        };

        float trans_x = m_lidar_params.decoder_param.transform_param.x * 0.01f;
        float trans_y = m_lidar_params.decoder_param.transform_param.y * 0.01f;
        float trans_z = m_lidar_params.decoder_param.transform_param.z * 0.01f;
        for (auto &point: msg->points) {
            if (point.intensity > 90) {
                continue;
            }

            x = rotationMatrix[0][0] * point.x + rotationMatrix[0][1] * point.y + rotationMatrix[0][2] * point.z + trans_x;
            y = rotationMatrix[1][0] * point.x + rotationMatrix[1][1] * point.y + rotationMatrix[1][2] * point.z + trans_y;
            z = rotationMatrix[2][0] * point.x + rotationMatrix[2][1] * point.y + rotationMatrix[2][2] * point.z + trans_z;

            r = sqrt(x * x + y * y + z * z) * 400.0f;
            roll = (atan2(z, sqrt(x * x + y * y)) * 18000.0f) / float(M_PI);
            yaw = (atan2(x, y) * 18000.0f) / float(M_PI);

            if (roll < 0) {
                roll += 36000;
            }
            if (roll > 36000) {
                roll -= 36000;
            }
            if (yaw < 0) {
                yaw += 36000;
            }
            if (yaw > 36000) {
                yaw -= 36000;
            }

            m_tmp_data.push_back((unsigned short) r >> 8);
            m_tmp_data.push_back((unsigned short) r);
            m_tmp_data.push_back((unsigned short) roll >> 8);
            m_tmp_data.push_back((unsigned short) roll);
            m_tmp_data.push_back((unsigned short) yaw >> 8);
            m_tmp_data.push_back((unsigned short) yaw);
            m_tmp_data.push_back(point.intensity);
        }

        std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
        if (diff.count() * 1000 > 10) {
                int tmp_size = m_tmp_data.size() / 7;
                for (int i = tmp_size; i < 2880 * 10; i++) {
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                    m_tmp_data.emplace_back(0);
                }
            m_capture_time = std::chrono::steady_clock::now();
            std::lock_guard<std::mutex> lck(m_cloud_mutex);
            DLOG(INFO) << "save cloud data size: " << m_tmp_data.size() / 7;
            m_lidar_data.push_back(m_tmp_data);
            m_tmp_data.clear();
            if (m_lidar_data.size() > 100) {
                m_lidar_data.pop_front();
            }
        }
    }
}

void RslidarDevice::exceptionCallback(const Error &code) {

}

void RslidarDevice::packetCallback(const Packet &pkt) {
    return;
}