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