V-rep仿真：Python获取激光雷达数据

原創 培培哥 2019-03-12 00:38

        这几天打算使用Vrep机器人仿真平台做实验，但却在获取激光雷达测量数据的过程中遇到了困难：机器人所使用的雷达由两个vision sensor组成，但Vrep的remoteAPI没有接口可以直接获取传感器的深度数据，更没有接口直接得到雷达的测量数据。琢磨了几天，终于解决了这个困难，以下是我使用python获取Vrep场景内激光雷达测量数据的操作步骤。

一、在Scene中添加一个激光雷达

打开Model browser -> components -> sensor，从中选择一个激光雷达（以Hokuyo URG 04LX UG01_Fast.ttm为例）。

2、修改脚本

双击脚本，在function sysCall_init() 的最后添加代码以开启remoteAPI端口：

simRemoteApi.start(19999)

将function sysCall_sensing() 增加一些代码（新增的代码后标记了“added”，整份代码的解释放在文章最后）：

function sysCall_sensing() 
    measuredData={}
    ranges = {} ---------------------------------------------------added
    if notFirstHere then
        -- We skip the very first reading
        sim.addDrawingObjectItem(lines,nil)
        showLines=sim.getScriptSimulationParameter(sim.handle_self,'showLaserSegments')
        r,t1,u1=sim.readVisionSensor(visionSensor1Handle)
        r,t2,u2=sim.readVisionSensor(visionSensor2Handle)

        m1=sim.getObjectMatrix(visionSensor1Handle,-1)
        m01=simGetInvertedMatrix(sim.getObjectMatrix(sensorRef,-1))
        m01=sim.multiplyMatrices(m01,m1)
        m2=sim.getObjectMatrix(visionSensor2Handle,-1)
        m02=simGetInvertedMatrix(sim.getObjectMatrix(sensorRef,-1))
        m02=sim.multiplyMatrices(m02,m2)
        if u1 then
            p={0,0,0}
            p=sim.multiplyVector(m1,p)
            t={p[1],p[2],p[3],0,0,0}
            for j=0,u1[2]-1,1 do
                for i=0,u1[1]-1,1 do
                    w=2+4*(j*u1[1]+i)
                    v1=u1[w+1]
                    v2=u1[w+2]
                    v3=u1[w+3]
                    v4=u1[w+4]
                    if (v4<maxScanDistance_) then
                        p={v1,v2,v3}
                        p=sim.multiplyVector(m01,p)
                        table.insert(measuredData,p[1])
                        table.insert(measuredData,p[2])
                        table.insert(measuredData,p[3])
                        table.insert(ranges, v4) ------------------added
                    else ------------------------------------------added
                        table.insert(ranges, 0) -------------------added

                    end
                    if showLines then
                        p={v1,v2,v3}
                        p=sim.multiplyVector(m1,p)
                        t[4]=p[1]
                        t[5]=p[2]
                        t[6]=p[3]
                        sim.addDrawingObjectItem(lines,t)
                    end
                end
            end
        end
        if u2 then
            p={0,0,0}
            p=sim.multiplyVector(m2,p)
            t={p[1],p[2],p[3],0,0,0}
            for j=0,u2[2]-1,1 do
                for i=0,u2[1]-1,1 do
                    w=2+4*(j*u2[1]+i)
                    v1=u2[w+1]
                    v2=u2[w+2]
                    v3=u2[w+3]
                    v4=u2[w+4]
                    if (v4<maxScanDistance_) then
                        p={v1,v2,v3}
                        p=sim.multiplyVector(m02,p)
                        table.insert(measuredData,p[1])
                        table.insert(measuredData,p[2])
                        table.insert(measuredData,p[3])
                        table.insert(ranges, v4) ------------------added
                    else ------------------------------------------added
                        table.insert(ranges, 0) -------------------added
                    end
                    if showLines then
                        p={v1,v2,v3}
                        p=sim.multiplyVector(m2,p)
                        t[4]=p[1]
                        t[5]=p[2]
                        t[6]=p[3]
                        sim.addDrawingObjectItem(lines,t)
                    end
                end
            end
        end
        ranges = sim.packFloatTable(ranges) -----------------------added
        sim.setStringSignal('scan ranges', ranges) ----------------added
    end
    notFirstHere=true
