Matlab+Vrep 激光雷达2D动态建图

Matlab+Vrep仿真激光雷达2D建图

写在前面

这是我的第一篇CSDN博客，写得很烂是肯定的，会有点伤眼睛，请事先做好准备。
本人是自动化类研一新生，即将在今年9月份入学，我也是接触Vrep不久，之前本科阶段都是在做嵌入式软硬件开发的工作，研究生阶段换了研究方向，要用到Vrep仿真，这篇文章就记录一下我的一个练手小项目，用Matlab+Vrep实现动态建图。
文章中有三个小工程：Vrep工程，Matlab工程与python工程，其中python工程就是写了一个小的遥控器来遥控小车动起来而已。
此外，这个工程是在windows下运行的。

最终效果视频

视频链接：
(bilibili) Matlab+Vrep动态建图

项目Github

Github链接：
点这里直接跳转
https://github.com/wangrui449966/Matlab_Vrep_2d_mapping

用法：
分别用Vrep、Matlab（我用的是R2019a 64-bit）、python（建议使用pyCharm）打开三个工程以后，
先运行Vrep仿真，然后运行Matlab和python工程，选中python工程的窗口，按键盘上的上下左右键来控制小车的运动，同时matlab会进行动态建图。
注：python需要安装pygame包（pip install pygame）

带有RemoteApi的工程创建

Vrep为了与其他语言联调，提供了很方便的remoteApi

带有RemoteApi的Matlab工程创建

在Vrep的安装目录中可以找到包含RemoteApi的.m文件的文件夹

[Vrep安装目录]\programming\remoteApiBindings\matlab\matlab

我们只要用到remApi.m与remoteApiProto.m即可
将这两个文件复制到自己的matlab工程的目录里，就可以调用Vrep的RemoteApi了。

由于RemoteApi的底层封装成了dll文件，需要将dll文件复制到matlab的相应文件夹下。
在 [vrep安装目录]\programming\remoteApiBindings\lib\lib\64Bit 中有 remoteApi.dll，将它复制到 [matlab安装目录]\bin\win64 中即可。
到此，带有RemoteApi的Matlab工程创建完毕。

带有RemoteApi的python工程创建

在这个项目中，这不是重点，只是用python写了一个遥控器来控制小车的运动

在Vrep的安装目录中可以找到包含RemoteApi的.py文件的文件夹

[Vrep安装目录]\programming\remoteApiBindings\python\python

我们只要用到vrep.py与vrepConst.py即可
将这两个文件复制到自己的python工程目录里，就可以调用Vrep的RemoteApi了。

由于RemoteApi的底层封装成了dll文件，需要将dll文件复制到python工程目录下。
在**[vrep安装目录]\programming\remoteApiBindings\lib\lib\64Bit** 中有 remoteApi.dll，将它复制到python工程目录下即可。

使用remoteApi与Vrep连接起来

这个项目中用到了两个终端（python端与matlab端）来与Vrep通信，所以需要为这两个终端各开一个端口

1. Matlab端：
   在Vrep中，打开脚本，在初始化体内添加Socket终端，在这个项目中，我用的是dr20这个小车的脚本

if (sim_call_type==sim_childscriptcall_initialization) then 

    simExtRemoteApiStart(66666)-- Socket Port=66666(for matlab)

在matlab中，添加如下代码即可连接到Vrep

vrep=remApi('remoteApi'); % vrep remoteApi  remApi类实体
vrep.simxFinish(-1); % 关闭所有已经连接的remote，实测不会影响python终端，如果影响了，直接把这句删了完事
dr20ID=c_vrep.simxStart('127.0.0.1',66666,true,true,5000,5);%66666是小车的端口

2. python端：
   在Vrep中，打开脚本，在初始化体内添加Socket终端，在这个项目中，我用的是dr20这个小车的脚本

if (sim_call_type==sim_childscriptcall_initialization) then 

    simExtRemoteApiStart(56666)-- Socket Port=56666(for python)

在python中，添加以下代码即可连接到Vrep

import vrep
# vrep.simxFinish(-1)  # 断开所有已经连接的remote，这里不执行，避免把matlab中的连接断开
# 我直接把断开所有已经连接的remote给注释了，也没有测试不注释的情况下会不会影响matlab的连接
dr20ID = vrep.simxStart("127.0.0.1", 56666, True, True, 5000, 5)

工程代码细节

Vrep工程脚本

Vrep工程脚本 本文主要就介绍打开Socket端口，至于激光雷达与小车在Vrep中的通信，本文不详细介绍，以后有时间专门另写一篇文章作详细介绍。

