V-rep中显示激光扫描点
在VREP自带的场景中找到practicalPathPlanningDemo.ttt文件，删除场景中多余的物体只保留静态的地图。然后在Model browser→components→sensors中找到SICK TiM310 Fast激光雷达，拖入场景中：
打开脚本参数修改器，可以修改雷达扫描范围（默认为270°），是否显示雷达扫描线（true），以及最大探测距离（默认为4m）这三个参数。地图大小为5m×5m，我们将雷达最大探测距离改为2m
将激光雷达放到地图中任意位置，点击仿真按钮可以看到扫描光线（如果电脑比较卡可以将showLaserSegments这个参数设为false，就不会显示扫描线）如下图所示：
SICK_TiM310激光雷达在V-rep中是由两个视角为135°的视觉传感器模拟的，这两个视觉传感器可以探测深度信息：
双击视觉传感器图标，修改Filter中Coordinate Extraction的参数与传感器X/Y方向分辨率一致。X方向默认值为135，即会返回135个数据点，这里要改为256。
我们可以在V-rep中绘制出激光扫描图：在场景中添加一个Graph，将其设为显示处理（Explicit handling），然后添加用户自定义数据x和y：
然后点击Edit XY graphs按钮，在弹出的对话框中添加一个新的曲线。X-value选择我们之前自定义的数据x，Y-value选择自定义的数据y，并去掉Link points选项：
将SICK_TiM310_fast的lua脚本代码修改如下：
if (sim_call_type==sim_childscriptcall_initialization) then visionSensor1Handle=simGetObjectHandle("SICK_TiM310_sensor1") visionSensor2Handle=simGetObjectHandle("SICK_TiM310_sensor2") joint1Handle=simGetObjectHandle("SICK_TiM310_joint1") joint2Handle=simGetObjectHandle("SICK_TiM310_joint2") sensorRefHandle=simGetObjectHandle("SICK_TiM310_ref") graphHandle = simGetObjectHandle("Graph") maxScanDistance=simGetScriptSimulationParameter(sim_handle_self,'maxScanDistance') if maxScanDistance>1000 then maxScanDistance=1000 end if maxScanDistance<0.1 then maxScanDistance=0.1 end simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_far_clipping,maxScanDistance) simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_far_clipping,maxScanDistance) maxScanDistance_=maxScanDistance*0.9999 scanningAngle=simGetScriptSimulationParameter(sim_handle_self,'scanAngle') if scanningAngle>270 then scanningAngle=270 end if scanningAngle<2 then scanningAngle=2 end scanningAngle=scanningAngle*math.pi/180 simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2) simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2) simSetJointPosition(joint1Handle,-scanningAngle/4) simSetJointPosition(joint2Handle,scanningAngle/4) red={1,0,0} lines=simAddDrawingObject(sim_drawing_lines,1,0,-1,1000,nil,nil,nil,red) if (simGetInt32Parameter(sim_intparam_program_version)<30004) then simDisplayDialog("ERROR","This version of the SICK sensor is only supported from V-REP V3.0.4 and upwards.&&nMake sure to update your V-REP.",sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end end if (sim_call_type==sim_childscriptcall_cleanup) then simRemoveDrawingObject(lines) simResetGraph(graphHandle) end if (sim_call_type==sim_childscriptcall_sensing) then measuredData={} if notFirstHere then -- We skip the very first reading simAddDrawingObjectItem(lines,nil) showLines=simGetScriptSimulationParameter(sim_handle_self,'showLaserSegments') r,t1,u1=simReadVisionSensor(visionSensor1Handle) r,t2,u2=simReadVisionSensor(visionSensor2Handle) m1=simGetObjectMatrix(visionSensor1Handle,-1) m01=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1)) m01=simMultiplyMatrices(m01,m1) m2=simGetObjectMatrix(visionSensor2Handle,-1) m02=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1)) m02=simMultiplyMatrices(m02,m2) if u1 then p={0,0,0} p=simMultiplyVector(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=simMultiplyVector(m01,p) table.insert(measuredData,p[1]) table.insert(measuredData,p[2]) table.insert(measuredData,p[3]) end if showLines then p={v1,v2,v3} p=simMultiplyVector(m1,p) t[4]=p[1] t[5]=p[2] t[6]=p[3] simAddDrawingObjectItem(lines,t) end end end end if u2 then p={0,0,0} p=simMultiplyVector(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=simMultiplyVector(m02,p) table.insert(measuredData,p[1]) table.insert(measuredData,p[2]) table.insert(measuredData,p[3]) end if showLines then p={v1,v2,v3} p=simMultiplyVector(m2,p) t[4]=p[1] t[5]=p[2] t[6]=p[3] simAddDrawingObjectItem(lines,t) end end end end end notFirstHere=true --stringData = simPackFloatTable(measuredData) -- Packs a table of floating-point numbers into a string --simSetStringSignal("UserData", stringData) simResetGraph(graphHandle) for i=1,#measuredData/3,1 do simSetGraphUserData(graphHandle,'x',measuredData[3*(i-1)+1]) simSetGraphUserData(graphHandle,'y',measuredData[3*(i-1)+2]) simHandleGraph(graphHandle,0) end end
点击仿真按钮，可以在X/Y graph窗口中看到激光扫描结果如下：