end

3、Python remoteAPI获取雷达数据

代码如下：

import vrep
import sys
import time
import matplotlib.pyplot as plt

#断开以前的连接
vrep.simxFinish(-1)
#连接服务器
clientID = vrep.simxStart('V-rep的IP', 19999, True, True, 5000, 5)

if clientID != -1:
    print('Server is connected!')
else:
    print('Server is unreachable!')
    sys.exit(0)

#第1次获取数据，数据无效
errorCode, ranges = vrep.simxGetStringSignal(clientID, 'scan ranges', vrep.simx_opmode_streaming)
time.sleep(0.1)

#获取有效数据
errorCode, ranges = vrep.simxGetStringSignal(clientID, 'scan ranges', vrep.simx_opmode_buffer)
#转换将string为float列表，列表中的值即为雷达的测量值
ranges = vrep.simxUnpackFloats(ranges)
plt.xlim(0, 684)
plt.ylim(0, 5)
x = range(len(ranges))
y = ranges
#绘制结果
plt.scatter(x, y)

4、测试

结果如下：

 

附上在scprit关键部分代码解释（提示：“--”是Lua代码的注释符号）

------------------------------------sysCall_init函数-----------------------------------
visionSensor1Handle=sim.getObjectHandle("fastHokuyo_sensor1")   -- 获取第1个Vision sensor的Handle
visionSensor2Handle=sim.getObjectHandle("fastHokuyo_sensor2")   -- 获取第2个Vision sensor的Handle
-- ……
sensorRef=sim.getObjectHandle("fastHokuyo_ref")                 -- 获取Dummy的的Handle
-- ……


---------------------------------sysCall_sensing() 函数---------------------------------
measuredData={}

r,t1,u1=sim.readVisionSensor(visionSensor1Handle)   -- 获取第1个Vision Sensor的状态
r,t2,u2=sim.readVisionSensor(visionSensor2Handle)   -- 获取第2个Vision Sensor的状态

-- Manual给出的readVisionSensor返回值解释：
-- 1、result: detection state (0 or 1), or -1 in case of an error
-- 2、auxiliaryValuePacket1: default auxiliary value packet (same as for the C-function)
-- 3、auxiliaryValuePacket2: additional auxiliary value packet (e.g. from a filter component)
-- 4、auxiliaryValuePacket3: etc. (the function returns as many tables as there are auxiliary value packets)

-- hokuyo的雷达属性中添加了滤波器“Extract coordinates from working image”，由此可得：
-- r表示result
-- t1表示第1个Vision Sensor的auxiliaryValuePacket1
-- u1表示第1个Vision Sensor的“Extract coordinates from working image”滤波器的返回值

-- “Extract coordinates from working image”滤波器的返回值格式如下：
-- u1[1]图像在x方向上的点数、u1[2]图像在y方向上点数
-- u1[3]point1的x值、u1[4]point1的y值、u1[5]point1的z值、u1[6]point1的距离
-- u1[7]point2的x值、u1[8]point2的y值、u1[9]point2的z值、u1[10]point2的距离
-- u1[11]point3的x值、u1[12]point3的y值、u1[13]point3的z值、u1[14]point3的距离
-- ……
-- 注：这些点的座标是相对于Vision senor座标的


m1=sim.getObjectMatrix(visionSensor1Handle,-1)      -- 获取第1个Vision sensor在世变换矩阵变换矩阵
m01=simGetInvertedMatrix(sim.getObjectMatrix(sensorRef,-1)) -- 获取第Hokuyo在世界的座标变换矩阵的逆矩阵
m01=sim.multiplyMatrices(m01,m1)
m2=sim.getObjectMatrix(visionSensor2Handle,-1)      -- 获取第2个Vision sensor在世变换矩阵变换矩阵
m02=simGetInvertedMatrix(sim.getObjectMatrix(sensorRef,-1))
m02=sim.multiplyMatrices(m02,m2)


