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窗口中看到激光掃描結果如下：
V-rep中的視覺傳感器可以探測到障礙物的坐標以及與其距離，上面的X-Y圖就是直接采用坐標點畫出的。然而一般激光雷達只能探測障礙物距離，不能直接獲取其坐標，我們可以將距離畫成與角度對應的極坐標圖。將距離數據保存為CSV文件，用Mathematica讀入並畫出極坐標圖：
ranges = Flatten[Import["C:\\Users\\Administrator\\Desktop\\distance.csv"]];
ListPolarPlot[ranges, DataRange -> {-135 Degree, 135 Degree}]
發布LaserScan消息
下面的代碼將激光雷達掃描數據按照LaserScan的消息格式發布出去：
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") 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 -- 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) / 512 range_min = 0.05 range_max = maxScanDistance -- scan can measure upto this range end if (sim_call_type==sim_childscriptcall_cleanup) then simRemoveDrawingObject(lines) simExtRosInterface_shutdownPublisher(pub) end if (sim_call_type==sim_childscriptcall_sensing) then measuredData={} 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 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] table.insert(distanceData,v4) 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] table.insert(distanceData,v4) 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 -- 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
注意代碼中發布的距離是相對於視覺傳感器坐標系的，因為模型中視覺傳感器坐標系與激光雷達坐標系（SICK_TiM310_ref）在X、Y方向的位置是一致的，而Z坐標只存在一點高度差異，並不會影響X-Y平面內障礙物相對於SICK_TiM310_ref參考坐標系的位置坐標。如果這兩個坐標系在X、Y方向存在偏差，就需要將采集到的數據點轉換到SICK_TiM310_ref坐標系中。
另外代碼中變量v4為激光雷達探測到的距物體的距離，如果在最大掃描范圍內沒有探測到物體，則會返回最大值。由於這個距離與掃描角度是一一對應的，因此要注意table.insert函數的使用，不能放在下一句的if語句之中，否則在超過最大掃描范圍的地方不會向列表內插入距離數據，這樣會造成距離與角度不匹配，可能導致激光圖像出現歪斜。
點擊仿真按鈕，程序運行沒問題后在rviz中可以添加LaserScan進行查看：
輸入rostopic hz /scan可以查看消息發布的頻率：
這里有一個小問題，從上圖可以看出激光雷達信息發布的頻率約為43Hz，但是V-rep仿真的時間步長為50ms，消息發布的頻率應該為20Hz。這是因為V-rep中默認情況下仿真並不是以實際時間在運行，在工具欄上點擊real-time mode按鈕，開始實時模式：
現在再查看消息發布的頻率，可以看到頻率和我們設定的一樣了：
另外，通過rostopic echo /scan命令可以查看消息的具體內容（方便我們檢查出可能存在的錯誤：我在虛擬機下運行得到的數據很奇怪，但是換到實體系統上就沒有問題）：
發布nav_msgs/Odometry里程計信息及tf變換
在V-rep中進行地圖構建仿真時可以用鍵盤控制機器人的位置（這里直接簡化為控制激光雷達），那么機器人相對於初始時刻odom坐標系的位置和姿態等信息可以通過航跡推算（使用里程計或慣性傳感器根據機器人運動學模型計算）獲得。然后需要將其按照nav_msgs/Odometry消息的格式包裝好，發布到/odom話題上；並且還要發布機器人坐標系base_link相對於odom坐標系的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腳本中發布tf變換主要用下面這兩個函數，區別在於simExtRosInterface_sendTransform調用一次只能發送一對變換，而simExtRosInterface_sendTransforms則可以一次發送多對變換，函數參數是變換的列表：
根據V-rep中物體的句柄和名稱發布坐標系變換的代碼如下：
function getTransformStamped(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系統發布了坐標系之間的變換，有時可能會出現許多錯誤。為了方便排查錯誤，ros提供了一系列tf調試工具。下面兩種命令都可以以圖形化的方式查看坐標系之間的tf關系：
$ rosrun tf view_frames
$ rosrun rqt_tf_tree rqt_tf_tree
打開生成的pdf文件或在彈出的rqt窗口中，可以很清楚的看出里程計坐標系odom，機器人坐標系base_link，以及激光雷達坐標系SICK_TiM310_ref之間的關系：
tf_echo命令可以用於查看兩個坐標系之間具體的變換關系（注意輸出的是target_frame相對於reference_frame的關系）：
$ rosrun tf tf_echo reference_frame target_frame
如下圖所示，會輸出激光傳感器坐標系SICK_TiM310_ref相對於機器人坐標系base_link的變換（V-rep模型中這兩個坐標系是重合的）：
在/odom話題上發布nav_msgs/Odometry消息的代碼如下（注意這里直接調用函數獲取到相對於odom的位置和姿態，省去了航跡推算的過程。如果在真實的小車上進行測試，就需要根據里程計數據來推算小車的位置和姿態等信息，然后再發送出去）：
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構建地圖
gmaping包是用來生成地圖的，它需要從ROS系統監聽多個Topic，並輸出map。The slam_gmapping node takes in sensor_msgs/LaserScan messages and builds a map (nav_msgs/OccupancyGrid)
- Subscribed Topics:
- tf (tf/tfMessage)：Transforms necessary to relate frames for laser, base, and odometry
- scan (sensor_msgs/LaserScan)：Laser scans to create the map from
- Required tf Transforms:
- <the frame attached to incoming scans> → 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以及laser scan data構建地圖的步驟如下：
1. 鍵盤或手柄控制機器人在空間中運動時，使用rosbag記錄激光及tf數據包，記錄完成后按Ctrl+C鍵結束。
$ rosbag record -O my_scan_data /scan /tf
2. 設置參數，確保在任何節點使用前use_sim_time參數為true。我們重播一個記錄歷史文件時，里面記錄的是歷史時間，所以我們需要告訴ROS從現在起開始啟用模擬時間。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. 關於時鍾問題可以參考http://wiki.ros.org/Clock.
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參數為false時的情況：
設置use_sim_time為true，rosbag回放開始后ROS Time與Bag Time一致：
3. 運行slam_gmapping節點，它將在scan主題上監聽激光掃描數據並創建地圖（可以在命令行中設置建圖參數：比如地圖分辨率、粒子數目、迭代次數、地圖更新間隔等參數）
$ rosrun gmapping slam_gmapping scan:=scan _xmin:=-2.5 _xmax:=2.5 _ymin:=-2.5 _ymax:=2.5 ...
比較重要的幾個參數有：
- particles (int, default: 30) gmapping算法中的粒子數，因為gmapping使用的是粒子濾波算法，粒子在不斷地迭代更新，所以選取一個合適的粒子數可以讓算法在保證比較准確的同時有較高的速度。
- minimumScore (float, default: 0.0) 最小匹配得分，這個參數很重要，它決定了對激光的一個置信度，越高說明對激光匹配算法的要求越高，激光的匹配也越容易失敗而轉去使用里程計數據，而設的太低又會使地圖中出現大量噪聲，所以需要權衡調整。在V-rep仿真中里程計數據是直接通過函數獲取的，沒有誤差，因此可以將這個值調高一點，讓地圖的匹配更多依賴里程計數據。
- lskip(int, default: 0)的值如果為0，則所有的激光數據幀都會用來進行scan matching，如果lskip的值大於0則會跳過幾幀來進行scan matching。有時激光數據的噪聲會比較大，對所有數據幀進行匹配的效果可能會不好，這時可以加大lskip的值。
xmin,xmax,ymin和ymax這4個參數設定了地圖的初始尺寸，但並非最終尺寸（make the starting size small，參考Gmapping does not correctly set up map size parameters )- ...
slam_gmapping節點用到的參數相當多，有很多參數需要在實際中測試多次來確定其值。如果參數太多在命令行中輸入會不太方便，可以寫成launch文件來運行：
<launch> <arg name="scan_topic" default="scan" /> <!-- laser的topic名稱，與自己的激光的topic相對應 --> <arg name="base_frame" default="base_link"/> <!-- 機器人基坐標系 --> <arg name="odom_frame" default="odom"/> <!-- 里程計坐標系 --> <arg name="map_frame" default="map" /> <!-- 地圖坐標系 --> <!-- 啟動slam_gmapping節點 --> <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true"> <!-- clear_params: Delete all parameters in the node's private namespace before launch --> <!-- Parameters used by gmapping wrapper --> <param name="base_frame" value="$(arg base_frame)"/> <!-- The frame attached to the mobile base --> <param name="odom_frame" value="$(arg odom_frame)"/> <!-- The frame attached to the odometry system --> <param name="map_frame" value="$(arg map_frame)" /> <!-- The frame attached to the map --> <param name="map_update_interval" value="0.5"/> <!-- 地圖更新時間間隔 Lowering this number updates the occupancy grid more often, at the expense of greater computational load. --> <param name="throttle_scans" value="1"/> <!-- throw away every nth laser scan. set it to a higher number to skip more scans --> <!-- Parameters used by gmapping itself --> <!-- Laser Parameters --> <param name="maxUrange" value="1.8"/> <!-- maximum range of the laser scanner that is used for map building. set maxUrange < maximum range of the real sensor <= maxRange --> <param name="maxRange" value="2.0"/> <!-- maximum range of the laser scans. Rays beyond this range get discarded completely --> <param name="sigma" value="0.05"/> <!-- standard deviation for the scan matching process --> <param name="kernelSize" value="3"/> <!-- search window for the scan matching process --> <param name="lstep" value="0.05"/> <!-- The optimization step in translation 平移優化步長 --> <param name="astep" value="0.05"/> <!-- The optimization step in rotation 旋轉優化步長--> <param name="iterations" value="5"/> <!-- number of refinement steps in the scan matching 掃描匹配迭代步數--> <param name="lsigma" value="0.075"/> <!-- standard deviation for the scan matching process --> <param name="ogain" value="3.0"/> <!-- gain for smoothing the likelihood --> <param name="lskip" value="5"/> <!-- 0表示所有的激光都處理，如果計算壓力過大可以將該值調大。 take only every (n+1)th laser ray for computing a match (0 = take all rays) --> <param name="minimumScore" value="80"/> <!-- 判斷scanmatch是否成功的閾值，過高的話會使scanmatch失敗，從而影響地圖更新速率 --> <!-- Motion Model Parameters (all standard deviations of a gaussian noise model). 運動模型的噪聲參數 --> <param name="srr" value="0.01"/> <!-- linear noise component (x and y) --> <param name="stt" value="0.02"/> <!-- angular noise component (theta) --> <param name="srt" value="0.02"/> <!-- linear -> angular noise component --> <param name="str" value="0.01"/> <!-- angular -> linear noise component --> <!-- Initial map dimensions and resolution --> <param name="xmin" value="-2.5"/> <!-- minimum x position in the map [m] --> <param name="xmax" value="2.5"/> <!-- maximum x position in the map [m] --> <param name="ymin" value="-2.5"/> <!-- minimum y position in the map [m] --> <param name="ymax" value="2.5"/> <!-- maximum y position in the map [m] --> <param name="delta" value="0.05"/> <!-- size of one pixel [m], 地圖分辨率 --> <!-- Likelihood sampling (used in scan matching) --> <param name="llsamplerange" value="0.01"/> <!-- linear range --> <param name="lasamplerange" value="0.005"/> <!-- linear step size --> <param name="llsamplestep" value="0.01"/> <!-- linear range --> <param name="lasamplestep" value="0.005"/> <!-- angular step size --> <!-- Others --> <param name="linearUpdate" value="0.05"/> <!-- 機器人移動linearUpdate距離，進行scanmatch --> <param name="angularUpdate" value="0.0436"/> <!-- 機器人選裝angularUpdate角度，進行scanmatch --> <param name="resampleThreshold" value="0.5"/> <!-- 重采樣門限Neff. threshold at which the particles get resampled. Higher means more frequent resampling --> <param name="particles" value="100"/> <!-- 濾波器中粒子數目 Each particle represents a possible trajectory that the robot has traveled --> <remap from="scan" to="$(arg scan_topic)"/> </node> </launch>
4. 在新終端中啟動bag包回放，將數據提供給slam_gmapping節點
$ rosbag play my_scan_data.bag
在數據回放過程中也可以打開rviz進行查看：啟動rviz，在左下方點擊add按鈕，然后選擇map，創建一副空地圖；接着制定rviz的topic為/map可以監聽到地圖數據。下圖是rosbag回放過程中建圖的動態過程：
5. 使用map_server生成地圖
$ rosrun map_server map_saver -f my_map
使用map_saver命令后會生成兩個文件。my_map.pgm是地圖的PGM格式的圖片，PGM格式是便攜式灰度圖像格式(portable graymap file format)。my_map.yaml文件描述地圖元數據。
my_map.yaml文件內容如下：
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：地圖中左下角像素的位置和姿態（x，y，yaw），偏航為逆時針旋轉（yaw = 0表示無旋轉）
- occupancy_thresh：概率大於該閾值的像素被認為完全占用
- free_thresh：概率小於該閾值的像素被認為是完全自由的
- negate：“白/黑”對應“自由/占用”語義是否應該被反轉
最終生成的地圖如下圖所示，圖中越亮/白的像素表示沒有障礙物(free)的概率越大，越暗/黑的像素表示被障礙物占據(occupied)的概率越大，灰色表示狀態未知。
用屏幕測量工具測量圖片上的像素間的距離，再乘以分辨率可以得到實際尺寸。例如，左下角點和右下角點的像素間距測量結果為102（可能點取的不精確），對應的實際距離為5.1m，這與真實地圖大小一致。
另外，需要注意map坐標系的原點在圖片左下角，origin: [-12.200000, -12.200000, 0.000000]表示map原點相對於odom坐標系（機器人初始位置）的位置和姿態。
6. 在建圖結束后不要忘記重置use_sim_time參數
$ rosparam set use_sim_time false
參考：
Learning about tf and time (C++)
Publishing Odometry Information over ROS