V-rep中顯示激光掃描點(diǎn)

在VREP自帶的場景中找到practicalPathPlanningDemo.ttt文件，刪除場景中多余的物體只保留靜態(tài)的地圖。然后在Model browser→components→sensors中找到SICK TiM310 Fast激光雷達(dá)，拖入場景中：

打開腳本參數(shù)修改器，可以修改雷達(dá)掃描范圍(默認(rèn)為270°)，是否顯示雷達(dá)掃描線(true)，以及最大探測距離(默認(rèn)為4m)這三個(gè)參數(shù)。地圖大小為5m×5m，我們將雷達(dá)最大探測距離改為2m

將激光雷達(dá)放到地圖中任意位置，點(diǎn)擊仿真按鈕可以看到掃描光線(如果電腦比較卡可以將showLaserSegments這個(gè)參數(shù)設(shè)為false，就不會(huì)顯示掃描線)如下圖所示：

SICK_TiM310激光雷達(dá)在V-rep中是由兩個(gè)視角為135°的視覺傳感器模擬的，這兩個(gè)視覺傳感器可以探測深度信息：

雙擊視覺傳感器圖標(biāo)，修改Filter中Coordinate Extraction的參數(shù)與傳感器X/Y方向分辨率一致。X方向默認(rèn)值為135，即會(huì)返回135個(gè)數(shù)據(jù)點(diǎn)，這里要改為256。

我們可以在V-rep中繪制出激光掃描圖：在場景中添加一個(gè)Graph，將其設(shè)為顯示處理(Explicit handling)，然后添加用戶自定義數(shù)據(jù)x和y：

然后點(diǎn)擊Edit XY graphs按鈕，在彈出的對話框中添加一個(gè)新的曲線。X-value選擇我們之前自定義的數(shù)據(jù)x，Y-value選擇自定義的數(shù)據(jù)y，并去掉Link points選項(xiàng)：

將SICK_TiM310_fast的lua腳本代碼修改如下：

if (sim_call_type==sim_childscriptcall_initialization) thenvisionSensor1Handle=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 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 end

if 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) thensimRemoveDrawingObject(lines)

simResetGraph(graphHandle)end

if (sim_call_type==sim_childscriptcall_sensing) thenmeasuredData={}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 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 do

for 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 (v4

p=simMultiplyVector(m01,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2])table.insert(measuredData,p[3])end

if showLines thenp={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 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 do

for 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 (v4

p=simMultiplyVector(m02,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2])table.insert(measuredData,p[3])end

if showLines thenp={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

endnotFirstHere=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 dosimSetGraphUserData(graphHandle,'x',measuredData[3*(i-1)+1])

simSetGraphUserData(graphHandle,'y',measuredData[3*(i-1)+2])

simHandleGraph(graphHandle,0)end

end

點(diǎn)擊仿真按鈕，可以在X/Y graph窗口中看到激光掃描結(jié)果如下：

V-rep中的視覺傳感器可以探測到障礙物的坐標(biāo)以及與其距離，上面的X-Y圖就是直接采用坐標(biāo)點(diǎn)畫出的。然而一般激光雷達(dá)只能探測障礙物距離，不能直接獲取其坐標(biāo)，我們可以將距離畫成與角度對應(yīng)的極坐標(biāo)圖。將距離數(shù)據(jù)保存為CSV文件，用Mathematica讀入并畫出極坐標(biāo)圖：

ranges = Flatten[Import["C:\\Users\\Administrator\\Desktop\\distance.csv"]];

ListPolarPlot[ranges, DataRange -> {-135 Degree, 135 Degree}]

發(fā)布LaserScan消息

下面的代碼將激光雷達(dá)掃描數(shù)據(jù)按照LaserScan的消息格式發(fā)布出去：

if (sim_call_type==sim_childscriptcall_initialization) thenvisionSensor1Handle=simGetObjectHandle("SICK_TiM310_sensor1")

visionSensor2Handle=simGetObjectHandle("SICK_TiM310_sensor2")

joint1Handle=simGetObjectHandle("SICK_TiM310_joint1")

joint2Handle=simGetObjectHandle("SICK_TiM310_joint2")

sensorRefHandle=simGetObjectHandle("SICK_TiM310_ref")

maxScanDistance=simGetScriptSimulationParameter(sim_handle_self,'maxScanDistance')if maxScanDistance>1000 then maxScanDistance=1000 end

if 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 end

if 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

--Enable an LaserScan publisher:

pub = simExtRosInterface_advertise('/scan', 'sensor_msgs/LaserScan')--After calling this function, this publisher will treat uint8 arrays as string. Using strings should be in general much faster that using int arrays in Lua.

simExtRosInterface_publisherTreatUInt8ArrayAsString(pub) --treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)

angle_min= -135 * (math.pi/180); --angle correspond to FIRST beam in scan ( in rad)

angle_max= 135 * (math.pi/180) --angle correspond to LAST beam in scan ( in rad)

angle_increment = 270*(math.pi/180)/512 --Angular resolution i.e angle between 2 beams

--sensor scans every 50ms with 512 beams. Each beam is measured in (50 ms/ 512 )

time_increment = (1 / 20) / 512range_min= 0.05range_max= maxScanDistance --scan can measure upto this range

end

if (sim_call_type==sim_childscriptcall_cleanup) thensimRemoveDrawingObject(lines)

simExtRosInterface_shutdownPublisher(pub)end

if (sim_call_type==sim_childscriptcall_sensing) thenmeasuredData={}

distanceData={}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 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 do

for 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]table.insert(distanceData,v4)if (v4

p=simMultiplyVector(m01,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2])table.insert(measuredData,p[3])end

