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

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

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

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

2、修改脚本中的LUA代码

双击脚本,在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”滤波器的呢?这是在传感器的属性中查看的

  • 11
    点赞
  • 46
    收藏
  • 打赏
    打赏
  • 4
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页
评论 4

打赏作者

培培哥

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值