打开RemoteApi Socket端口：
在dr20的脚本文件中的初始化段添加如下代码，分别为matlab与python终端打开RemoteApi的Socket接口：

if (sim_call_type==sim_childscriptcall_initialization) then 

    simExtRemoteApiStart(66666)--WR:Socket Port=66666(for matlab)
    simExtRemoteApiStart(56666)--WR:Socket Port=56666(for python)

Matlab 工程代码细节

在Github上传的工程中已经有比较详细的代码注释，本文将进一步讲解。
（太懒了，有空再写）

python 工程代码细节

我之前是用C和C++比较多，学python还没有一个月，发现这个语言是真的很简单，包也很多很好用，而且作为脚本语言开发起来很方便，不用像CPP工程那样还需要繁琐的配置工程的一些步骤，但是总感觉少了灵魂。

1. 首先，导入包，这没啥好说的

import pygame as pg  # pygame 包来读取键盘
import vrep

2. 设置两个方便调节的参数，一个是线速度，一个是角速度，用于遥控

#参数，线速度参数和角速度参数
SET_LINE_SPEED = 3.0
SET_ROTATE_SPEED = 1.0

3. 初始化Vrep与pygame
   vrep初始化：
   当simxStart返回的clientID小于0时表示获取失败了，表明连接失败，在本项目中出现这种情况有可能是vrep的仿真没有运行。
   如果clientID获取成功，则继续获取轮子的句柄，供之后遥控小车时使用。
   pygame初始化：
   建立一个320*240的窗口，取名叫"vrep_car_control_window"

if True:  # 初始化
    # vrep初始化
    # vrep.simxFinish(-1)  # 断开所有已经连接的remote，这里不执行，避免把matlab中的连接断开
    dr20ID = vrep.simxStart("127.0.0.1", 56666, True, True, 5000, 5)
    if dr20ID < 0:
        print("vrep_connection_failed")
        exit()
    else:
        print("vrep_connection_success")
        handle_left_wheel = vrep.simxGetObjectHandle(dr20ID, "dr20_leftWheelJoint_", vrep.simx_opmode_blocking)
        handle_right_wheel = vrep.simxGetObjectHandle(dr20ID, "dr20_rightWheelJoint_", vrep.simx_opmode_blocking)
        vrep.simxAddStatusbarMessage(dr20ID, "python_remote_connected\n", vrep.simx_opmode_oneshot)

    # pygame初始化
    pg.init()
    window = pg.display.set_mode((320, 240))
    pg.display.set_caption("vrep_car_control_window")

4. 正常运行时的主循环：
   在主循环中采集键盘的按键情况，并分别判断上下左右是否被按下，并用窗口里的红色小方块的位置来代表按键情况。通过按键情况来设置左右车轮的速度，并发送给Vrep来控制小车的运动。（小车是两轮差速模型）

# 正常运行
end_flag = False
while not end_flag:
    pg.time.delay(30)

    # 显示，背景色
    window.fill((100, 100, 100))

    # pygame处理
    for event in pg.event.get():
        if event.type == pg.QUIT:
            end_flag = True # 当窗口被关闭时，end_flag为真，退出循环

    # 检测按键，并遥控小车，并更新显示
    key_status = pg.key.get_pressed()
    window_block_position = [160 - 25, 120 - 25]
    v_line = 0.0
    v_rotate = 0.0
    if key_status[pg.K_UP] == 1:
        window_block_position[1] -= 100
        v_line += SET_LINE_SPEED
    if key_status[pg.K_DOWN] == 1:
        window_block_position[1] += 100
        v_line -= SET_LINE_SPEED
    if key_status[pg.K_LEFT] == 1:
        window_block_position[0] -= 100
        v_rotate -= SET_ROTATE_SPEED
    if key_status[pg.K_RIGHT] == 1:
        window_block_position[0] += 100
        v_rotate += SET_ROTATE_SPEED

    # 显示矩形
    pg.draw.rect(window, (255, 0, 0), (window_block_position[0], window_block_position[1], 50, 50))
    # 发送速度
    vrep.simxSetJointTargetVelocity(dr20ID, handle_left_wheel[1], v_line + v_rotate, vrep.simx_opmode_streaming)
    vrep.simxSetJointTargetVelocity(dr20ID, handle_right_wheel[1], v_line - v_rotate, vrep.simx_opmode_streaming)

    pg.display.update()

5. 窗口关闭后退出循环，退出pygame，断开与Vrep的连接

pg.quit()
vrep.simxFinish(dr20ID)
print("python exit")