if showLines thenp={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 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 do

for 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]table.insert(distanceData,v4)if (v4

p=simMultiplyVector(m02,p)table.insert(measuredData,p[1])table.insert(measuredData,p[2])table.insert(measuredData,p[3])end

if showLines thenp={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

endnotFirstHere=true

--populate the LaserScan message

scan={}

scan['header']={seq=0,stamp=simExtRosInterface_getTime(), frame_id="SICK_TiM310_ref"}

scan['angle_min']=angle_min

scan['angle_max']=angle_max

scan['angle_increment']=angle_increment

scan['time_increment']=time_increment

scan['scan_time']=simExtRosInterface_getTime() --Return the current ROS time i.e. the time returned by ros::Time::now()

scan['range_min']=range_min

scan['range_max']=range_max

scan['ranges'] =distanceData

scan['intensities']={}

simExtRosInterface_publish(pub, scan)end

注意代碼中發(fā)布的距離是相對于視覺傳感器坐標(biāo)系的，因?yàn)槟Ｐ椭幸曈X傳感器坐標(biāo)系與激光雷達(dá)坐標(biāo)系(SICK_TiM310_ref)在X、Y方向的位置是一致的，而Z坐標(biāo)只存在一點(diǎn)高度差異，并不會(huì)影響X-Y平面內(nèi)障礙物相對于SICK_TiM310_ref參考坐標(biāo)系的位置坐標(biāo)。如果這兩個(gè)坐標(biāo)系在X、Y方向存在偏差，就需要將采集到的數(shù)據(jù)點(diǎn)轉(zhuǎn)換到SICK_TiM310_ref坐標(biāo)系中。

另外代碼中變量v4為激光雷達(dá)探測到的距物體的距離，如果在最大掃描范圍內(nèi)沒有探測到物體，則會(huì)返回最大值。由于這個(gè)距離與掃描角度是一一對應(yīng)的，因此要注意table.insert函數(shù)的使用，不能放在下一句的if語句之中，否則在超過最大掃描范圍的地方不會(huì)向列表內(nèi)插入距離數(shù)據(jù)，這樣會(huì)造成距離與角度不匹配，可能導(dǎo)致激光圖像出現(xiàn)歪斜。

點(diǎn)擊仿真按鈕，程序運(yùn)行沒問題后在rviz中可以添加LaserScan進(jìn)行查看：

輸入rostopic hz /scan可以查看消息發(fā)布的頻率：

這里有一個(gè)小問題，從上圖可以看出激光雷達(dá)信息發(fā)布的頻率約為43Hz，但是V-rep仿真的時(shí)間步���為50ms，消息發(fā)布的頻率應(yīng)該為20Hz。這是因?yàn)閂-rep中默認(rèn)情況下仿真并不是以實(shí)際時(shí)間在運(yùn)行，在工具欄上點(diǎn)擊real-time mode按鈕，開始實(shí)時(shí)模式：

