ROS中利用V-rep进行地图构建仿真

 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 endif maxScanDistance<0.1 then maxScanDistance=0.1 endsimSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_far_clipping,maxScanDistance)simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_far_clipping,maxScanDistance)maxScanDistance_=maxScanDistance*0.9999scanningAngle=simGetScriptSimulationParameter(sim_handle_self,'scanAngle')if scanningAngle>270 then scanningAngle=270 endif scanningAngle<2 then scanningAngle=2 endscanningAngle=scanningAngle*math.pi/180simSetObjectFloatParameter(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) thensimDisplayDialog("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 readingsimAddDrawingObjectItem(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 thenp={0,0,0}p=simMultiplyVector(m1,p)t={p[1],p[2],p[3],0,0,0}for j=0,u1[2]-1,1 dofor i=0,u1[1]-1,1 dow=2+4*(j*u1[1]+i)v1=u1[w+1]v2=u1[w+2]v3=u1[w+3]v4=u1[w+4]if (v4thenp={v1,v2,v3}p=simMultiplyVector(m01,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2]) table.insert(measuredData,p[3])endif showLines thenp={v1,v2,v3}p=simMultiplyVector(m1,p)t[4]=p[1]t[5]=p[2]t[6]=p[3]simAddDrawingObjectItem(lines,t)endendendendif u2 thenp={0,0,0}p=simMultiplyVector(m2,p)t={p[1],p[2],p[3],0,0,0}for j=0,u2[2]-1,1 dofor i=0,u2[1]-1,1 dow=2+4*(j*u2[1]+i)v1=u2[w+1]v2=u2[w+2]v3=u2[w+3]v4=u2[w+4]if (v4thenp={v1,v2,v3}p=simMultiplyVector(m02,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2])

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！