文档章节

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

Pulsar-V
 Pulsar-V
发布于 2018/10/26 23:30
字数 963
阅读 106
收藏 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;
}

© 著作权归作者所有

Pulsar-V

Pulsar-V

粉丝 61
博文 150
码字总数 135866
作品 1
成都
后端工程师
私信 提问
OpenGL开发之旅基础知识介绍

OpenGL开发之旅基础知识介绍 glLoadIdentity();gluortho2D(-1.0,1.0,-1.0,1.0) glEnd(); glPushMatrix();glPopMatrix(); 0000FF;">void) { // 清除颜色缓冲区 glClear(GLCOLORBUFFERBIT); /......

andyhe91
2013/05/11
157
0
轻量级高品质视频播放器 MPV 0.27.0 发布

MPV 0.27.0 已发布,MPV 是一款基于 mplayer2 和 MPlayer 的轻量级高品质视频播放器。MPV 基于 OpenGL 视频输出,支持视频缩放、高质量算法、色彩管理、帧定时、插值、HDR 等功能。同时,利用...

王练
2017/09/16
2.6K
2
opengl超级宝典笔记——Using Opengl

第二章 使用OpenGL (一)OpenGl 的工作原理 OpenGL是过程式的而非描述性的图形API。开发人员只须规定必要的步骤去实现显示的效果。这些步骤包含了许多OpenGL的命令。这些命令用于绘制许多的...

Mario_Q
2013/09/02
1K
0
Android 使用 OpenGL ES 绘制三角形

1. OpenGL ES 简介 OpenGL 是一个跨平台的图形 API,为 3D 图形处理硬件制定了一个标准软件接口。OpenGL ES 是为嵌入式设备设计的 OpenGL 规范,Android 提供了对 OpenGL ES 的支持。 OpenGL...

落英坠露
05/02
0
0
用多线程方法实现在MFC/WIN32中调用OpenGL函数并创建OpenGL窗口

OpenGL相关的工具库中的OpenGL程序往往都是在C函数main中初始化和创建的,使用控制台来完成显示和控制颇为不便。如果能够在MFC中OpenGL函数并创建OpenGL窗口,并且可以将控制参数传入给OpenG...

guoliang
2014/05/29
622
0

没有更多内容

加载失败,请刷新页面

加载更多

反编译9.png图片还原

本文链接:https://blog.csdn.net/a1140778530/article/details/10528507 经常反编译apk文件找资源,9.png的文件处理起来很麻烦。 最近使用Ant自动编译打包app时,从别处搜罗来的9.png文件导...

shzwork
26分钟前
6
0
Shell脚本应用 – for、while循环语句

一、for循环语句 在实际工作中,经常会遇到某项任务需要多次执行的情况,而每次执行时仅仅是处理的对象不一样,其他命令相同。例如:根据通讯录中的姓名列表创建系统账号等情况。 当面对各种...

linux-tao
26分钟前
5
0
RPA风潮下企业财务工作模式的变革

RPA(机器人流程自动化)在财务领域的应用,正给企业财务带来前所未有的改变。 前RPA时代,财务领域面临的痛点 在RPA机器人应用之前,企业财务工作进程的推进,主要通过财务人员人工操作或信...

UiBot
31分钟前
5
0
Hive之命令行修改表注释

最近遇到一个需求,在不重建表的情况下,修改表的注释,hive有没有类似关系型数据库的SQL命令来修改呢,找了下,亲测有效,如下List-1 List-1 hive>use your_schemahvie>ALTER TABLE tabl...

克虏伯
32分钟前
5
0
是什么,它的作用是什么

在HTML文档的首部往往会有这么一句话<!DOCTYPE html>,许多时候我们忽视了它的存在,它实际上是一个声明,告诉浏览器用哪种HTML版本的规范来解读HTML文档。 尽管我们不给出这句声明浏览器照样...

前端老手
37分钟前
4
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部