現(xiàn)在再查看消息發(fā)布的頻率，可以看到頻率和我們設(shè)定的一樣了：

另外，通過rostopic echo /scan命令可以查看消息的具體內(nèi)容(方便我們檢查出可能存在的錯(cuò)誤：我在虛擬機(jī)下運(yùn)行得到的數(shù)據(jù)很奇怪，但是換到實(shí)體系統(tǒng)上就沒有問題)：

發(fā)布nav_msgs/Odometry里程計(jì)信息及tf變換

在V-rep中進(jìn)行地圖構(gòu)建仿真時(shí)可以用鍵盤控制機(jī)器人的位置(這里直接簡化為控制激光雷達(dá))，那么機(jī)器人相對于初始時(shí)刻odom坐標(biāo)系的位置和姿態(tài)等信息可以通過航跡推算(使用里程計(jì)或慣性傳感器根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型計(jì)算)獲得。然后需要將其按照nav_msgs/Odometry消息的格式包裝好，發(fā)布到/odom話題上；并且還要發(fā)布機(jī)器人坐標(biāo)系base_link相對于odom坐標(biāo)系的tf變換。The nav_msgs/Odometry?message stores an estimate of the position and velocity of a robot in free space.The "tf" software library is responsible for managing the relationships between coordinate frames relevant to the robot in a transform tree. Therefore, any odometry source must publish information about the coordinate frame that it manages.

V-rep腳本中發(fā)布tf變換主要用下面這兩個(gè)函數(shù)，區(qū)別在于simExtRosInterface_sendTransform調(diào)用一次只能發(fā)送一對變換，而simExtRosInterface_sendTransforms則可以一次發(fā)送多對變換，函數(shù)參數(shù)是變換的列表：

根據(jù)V-rep中物體的句柄和名稱發(fā)布坐標(biāo)系變換的代碼如下：

functiongetTransformStamped(objHandle, name, relTo, relToName)--This function retrieves the stamped transform for a specific object

t = simExtRosInterface_getTime()

p= simGetObjectPosition(objHandle, relTo)

o= simGetObjectQuaternion(objHandle, relTo)return{

header= {stamp=t, frame_id=relToName},

child_frame_id= name,

transform= {

translation={x=p[1],y=p[2],z=p[3]},

rotation={x=o[1],y=o[2],z=o[3],w=o[4]}

}

}end----------------------------------------------------------------------------------------------------------------------