if u1 then
    p={0,0,0}
    p=sim.multiplyVector(m1,p)
    t={p[1],p[2],p[3],0,0,0}
    for j=0,u1[2]-1,1 do        -- u1[2]表示图像在y方向上的点数
        for i=0,u1[1]-1,1 do    -- u1[1]表示图像在x方向上的点数
            w=2+4*(j*u1[1]+i)   -- 获取测量值的起始所引值（一个测量点有4个值，分别为x、y、z和距离）
            v1=u1[w+1]          -- 测量点的x轴座标值
            v2=u1[w+2]          -- 测量点的y轴座标值
            v3=u1[w+3]          -- 测量点的z轴座标值
            v4=u1[w+4]          -- 测量点与传感器距离值
            if (v4<maxScanDistance_) then
                p={v1,v2,v3}                        -- p为基于Vision sensor的测量点的座标
                p=sim.multiplyVector(m01,p)         -- 对测量点进行座标变换，得到新的测量点座标（基于世界座标系）
                table.insert(measuredData,p[1])     -- 新x添加到measuredData
                table.insert(measuredData,p[2])     -- 新y添加到measuredData
                table.insert(measuredData,p[3])     -- 新z添加到measuredData
                table.insert(ranges,v4)             -- 距离值添加到ranges
            else 
                table.insert(ranges,0)              -- 大于最大距离，距离值设为0添加到ranges
            end
            if showLines then
               -- 这段代码与场景中绘制雷达测量线条有关，在此不解释
            end
        end
    end
end

if u2 then
    -- 代码：与u1同理，不再列出
end

ranges = sim.packFloatTable(ranges)         -- 将float型的数组打包成一个String
sim.setStringSignal('scan ranges', ranges)  -- 将ranges设置到一个信号中，让远程API可以取得数据

如何知道hokuyo的雷达属性中添加了“Extract coordinates from working image”滤波器的呢？这是在传感器的属性中查看的

發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

Vrep的安装与小车建模

文章目錄準備工作任務1：智能小車平臺建模實現搭建四輪小車1. 添加小車 carRob2. 添加接近傳感器 proximity sensor3. 添加車輪 wheel4. 添加關節 revolute5. 使 carRob 具有穩定性

qiao_zhang 2020-06-14 04:49:23

V-REP使用笔记（1） —— 力传感器的配置和使用

文章目錄1.Force Sensors的添加2. 傳感器 介紹3. 傳感器屬性設置4. API使用及數據可視化5. 應用場景 1.Force Sensors的添加 Add→Force sensor 然後拖到響應的模型樹位置，調整位

罗伯特祥 2020-06-14 02:18:52

VREP Remote API工作模式详解（未写完，完成度90%）

千千Sama 2020-06-03 09:13:35

vrep结合centerOfMassVisualization tool得到机器人质心座标

犹有傲霜枝 2020-04-14 19:05:18

VREP-Python机械臂串联结构逆向运动学解法（Cyclic Coordinate Descent Method）

kiomi_kiomi 2019-09-05 23:53:02

建立不同关节以及各种组合(gazebo转vrep)

kiomi_kiomi 2019-08-28 15:22:49

闭环关节设置以及仿真引擎选择

kiomi_kiomi 2019-08-28 15:22:49

Vrep系列教程（二）——BubbleRob

千千Sama 2019-08-23 02:13:36

Ubuntu安装vrep遇到问题（libQt5Score.so.5 file too short）

bella_2018 2019-04-13 23:19:23

vrep遇到的问题杂记

千千Sama 2019-02-02 00:57:17

VREP Remote API工作模式详解（未写完，完成度60%）

千千Sama 2018-12-24 01:09:49

Ubuntu16.04安装V-Rep并配置ros

菜鸟粮举非要做开发 2018-12-07 15:53:28

vrep的bubbeRob仿真机器人制作教程

浅夏忆 2018-10-13 10:50:09

V-rep:simCreateIkGroup

小鱼儿哥哥 2018-09-05 03:47:57

v-rep Vision sensor 内参

梦之泪殇 2018-09-04 15:39:53