SLAM-OpenGL实现rplidar A2激光雷达扫描显示

待我称王封你为后i 2022-03-20 15:08 475阅读 0赞

//
    // Created by PulsarV on 18-10-26.
    //
    
    #include <rplidar.h>
    #include <GL/glut.h>
    #include <projects.h>
    #include <rplidar_driver.h>
    #include <unistd.h>
    #include <cstdio>
    #include <signal.h>
    #include <cstdlib>
    #include <iostream>
    
    #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
    
    #define GL_WIDTH 480
    #define MAX_RADAR_DIST 1500 //最大扫距离
    #define RADAR_STEP 0.01 //雷达扫描线移动距离
    using namespace rp::standalone::rplidar;
    //雷达扫描线起始坐标
    float p1_x = 0, p1_y = 0;
    float p2_x = 0, p2_y = 0;
    //雷达扫描线起始步长
    float p1step = 1.75f;
    float p2step = 2.25f;
    RPlidarDriver *drv;
    //扫描点
    class DOT {
    public:
        double theta, dist;
    
        DOT(double theta, double dist) {
            this->dist = dist;
            this->theta = theta;
        }
    };
    
    std::vector<DOT> dots;
    
    //绘制线
    void draw_line(float x1, float y1, float x2, float y2) {
        GLfloat line_size = 3;
        glLineWidth(line_size);
        glBegin(GL_LINES);
        glVertex2f(x1, y1);
        glVertex2f(x2, y2);
        glEnd();
    }
    //绘制点
    void draw_point(float x, float y, float R, float G, float B, float alpha) {
        GLfloat point_size = 5;
        glPointSize(point_size);
        glColor4f(R, G, B, alpha);
        glBegin(GL_POINTS);
        glVertex2f(x, y);
        glEnd();
    }
    
    
    void draw_rader() {
        glClear(GL_COLOR_BUFFER_BIT);
        glClearColor(0, 0, 0, 0);
        glColor4f(0, 1, 0, 0);
        int n = 1000;
        for (float scale = 1; scale > 0.1; scale -= 0.1) {
            glBegin(GL_LINE_LOOP);
            for (int i = 0; i < n; i++) {
                glVertex2f(scale * cos(2 * PI * i / n), scale * sin(2 * PI * i / n));   //定义顶点
            }
            glEnd();
        }
        glColor4f(1, 0, 0, 0);
    	//绘制雷达界面
        draw_line(0, 0, 0, 1);
        draw_line(0, 0, 1, 1);
        draw_line(0, 0, -1, 1);
        draw_line(0, 0, 1, -1);
        draw_line(0, 0, -1, -1);
        draw_line(0, 0, 0, -1);
        draw_line(0, 0, 1, 0);
        draw_line(0, 0, -1, 0);
        draw_point(p1_x, p1_y, 1, 1, 1, 0);
        draw_point(p2_x, p2_y, 1, 1, 1, 0);
        draw_line(0, 0, p1_x, p1_y);
        draw_line(0, 0, p2_x, p2_y);
        std::vector<DOT>::iterator it;
    	//绘制扫描点
        for (it = dots.begin(); it != dots.end(); ++it) {
            float x = 0, y = 0;
            x = cos(it->theta / 180 * PI) * (it->dist / MAX_RADAR_DIST);
            y = sin(it->theta / 180 * PI) * (it->dist / MAX_RADAR_DIST);
            draw_point(x, y, 0, 0, 1, 0);
        }
    
        glFlush();
        glutSwapBuffers();
    }
    //通过坐标获取角度
    float get_angle(float x, float y) {
        float trangel = atan(x / y) * 180 / PI;
        if (y < 0 && x > 0 || y < 0 && x < 0)
            trangel = trangel + 180;
        if (x < 0 && y > 0)
            trangel = trangel + 360;
        trangel = trangel == 360 ? 0 : trangel;
    }
    //超时函数
    void TimerFunction(int value) {
        p1_x = sin(PI * p1step);
        p1_y = cos(PI * p1step);
        p2_x = sin(PI * p2step);
        p2_y = cos(PI * p2step);
        if (p1step == 2.5 + 1.75)
            p1step = 1.75;
        if (p2step == 2.5 + 2.25)
            p2step = 2.25;
        p1step += RADAR_STEP;
        p2step += RADAR_STEP;
        rplidar_response_measurement_node_t nodes[360 * 22];
        size_t count = _countof(nodes);
        u_result op_result;
        op_result = drv->grabScanData(nodes, count);
    	//获取扫描点并把它放到vector中
        if (IS_OK(op_result)) {
            drv->ascendScanData(nodes, count);
    		//清空之前的扫描点
            dots.clear();
            for (int pos = 0; pos < (int) count; ++pos) {
                DOT dot(((nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f),
                        (nodes[pos].distance_q2 / 4.0f));
                dots.push_back(dot);
            }
        }
    
        glutPostRedisplay();
        glutTimerFunc(1, TimerFunction, 1);
    //    std::cout<<"当前角度1: "<<get_angle(p1_x,p1_y)<<" 当前角度2: "<<get_angle(p2_x,p2_y)<<std::endl;
    }
    //雷达状态检测函数
    bool checkRPLIDARHealth(RPlidarDriver *drv) {
        u_result op_result;
        rplidar_response_device_health_t healthinfo;
        op_result = drv->getHealth(healthinfo);
        if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
            printf("RPLidar health status : %d\n", healthinfo.status);
            if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
                fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
                return false;
            } else {
                return true;
            }
    
        } else {
            fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
            return false;
        }
    }
    //OpenGL按钮事件 当按下q时，退出雷达
    void KeyBoards(unsigned char key, int x, int y) {
        switch (key) {
            case 'q':
                drv->stop();
                drv->stopMotor();
                RPlidarDriver::DisposeDriver(drv);
                drv = NULL;
                exit(0);
                break;
        }
    }
    
    int main(int argc, char **argv) {
        const char *opt_com_path = NULL;
        _u32 baudrateArray[2] = {115200, 256000};
        _u32 opt_com_baudrate = 0;
        u_result op_result;
        bool useArgcBaudrate = false;
    
        opt_com_path = "/dev/ttyUSB0";
        bool connectSuccess = false;
        rplidar_response_device_info_t devinfo;
    
        drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
        // 连接雷达
        if (useArgcBaudrate) {
            if (!drv)
                drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
            if (IS_OK(drv->connect(opt_com_path, opt_com_baudrate))) {
                op_result = drv->getDeviceInfo(devinfo);
    
                if (IS_OK(op_result)) {
                    connectSuccess = true;
                } else {
                    delete drv;
                    drv = NULL;
                }
            }
        } else {
            size_t baudRateArraySize = (sizeof(baudrateArray)) / (sizeof(baudrateArray[0]));
            for (size_t i = 0; i < baudRateArraySize; ++i) {
                if (!drv)
                    drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
                if (IS_OK(drv->connect(opt_com_path, baudrateArray[i]))) {
                    op_result = drv->getDeviceInfo(devinfo);
    
                    if (IS_OK(op_result)) {
                        connectSuccess = true;
                        break;
                    } else {
                        delete drv;
                        drv = NULL;
                    }
                }
            }
        }
        if (!connectSuccess) {
            fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n", opt_com_path);
            exit(0);
        }
        printf("RPLIDAR S/N: ");
        for (int pos = 0; pos < 16; ++pos) {
            printf("%02X", devinfo.serialnum[pos]);
        }
    
        printf("\n"
                       "Firmware Ver: %d.%02d\n"
                       "Hardware Rev: %d\n", devinfo.firmware_version >> 8, devinfo.firmware_version & 0xFF,
               (int) devinfo.hardware_version);
    
    
    
        // 检测雷达状态
        if (!checkRPLIDARHealth(drv)) {
            exit(0);
        }
    
        signal(SIGINT, ctrlc);
    
        drv->startMotor();
        // 开启雷达扫描
        drv->startScan(0, 1);
        drv->startMotor();
        glutInit(&argc, argv);
        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);//设置显示方式
        glutInitWindowPosition(100, 100);
        glutInitWindowSize(GL_WIDTH, GL_WIDTH);//设置窗口大小
        glMatrixMode(GL_PROJECTION);//设定投影方式
        glutCreateWindow("radar map");
        glutKeyboardFunc(&KeyBoards);
        glutDisplayFunc(draw_rader);
        glutTimerFunc(1, TimerFunction, 1);
        glutMainLoop();
    
        return 0;
    }

转载至链接:https://my.oschina.net/VenusV/blog/2253177