simExtRosInterface_sendTransforms({getTransformStamped(sensorRefHandle,'SICK_TiM310_ref',baseLinkHandle,'base_link'),

getTransformStamped(baseLinkHandle,'base_link',odomHandle,'odom')})--simExtRosInterface_sendTransform(getTransformStamped(sensorRefHandle,'SICK_TiM310_ref',baseLinkHandle,'base_link')

--simExtRosInterface_sendTransform(getTransformStamped(baseLinkHandle,'base_link',odomHandle,'odom'))

我們在V-rep的腳本程序中向ros系統(tǒng)發(fā)布了坐標(biāo)系之間的變換，有時(shí)可能會(huì)出現(xiàn)許多錯(cuò)誤。為了方便排查錯(cuò)誤，ros提供了一系列tf調(diào)試工具。下面兩種命令都可以以圖形化的方式查看坐標(biāo)系之間的tf關(guān)系：

$ rosrun tf view_frames

$rosrun rqt_tf_tree rqt_tf_tree

打開生成的pdf文件或在彈出的rqt窗口中，可以很清楚的看出里程計(jì)坐標(biāo)系odom，機(jī)器人坐標(biāo)系base_link，以及激光雷達(dá)坐標(biāo)系SICK_TiM310_ref之間的關(guān)系：

tf_echo命令可以用于查看兩個(gè)坐標(biāo)系之間具體的變換關(guān)系(注意輸出的是target_frame相對于reference_frame的關(guān)系)：

$ rosrun tf tf_echo reference_frame target_frame

如下圖所示，會(huì)輸出激光傳感器坐標(biāo)系SICK_TiM310_ref相對于機(jī)器人坐標(biāo)系base_link的變換(V-rep模型中這兩個(gè)坐標(biāo)系是重合的)：

在/odom話題上發(fā)布nav_msgs/Odometry消息的代碼如下(注意這里直接調(diào)用函數(shù)獲取到相對于odom的位置和姿態(tài)，省去了航跡推算的過程。如果在真實(shí)的小車上進(jìn)行測試，就需要根據(jù)里程計(jì)數(shù)據(jù)來推算小車的位置和姿態(tài)等信息，然后再發(fā)送出去)：

odomPub = simExtRosInterface_advertise('/odom', 'nav_msgs/Odometry')local pos =simGetObjectPosition(baseLinkHandle, odomHandle)local ori =simGetObjectQuaternion(baseLinkHandle, odomHandle)

odom={}

odom.header= {seq=0,stamp=simExtRosInterface_getTime(), frame_id="odom"}

odom.child_frame_id= 'base_link'odom.pose= { pose={position={x=pos[1],y=pos[2],z=pos[3]}, orientation={x=ori[1],y=ori[2],z=ori[3],w=ori[4]} } }

simExtRosInterface_publish(odomPub, odom)

使用gmapping構(gòu)建地圖

gmaping包是用來生成地圖的，它需要從ROS系統(tǒng)監(jiān)聽多個(gè)Topic，并輸出map。The slam_gmapping node takes in?sensor_msgs/LaserScan?messages and builds a map (nav_msgs/OccupancyGrid)

Subscribed Topics:

Transforms necessary to relate frames for laser, base, and odometry

Laser scans to create the map from

Required tf Transforms:

?→?base_link：usually a fixed value, broadcast periodically by a?robot_state_publisher, or a?tf?static_transform_publisher.

base_link?→?odom：usually provided by the odometry system (e.g., the driver for the mobile base)

Provided tf Transforms:

map?→?odom：the current estimate of the robot's pose within the map frame

使用記錄下的tf以及l(fā)aser scan data構(gòu)建地圖的步驟如下：

1. 鍵盤或手柄控制機(jī)器人在空間中運(yùn)動(dòng)時(shí)，使用rosbag記錄激光及tf數(shù)據(jù)包，記錄完成后按Ctrl+C鍵結(jié)束。

$ rosbag record -O my_scan_data /scan /tf

2. 設(shè)置參數(shù)，確保在任何節(jié)點(diǎn)使用前use_sim_time參數(shù)為true。我們重播一個(gè)記錄歷史文件時(shí)，里面記錄的是歷史時(shí)間，所以我們需要告訴ROS從現(xiàn)在起開始啟用模擬時(shí)間。This basically tells nodes on startup to use simulated time (ticked here by rosbag) instead of wall-clock time (as in a live system). It avoids confusing time-dependent components like?tf, which otherwise would wonder why messages are arriving with timestamps far in the past.?關(guān)于時(shí)鐘問題可以參考

Normally, the ROS?client libraries?will use your computer's system clock as a time source, also known as the "wall-clock" or "wall-time" (like the clock on the wall of your lab). When you are running a simulation or playing back logged data, however, it is often desirable to instead have the system use a simulated clock so that you can have accelerated, slowed, or stepped control over your system's perceived time. For example, if you are playing back sensor data into your system, you may wish to have your time correspond to the timestamps of the sensor data.

$ rosparam set use_sim_time true

下圖是use_sim_time參數(shù)為false時(shí)的情況：

設(shè)置use_sim_time為true，rosbag回放開始后ROS Time與Bag Time一致：

3.?運(yùn)行slam_gmapping節(jié)點(diǎn)，它將在scan主題上監(jiān)聽激光掃描數(shù)據(jù)并創(chuàng)建地圖(可以在命令行中設(shè)置建圖參數(shù)：比如地圖分辨率、粒子數(shù)目、迭代次數(shù)、地圖更新間隔等參數(shù))

$ rosrun gmapping slam_gmapping scan:=scan _xmin:=-2.5 _xmax:=2.5 _ymin:=-2.5 _ymax:=2.5 ...

比較重要的幾個(gè)參數(shù)有：

particles (int, default: 30) gmapping算法中的粒子數(shù)，因?yàn)間mapping使用的是粒子濾波算法，粒子在不斷地迭代更新，所以選取一個(gè)合適的粒子數(shù)可以讓算法在保證比較準(zhǔn)確的同時(shí)有較高的速度。

