【SLAM建图和导航仿真实例】（一）- 模型构建
引言
在這個-SLAM建圖和導航仿真實例-項目中，主要分為三個部分，分別是
- （一）模型構建
- （二）根據已知地圖進行定位和導航
- （三）使用RTAB-MAP進行建圖和導航
該項目的slam_bot已經上傳我的Github。
由于之前的虛擬機性能限制，我在這個項目中使用了新的ubantu 16.04環境，虛擬機配置
- 內存 8G
- CPU 四核
- 禁用硬件加速（重要：否則gazebo會打開失敗）
一、模型構建
1、使用Solidworks建立slam_bot包
使用Solidworks建立模型如下圖，其中有很多部分值得注意。
- 定義底盤底面為base_link坐標系的xoy平面，底面中點為坐標原點，小車正方向為x軸正方向建立右手系；
- 各個車輪坐標系原點為各個車輪內側平面圓心，x軸與base_link坐標系x軸對齊，y軸與車軸平齊（與base_link坐標系y軸對齊或相向）建立右手系；
- 車輪的旋轉軸分別為對應的轉軸軸線。
值得注意的是，在下圖中有兩個坐標系，分別是坐標系1和coordinate0。在一開始，我沒有意識到使用coordinate0作為坐標原點是一個多么錯誤的選擇。在后來使用skid_steer_drive_controllergazebo插件時，模型一直在原地轉圈，直到我在rviz中查看tf轉換關系時，才發現是錯誤的tf轉換，導致了插件的控制錯誤。所以一定要嚴格遵循上述所說的坐標系建立規則。
使用solidworks的urdf_exporter插件導出urdf和模型文件。在圖1-2中base_link忘記選擇參考坐標系了，若以上圖坐標系為準，則需要選擇 - 坐標系1。
關于更多urdf_exporter安裝和使用可以參考我的博客-【從零開始的ROS四軸機械臂控制】（一）- 實際模型制作、Solidworks文件轉urdf與rviz仿真.。
若選擇無誤，就可以導出模型了。
我在Github中提供了最初始的slam_bot package文件。
2.更改slam_bot包
（1）urdf文件夾
導航到urdf文件夾
$ ~/catkin_ws/src/slam_bot/urdf
將slam_bot.urdf 更名成slam_bot.xacro，并新建slam_bot.gazebo.xacro文件。
更改slam_bot.xacro
添加xacro namespace。
<robot name="slam_bot" xmlns:xacro="http://www.ros.org/wiki/xacro">
添加slam_bot.gazebo.xacro，導入gazebo插件
<xacro:include filename="$(find slam_bot)/urdf/slam_bot.gazebo.xacro" />
在base_link節點之前添加base_footprint節點
<!--base_footprint link--><link name="base_footprint"> </link><joint name="base_link_joint" type="fixed"><origin xyz="0 0 0" rpy="0 0 0" /><parent link="base_footprint"/><child link="base_link" /></joint>
在camera節點之后添加laser節點
<!--laser_link--><link name="hokuyo"><collision><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://slam_bot/meshes/hokuyo.dae"/></geometry></collision><visual><origin xyz="0 0 0" rpy="0 0 0"/><geometry><mesh filename="package://slam_bot/meshes/hokuyo.dae"/></geometry><material name="red"><color rgba="1.0 0 0 1.0"/></material></visual><inertial><mass value="1e-5" /><origin xyz="0 0 0" rpy="0 1.57 0"/><inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" /></inertial></link><joint name="hokuyo_joint" type="fixed"><axis xyz="0 0 0" /><origin xyz="0 0 0" rpy="0 0 0"/><parent link="base_link"/><child link="hokuyo"/></joint>
更改各個joint設置，如joint_wheel_FR。注意<axis>的設置，否則也會導致錯誤的控制。
<jointname="joint_wheel_FR"type="continuous"><originxyz="0.0800000000000013 -0.0799999999999997 0.04"rpy="0 0 0" /><parentlink="base_link" /><childlink="link_wheel_FR" /><axisxyz="0 1 0" /></joint>
joint type設置為“continuous”，類似于轉動關節，但對其旋轉沒有限制。它可以繞一個軸連續旋轉。關節會有自己的旋轉軸axis，一些特定的關節動力學dynamics與關節的物理特性(如“摩擦”)相對應，以及對該關節施加最大“努力”和“速度”的某些限制limits。這些限制對于物理機器人是有用的約束，并且可以幫助在仿真中創建更健壯的機器人模型。
點擊這里來更好地理解這些限制。
RGB-D點云是默認朝上,需向slam_bot.xacro中添加如下的link和joint
<link name="camera_depth_optical_frame" /><joint name="camera_depth_optical_joint" type="fixed"><origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0" /><parent link="camera"/><child link="camera_depth_optical_frame" /></joint>
更改slam_bot.gazebo.xacro
添加如下插件
- 四輪機器人控制驅動：
skid_steer_drive_controller - RGBD 深度攝像機:
libgazebo_ros_openni_kinect - 激光傳感器:
head_hokuyo_sensor
<?xml version="1.0"?>
<robot><gazebo><plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so"><alwaysOn>true</alwaysOn><updateRate>100.0</updateRate><robotNamespace>/</robotNamespace><leftFrontJoint>joint_wheel_FL</leftFrontJoint><rightFrontJoint>joint_wheel_FR</rightFrontJoint><leftRearJoint>joint_wheel_BL</leftRearJoint><rightRearJoint>joint_wheel_BR</rightRearJoint><wheelSeparation>0.16</wheelSeparation><wheelDiameter>0.08</wheelDiameter><robotBaseFrame>base_footprint</robotBaseFrame><torque>20</torque><commandTopic>cmd_vel</commandTopic><odometryTopic>odom</odometryTopic><odometryFrame>odom</odometryFrame><broadcastTF>1</broadcastTF></plugin></gazebo><!--RGBD camera --><gazebo reference="camera"><sensor type="depth" name="camera"><always_on>true</always_on><visualize>false</visualize><update_rate>15.0</update_rate><camera name="front"><horizontal_fov>1.047197</horizontal_fov><image><!-- openni_kinect plugin works only with BGR8 --><format>B8G8R8</format><width>400</width><height>300</height></image><clip><near>0.01</near><far>8</far></clip></camera><plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so"><baseline>0.1</baseline><alwaysOn>true</alwaysOn><updateRate>15.0</updateRate><cameraName>camera</cameraName><imageTopicName>/camera/rgb/image_raw</imageTopicName><cameraInfoTopicName>/camera/rgb/camera_info</cameraInfoTopicName><depthImageTopicName>/camera/depth/image_raw</depthImageTopicName><depthImageCameraInfoTopicName>/camera/depth_registered/camera_info</depthImageCameraInfoTopicName><pointCloudTopicName>/camera/depth_registered/points</pointCloudTopicName><frameName>camera_depth_optical_frame</frameName><pointCloudCutoff>0.35</pointCloudCutoff><pointCloudCutoffMax>4.5</pointCloudCutoffMax><CxPrime>0</CxPrime><Cx>0</Cx><Cy>0</Cy><focalLength>0</focalLength><hackBaseline>0</hackBaseline></plugin></sensor></gazebo><!-- hokuyo --><gazebo reference="hokuyo"><sensor type="ray" name="head_hokuyo_sensor"><pose>0 0 0 0 0 0</pose><visualize>false</visualize><update_rate>40</update_rate><ray><scan><horizontal><samples>720</samples><resolution>1</resolution><min_angle>-1.570796</min_angle><max_angle>1.570796</max_angle></horizontal></scan><range><min>0.10</min><max>30.0</max><resolution>0.01</resolution></range><noise><type>gaussian</type><!-- Noise parameters based on published spec for Hokuyo laserachieving "+-30mm" accuracy at range < 10m. A mean of 0.0m andstddev of 0.01m will put 99.7% of samples within 0.03m of the truereading. --><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so"><topicName>/slam_bot/laser/scan</topicName><frameName>hokuyo</frameName></plugin></sensor></gazebo>
</robot>
（2）worlds文件夾
在worlds中儲存gazebo world。world是模型的集合，還可以定義此world特定的其他幾個物理屬性。
讓我們創建一個簡單的世界，其中沒有將在以后的gazebo中啟動的對象或模型。
$ cd worlds
$ nano slam.world
將以下內容添加到 slam.world
<?xml version="1.0" ?><sdf version="1.4"><world name="default"><include><uri>model://ground_plane</uri></include><!-- Light source --><include><uri>model://sun</uri></include><!-- World camera --><gui fullscreen='0'><camera name='world_camera'><pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose><view_controller>orbit</view_controller></camera></gui></world>
</sdf>
.world 文件使用 XML 文件格式來描述所有被定義為 Gazeboeboard 環境的元素。在上面創建的簡單世界有以下元素
<sdf>: 基本元素，封裝了整個文件結構和內容。
<world>：世界元素定義了世界描述和與世界相關的幾個屬性。每個模型或屬性可以有更多的元素來描述它。例如，camera有一個pose元素，定義了它的位置和方向。
<include>：include元素和<uri>元素一起，提供了通往特定模型的路徑。在Gazebo中，有幾個模型是默認包含的，可以在創建環境時包含它們。
（3）launch文件夾
創建一個新的啟動文件，這將有助于加載URDF文件。
$ cd ~/catkin_ws/src/slam/launch/
$ nano robot_description.launch
將以下內容復制到上述文件中。
<launch><!-- send urdf to param server --><param name="robot_description" command="$(find xacro)/xacro --inorder '$(find slam_bot)/urdf/slam_bot.xacro'" /><!-- Send fake joint values--><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"><param name="use_gui" value="false"/></node><!-- Send robot states to tf --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"/></launch>上面的代碼定義了一個參數robot_description，該參數用于設置單個命令，以使用xacro軟件包從xacro文件生成URDF。
借助robot_description生成的URDF文件，gazebo_ros包生成對應模型。
接下來創建一個launch文件。ROS中的啟動文件使我們可以同時執行多個節點，這有助于避免在單獨的shell或終端中定義和啟動多個節點的繁瑣工作。
$ nano slam.launch
將以下內容添加到啟動文件中。
<?xml version="1.0" encoding="UTF-8"?>
<launch><include file="$(find slam_bot)/launch/robot_description.xml"/><arg name="world" default="empty"/> <arg name="paused" default="false"/><arg name="use_sim_time" default="true"/><arg name="gui" default="true"/><arg name="headless" default="false"/><arg name="debug" default="false"/><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find slam_bot)/worlds/kitchen_dining.world"/><arg name="paused" value="$(arg paused)"/><arg name="use_sim_time" value="$(arg use_sim_time)"/><arg name="gui" value="$(arg gui)"/><arg name="headless" value="$(arg headless)"/><arg name="debug" value="$(arg debug)"/></include><!--spawn a robot in gazebo world--><node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param robot_description -model slam_bot"/><!--launch rviz--><node name="rviz" pkg="rviz" type="rviz" respawn="false"/></launch>和.world文件一樣，.launch文件也是基于XML的。
首先，使用 <arg>元素定義某些參數。每個這樣的元素都會有一個name屬性和一個default值。
然后，在gazebo_ros 包中包含了empty_world.launch文件。empty_world文件包含了一組重要的定義，這些定義被我們創建的world所繼承。使用 world_name 參數和傳遞給該參數的value的.world文件的路徑，所以我們能夠在 Gazebo 中啟動world。
3.運行slam_bot
現在可以使用啟動文件來啟動Gazebo環境。
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
$ roslaunch slam slam.launch
若gazebo長時間打不開，解決辦法如下
cd ~/
hg clone https://bitbucket.org/osrf/gazebo_models
下載完成后將gazebo_models復制到~/.gazebo文件夾中，重命名為models。
若模型正常運行，則如圖3-1所示。
（1）測試RGBD相機和激光雷達
這次，涼亭和RViz都應該啟動。加載后
選擇RViz窗口，然后在左側的Displays中：
- 選擇“ odom”作為fixed frame
點擊“Add”按鈕，然后
- 添加“ RobotModel”
- 添加“pointcloud2”并選擇在gazebo插件中定義的/camera/depth_registered/points主題
- 添加“ LaserScan”并選擇在gazebo插件中定義的hokuyo主題。
機器人模型應在RViz中加載。
在涼亭中，單擊“插入”，然后從列表中添加機器人前面世界中的任何物品。
至此應該能夠在Rviz中的“pointcloud2”查看器中看到該項目，并且也可以對該對象進行激光掃描。
圖3-2來自較舊版本的模型，所以看起來與之前模型會有所不同。
（2）測試四輪驅動插件
在上述所有內容仍在運行時，打開一個新的終端窗口，然后輸入
$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:x: 0.1y: 0.0z: 0.0
angular:x: 0.0y: 0.0z: 0.0"
上面的命令會將消息發布到cmd_vel，這是在驅動器控制器插件中定義的主題。
可以看到模型正常移動，gazebo因為錄屏軟件奔潰了，不過正常運行是不會這樣的。
模型應當沿x軸正方向移動，如果出現運動不正常的現象，可以檢查
skid_steer_drive_controller定義- urdf中各個joint定義
- 檢查tf變換，使用
rosrun tf view_frames以查看tf信息
創建和查看tf-tree是確保所有鏈接順序正確的好方法。
也可以在RViz中繪制不同的框架并在那里進行圖形上的檢查。
總結
以上是生活随笔為你收集整理的【SLAM建图和导航仿真实例】（一）- 模型构建的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 【scipy 基础】--空间计算
- 下一篇： Python 之 Numpy 框架入门