minimumScore (float, default: 0.0) 最小匹配得分，這個(gè)參數(shù)很重要，它決定了對激光的一個(gè)置信度，越高說明對激光匹配算法的要求越高，激光的匹配也越容易失敗而轉(zhuǎn)去使用里程計(jì)數(shù)據(jù)，而設(shè)的太低又會(huì)使地圖中出現(xiàn)大量噪聲，所以需要權(quán)衡調(diào)整。在V-rep仿真中里程計(jì)數(shù)據(jù)是直接通過函數(shù)獲取的，沒有誤差，因此可以將這個(gè)值調(diào)高一點(diǎn)，讓地圖的匹配更多依賴?yán)锍逃?jì)數(shù)據(jù)。

lskip(int, default: 0)的值如果為0，則所有的激光數(shù)據(jù)幀都會(huì)用來進(jìn)行scan matching，如果lskip的值大于0則會(huì)跳過幾幀來進(jìn)行scan matching。有時(shí)激光數(shù)據(jù)的噪聲會(huì)比較大，對所有數(shù)據(jù)幀進(jìn)行匹配的效果可能會(huì)不好，這時(shí)可以加大lskip的值。

...

slam_gmapping節(jié)點(diǎn)用到的參數(shù)相當(dāng)多，有很多參數(shù)需要在實(shí)際中測試多次來確定其值。如果參數(shù)太多在命令行中輸入會(huì)不太方便，可以寫成launch文件來運(yùn)行：

4.?在新終端中啟動(dòng)bag包回放，將數(shù)據(jù)提供給slam_gmapping節(jié)點(diǎn)

$ rosbag play my_scan_data.bag

在數(shù)據(jù)回放過程中也可以打開rviz進(jìn)行查看：啟動(dòng)rviz，在左下方點(diǎn)擊add按鈕，然后選擇map，創(chuàng)建一副空地圖；接著制定rviz的topic為/map可以監(jiān)聽到地圖數(shù)據(jù)。下圖是rosbag回放過程中建圖的動(dòng)態(tài)過程：

5. 使用map_server生成地圖

$ rosrun map_server map_saver -f my_map

使用map_saver命令后會(huì)生成兩個(gè)文件。my_map.pgm是地圖的PGM格式的圖片，PGM格式是便攜式灰度圖像格式(portable graymap file format)。my_map.yaml文件描述地圖元數(shù)據(jù)。

my_map.yaml文件內(nèi)容如下：

image: my_map.pgm

resolution: 0.050000

origin: [-12.200000, -12.200000, 0.000000]

negate: 0

occupied_thresh: 0.65

free_thresh: 0.196

image：圖像文件的路徑；可以是絕對的，或相對于YAML文件的位置

resolution：地圖的分辨率，米/像素

origin：地圖中左下角像素的位置和姿態(tài)(x，y，yaw)，偏航為逆時(shí)針旋轉(zhuǎn)(yaw = 0表示無旋轉(zhuǎn))

occupancy_thresh：概率大于該閾值的像素被認(rèn)為完全占用

free_thresh：概率小于該閾值的像素被認(rèn)為是完全自由的

negate：“白/黑”對應(yīng)“自由/占用”語義是否應(yīng)該被反轉(zhuǎn)

最終生成的地圖如下圖所示，圖中越亮/白的像素表示沒有障礙物(free)的概率越大，越暗/黑的像素表示被障礙物占據(jù)(occupied)的概率越大，灰色表示狀態(tài)未知。

用屏幕測量工具測量圖片上的像素間的距離，再乘以分辨率可以得到實(shí)際尺寸。例如，左下角點(diǎn)和右下角點(diǎn)的像素間距測量結(jié)果為102(可能點(diǎn)取的不精確)，對應(yīng)的實(shí)際距離為5.1m，這與真實(shí)地圖大小一致。

6. 在建圖結(jié)束后不要忘記重置use_sim_time參數(shù)

$ rosparam set use_sim_time false

總結(jié)

以上是生活随笔為你收集整理的linux生成地图,ROS中利用V-rep进行地图构建仿真的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。