【ROS學(xué)習筆記17】ROS常用仿真組件URDF集成Gazebo
寫在前面，本系列筆記參考的是AutoLabor的教程，具體項目地址在 這里
前言
1. URDF集成Gazebo
1.1 URDF與Gazebo基本集成流程
URDF 與 Gazebo 集成流程與 Rviz 實現(xiàn)類似，主要步驟如下:
- 創(chuàng)建功能包，導(dǎo)入依賴項
- 編寫 URDF 或 Xacro 文件
- 啟動 Gazebo 并顯示機器人模型
1.創(chuàng)建功能包
創(chuàng)建新功能包，導(dǎo)入依賴包:
urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins
2.編寫URDF文件
<!--
創(chuàng)建一個機器人模型(盒狀即可)，顯示在 Gazebo 中
-->
<robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 1" />
</material>
</visual>
<collision>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="6" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
</gazebo>
</robot>
注意， 當 URDF 需要與 Gazebo 集成時，和 Rviz 有明顯區(qū)別:
1.必須使用 collision 標簽，因為既然是仿真環(huán)境，那么必然涉及到碰撞檢測，collision 提供碰撞檢測的依據(jù)。
2.必須使用 inertial 標簽，此標簽標注了當前機器人某個剛體部分的慣性矩陣，用于一些力學(xué)相關(guān)的仿真計算。
3.顏色設(shè)置，也需要重新使用 gazebo 標簽標注，因為之前的顏色設(shè)置為了方便調(diào)試包含透明度，仿真環(huán)境下沒有此選項。
3.啟動Gazebo并顯示模型
launch 文件實現(xiàn):
<launch>
<!-- 將 Urdf 文件的內(nèi)容加載到參數(shù)服務(wù)器 -->
<param name="robot_description" textfile="$(find demo02_urdf_gazebo)/urdf/urdf01_helloworld.urdf" />
<!-- 啟動 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 在 gazebo 中顯示機器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
代碼解釋:
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 啟動 Gazebo 的仿真環(huán)境，當前環(huán)境為空環(huán)境 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
<!--
在 Gazebo 中加載一個機器人模型，該功能由 gazebo_ros 下的 spawn_model 提供:
-urdf 加載的是 urdf 文件
-model mycar 模型名稱是 mycar
-param robot_description 從參數(shù) robot_description 中載入模型
-x 模型載入的 x 坐標
-y 模型載入的 y 坐標
-z 模型載入的 z 坐標
-->
示例結(jié)果：文章來源：http://www.zghlxwxcb.cn/news/detail-740252.html
1.2 URDF集成Gazebo相關(guān)設(shè)置
較之于 rviz，gazebo在集成 URDF 時，需要做些許修改，比如:必須添加 collision 碰撞屬性相關(guān)參數(shù)、必須添加 inertial 慣性矩陣相關(guān)參數(shù)，另外，如果直接移植 Rviz 中機器人的顏色設(shè)置是沒有顯示的，顏色設(shè)置也必須做相應(yīng)的變更。
1.collision
如果機器人link是標準的幾何體形狀，和link的 visual 屬性設(shè)置一致即可。
2.inertial
慣性矩陣的設(shè)置需要結(jié)合link的質(zhì)量與外形參數(shù)動態(tài)生成，標準的球體、圓柱與立方體的慣性矩陣公式如下(已經(jīng)封裝為 xacro 實現(xiàn)):
球體慣性矩陣
<!-- Macro for inertia matrix -->
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
圓柱慣性矩陣
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
立方體慣性矩陣
<xacro:macro name="Box_inertial_matrix" params="m l w h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
iyy="${m*(w*w + l*l)/12}" iyz= "0"
izz="${m*(w*w + h*h)/12}" />
</inertial>
</xacro:macro>
需要注意的是，原則上，除了 base_footprint 外，機器人的每個剛體部分都需要設(shè)置慣性矩陣，且慣性矩陣必須經(jīng)計算得出，如果隨意定義剛體部分的慣性矩陣，那么可能會導(dǎo)致機器人在 Gazebo 中出現(xiàn)抖動，移動等現(xiàn)象。
3.顏色設(shè)置
在 gazebo 中顯示 link 的顏色，必須要使用指定的標簽:
<gazebo reference="link節(jié)點名稱">
<material>Gazebo/Blue</material>
</gazebo>
**PS：**material 標簽中，設(shè)置的值區(qū)分大小寫，顏色可以設(shè)置為 Red Blue Green Black …
示例結(jié)果：
1.3 URDF集成Gazebo實操
需求描述:
將之前的機器人模型(xacro版)顯示在 gazebo 中
結(jié)果演示:
實現(xiàn)流程:
- 需要編寫封裝慣性矩陣算法的 xacro 文件
- 為機器人模型中的每一個 link 添加 collision 和 inertial 標簽，并且重置顏色屬性
- 在 launch 文件中啟動 gazebo 并添加機器人模型
1.編寫封裝慣性矩陣算法的 xacro 文件
<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- Macro for inertia matrix -->
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
<xacro:macro name="Box_inertial_matrix" params="m l w h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
iyy="${m*(w*w + l*l)/12}" iyz= "0"
izz="${m*(w*w + h*h)/12}" />
</inertial>
</xacro:macro>
</robot>
2.復(fù)制相關(guān) xacro 文件，并設(shè)置 collision inertial 以及 color 等參數(shù)
A.底盤 Xacro 文件
<!--
使用 xacro 優(yōu)化 URDF 版的小車底盤實現(xiàn)：
實現(xiàn)思路:
1.將一些常量、變量封裝為 xacro:property
比如:PI 值、小車底盤半徑、離地間距、車輪半徑、寬度 ....
2.使用 宏 封裝驅(qū)動輪以及支撐輪實現(xiàn)，調(diào)用相關(guān)宏生成驅(qū)動輪與支撐輪
-->
<!-- 根標簽，必須聲明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 封裝變量、常量 -->
<!-- PI 值設(shè)置精度需要高一些，否則后續(xù)車輪翻轉(zhuǎn)量計算時，可能會出現(xiàn)肉眼不能察覺的車輪傾斜，從而導(dǎo)致模型抖動 -->
<xacro:property name="PI" value="3.1415926"/>
<!-- 宏:黑色設(shè)置 -->
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
<!-- 底盤屬性 -->
<xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半徑 -->
<xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半徑 -->
<xacro:property name="base_link_length" value="0.08" /> <!-- base_link 長 -->
<xacro:property name="earth_space" value="0.015" /> <!-- 離地間距 -->
<xacro:property name="base_link_m" value="0.5" /> <!-- 質(zhì)量 -->
<!-- 底盤 -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="${base_footprint_radius}" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_link_radius}" length="${base_link_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 0.5" />
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${base_link_radius}" length="${base_link_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
</collision>
<xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}" />
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
</joint>
<gazebo reference="base_link">
<material>Gazebo/Yellow</material>
</gazebo>
<!-- 驅(qū)動輪 -->
<!-- 驅(qū)動輪屬性 -->
<xacro:property name="wheel_radius" value="0.0325" /><!-- 半徑 -->
<xacro:property name="wheel_length" value="0.015" /><!-- 寬度 -->
<xacro:property name="wheel_m" value="0.05" /> <!-- 質(zhì)量 -->
<!-- 驅(qū)動輪宏實現(xiàn) -->
<xacro:macro name="add_wheels" params="name flag">
<link name="${name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
<material name="black" />
</visual>
<collision>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
</collision>
<xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
<axis xyz="0 1 0" />
</joint>
<gazebo reference="${name}_wheel">
<material>Gazebo/Red</material>
</gazebo>
</xacro:macro>
<xacro:add_wheels name="left" flag="1" />
<xacro:add_wheels name="right" flag="-1" />
<!-- 支撐輪 -->
<!-- 支撐輪屬性 -->
<xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撐輪半徑 -->
<xacro:property name="support_wheel_m" value="0.03" /> <!-- 質(zhì)量 -->
<!-- 支撐輪宏 -->
<xacro:macro name="add_support_wheel" params="name flag" >
<link name="${name}_wheel">
<visual>
<geometry>
<sphere radius="${support_wheel_radius}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black" />
</visual>
<collision>
<geometry>
<sphere radius="${support_wheel_radius}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
</collision>
<xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" />
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
<axis xyz="1 1 1" />
</joint>
<gazebo reference="${name}_wheel">
<material>Gazebo/Red</material>
</gazebo>
</xacro:macro>
<xacro:add_support_wheel name="front" flag="1" />
<xacro:add_support_wheel name="back" flag="-1" />
</robot>
注意: 如果機器人模型在 Gazebo 中產(chǎn)生了抖動，滑動，緩慢位移 … 諸如此類情況，請查看
- 慣性矩陣是否設(shè)置了，且設(shè)置是否正確合理
- 車輪翻轉(zhuǎn)需要依賴于 PI 值，如果 PI 值精度偏低，也可能導(dǎo)致上述情況產(chǎn)生
B.攝像頭 Xacro 文件
<!-- 攝像頭相關(guān)的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 攝像頭屬性 -->
<xacro:property name="camera_length" value="0.01" /> <!-- 攝像頭長度(x) -->
<xacro:property name="camera_width" value="0.025" /> <!-- 攝像頭寬度(y) -->
<xacro:property name="camera_height" value="0.025" /> <!-- 攝像頭高度(z) -->
<xacro:property name="camera_x" value="0.08" /> <!-- 攝像頭安裝的x坐標 -->
<xacro:property name="camera_y" value="0.0" /> <!-- 攝像頭安裝的y坐標 -->
<xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 攝像頭安裝的z坐標:底盤高度 / 2 + 攝像頭高度 / 2 -->
<xacro:property name="camera_m" value="0.01" /> <!-- 攝像頭質(zhì)量 -->
<!-- 攝像頭關(guān)節(jié)以及l(fā)ink -->
<link name="camera">
<visual>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
<collision>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</collision>
<xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
</link>
<joint name="camera2base_link" type="fixed">
<parent link="base_link" />
<child link="camera" />
<origin xyz="${camera_x} ${camera_y} ${camera_z}" />
</joint>
<gazebo reference="camera">
<material>Gazebo/Blue</material>
</gazebo>
</robot>
C.雷達 Xacro 文件
<!--
小車底盤添加雷達
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷達支架 -->
<xacro:property name="support_length" value="0.15" /> <!-- 支架長度 -->
<xacro:property name="support_radius" value="0.01" /> <!-- 支架半徑 -->
<xacro:property name="support_x" value="0.0" /> <!-- 支架安裝的x坐標 -->
<xacro:property name="support_y" value="0.0" /> <!-- 支架安裝的y坐標 -->
<xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安裝的z坐標:底盤高度 / 2 + 支架高度 / 2 -->
<xacro:property name="support_m" value="0.02" /> <!-- 支架質(zhì)量 -->
<link name="support">
<visual>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="red">
<color rgba="0.8 0.2 0.0 0.8" />
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</collision>
<xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}" />
</link>
<joint name="support2base_link" type="fixed">
<parent link="base_link" />
<child link="support" />
<origin xyz="${support_x} ${support_y} ${support_z}" />
</joint>
<gazebo reference="support">
<material>Gazebo/White</material>
</gazebo>
<!-- 雷達屬性 -->
<xacro:property name="laser_length" value="0.05" /> <!-- 雷達長度 -->
<xacro:property name="laser_radius" value="0.03" /> <!-- 雷達半徑 -->
<xacro:property name="laser_x" value="0.0" /> <!-- 雷達安裝的x坐標 -->
<xacro:property name="laser_y" value="0.0" /> <!-- 雷達安裝的y坐標 -->
<xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷達安裝的z坐標:支架高度 / 2 + 雷達高度 / 2 -->
<xacro:property name="laser_m" value="0.1" /> <!-- 雷達質(zhì)量 -->
<!-- 雷達關(guān)節(jié)以及l(fā)ink -->
<link name="laser">
<visual>
<geometry>
<cylinder radius="${laser_radius}" length="${laser_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
<collision>
<geometry>
<cylinder radius="${laser_radius}" length="${laser_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</collision>
<xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}" />
</link>
<joint name="laser2support" type="fixed">
<parent link="support" />
<child link="laser" />
<origin xyz="${laser_x} ${laser_y} ${laser_z}" />
</joint>
<gazebo reference="laser">
<material>Gazebo/Black</material>
</gazebo>
</robot>
D.組合底盤、攝像頭與雷達的 Xacro 文件
<!-- 組合小車底盤與攝像頭 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
</robot>
Copy
3.在 gazebo 中執(zhí)行
launch 文件:
<launch>
<!-- 將 Urdf 文件的內(nèi)容加載到參數(shù)服務(wù)器 -->
<param name="robot_description" command="$(find xacro)/xacro $(find demo02_urdf_gazebo)/urdf/xacro/my_base_camera_laser.urdf.xacro" />
<!-- 啟動 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 在 gazebo 中顯示機器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
1.4 Gazebo仿真環(huán)境搭建
到目前為止，我們已經(jīng)可以將機器人模型顯示在 Gazebo 之中了，但是當前默認情況下，在 Gazebo 中機器人模型是在 empty world 中，并沒有類似于房間、家具、道路、樹木… 之類的仿真物，如何在 Gazebo 中創(chuàng)建仿真環(huán)境呢？
Gazebo 中創(chuàng)建仿真實現(xiàn)方式有兩種:
- 方式1: 直接添加內(nèi)置組件創(chuàng)建仿真環(huán)境
- 方式2: 手動繪制仿真環(huán)境(更為靈活)
也還可以直接下載使用官方或第三方提高的仿真環(huán)境插件。
1.添加內(nèi)置組件創(chuàng)建仿真環(huán)境
1.1啟動 Gazebo 并添加組件
1.2保存仿真環(huán)境
添加完畢后，選擇 file —> Save World as 選擇保存路徑(功能包下: worlds 目錄)，文件名自定義，后綴名設(shè)置為 .world
1.3 啟動
<launch>
<!-- 將 Urdf 文件的內(nèi)容加載到參數(shù)服務(wù)器 -->
<param name="robot_description" command="$(find xacro)/xacro $(find demo02_urdf_gazebo)/urdf/xacro/my_base_camera_laser.urdf.xacro" />
<!-- 啟動 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find demo02_urdf_gazebo)/worlds/hello.world" />
</include>
<!-- 在 gazebo 中顯示機器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
核心代碼: 啟動 empty_world 后，再根據(jù)arg加載自定義的仿真環(huán)境
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find demo02_urdf_gazebo)/worlds/hello.world" />
</include>
示例結(jié)果：
2.自定義仿真環(huán)境
2.1 啟動 gazebo 打開構(gòu)建面板，繪制仿真環(huán)境
2.2 保存構(gòu)建的環(huán)境
點擊: 左上角 file —> Save (保存路徑功能包下的: models)
然后 file —> Exit Building Editor
2.3 保存為 world 文件
可以像方式1一樣再添加一些插件，然后保存為 world 文件(保存路徑功能包下的: worlds)
2.4 啟動
同方式1
3.使用官方提供的插件
當前 Gazebo 提供的仿真道具有限，還可以下載官方支持，可以提供更為豐富的仿真實現(xiàn)，具體實現(xiàn)如下:
3.1 下載官方模型庫
git clone https://github.com/osrf/gazebo_models
之前是:hg clone https://bitbucket.org/osrf/gazebo_models但是已經(jīng)不可用
注意: 此過程可能比較耗時
3.2 將模型庫復(fù)制進 gazebo
將得到的gazebo_models文件夾內(nèi)容復(fù)制到 /usr/share/gazebo-*/models
3.3 應(yīng)用
重啟 Gazebo，選擇左側(cè)菜單欄的 insert 可以選擇并插入相關(guān)道具了
2. URDF、Gazebo、Rviz綜合應(yīng)用
關(guān)于URDF(Xacro)、Rviz 和 Gazebo 三者的關(guān)系，前面已有闡述: URDF 用于創(chuàng)建機器人模型、Rviz 可以顯示機器人感知到的環(huán)境信息，Gazebo 用于仿真，可以模擬外界環(huán)境，以及機器人的一些傳感器，如何在 Gazebo 中運行這些傳感器，并顯示這些傳感器的數(shù)據(jù)(機器人的視角)呢？本節(jié)主要介紹的重點就是將三者結(jié)合:通過 Gazebo 模擬機器人的傳感器，然后在 Rviz 中顯示這些傳感器感知到的數(shù)據(jù)。主要內(nèi)容包括:
- 運動控制以及里程計信息顯示
- 雷達信息仿真以及顯示
- 攝像頭信息仿真以及顯示
- kinect 信息仿真以及顯示
2.1 機器人運動控制及里程計信息顯示
gazebo 中已經(jīng)可以正常顯示機器人模型了，那么如何像在 rviz 中一樣控制機器人運動呢？在此，需要涉及到ros中的組件: ros_control。
1.ros_control 簡介
**場景:**同一套 ROS 程序，如何部署在不同的機器人系統(tǒng)上，比如：開發(fā)階段為了提高效率是在仿真平臺上測試的，部署時又有不同的實體機器人平臺，不同平臺的實現(xiàn)是有差異的，如何保證 ROS 程序的可移植性？ROS 內(nèi)置的解決方式是 ros_control。
**ros_control:**是一組軟件包，它包含了控制器接口，控制器管理器，傳輸和硬件接口。ros_control 是一套機器人控制的中間件，是一套規(guī)范，不同的機器人平臺只要按照這套規(guī)范實現(xiàn)，那么就可以保證 與ROS 程序兼容，通過這套規(guī)范，實現(xiàn)了一種可插拔的架構(gòu)設(shè)計，大大提高了程序設(shè)計的效率與靈活性。
gazebo 已經(jīng)實現(xiàn)了 ros_control 的相關(guān)接口，如果需要在 gazebo 中控制機器人運動，直接調(diào)用相關(guān)接口即可
2.運動控制實現(xiàn)流程(Gazebo)
承上，運動控制基本流程:
- 已經(jīng)創(chuàng)建完畢的機器人模型，編寫一個單獨的 xacro 文件，為機器人模型添加傳動裝置以及控制器
- 將此文件集成進xacro文件
- 啟動 Gazebo 并發(fā)布 /cmd_vel 消息控制機器人運動
2.1 為 joint 添加傳動裝置以及控制器
兩輪差速配置
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 傳動實現(xiàn):用于連接控制器與關(guān)節(jié) -->
<xacro:macro name="joint_trans" params="joint_name">
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${joint_name}_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${joint_name}_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- 每一個驅(qū)動輪都需要配置傳動裝置 -->
<xacro:joint_trans joint_name="left_wheel2base_link" />
<xacro:joint_trans joint_name="right_wheel2base_link" />
<!-- 控制器 -->
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>true</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>1</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>left_wheel2base_link</leftJoint> <!-- 左輪 -->
<rightJoint>right_wheel2base_link</rightJoint> <!-- 右輪 -->
<wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 車輪間距 -->
<wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 車輪直徑 -->
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic> <!-- 運動控制話題 -->
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic> <!-- 里程計話題 -->
<robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐標系 -->
</plugin>
</gazebo>
</robot>
2.2 xacro文件集成
最后還需要將上述 xacro 文件集成進總的機器人模型文件，代碼示例如下:
<!-- 組合小車底盤與攝像頭 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
</robot>
當前核心: 包含 控制器以及傳動配置的 xacro 文件
<xacro:include filename="move.urdf.xacro" />
2.3 啟動 gazebo并控制機器人運動
launch文件:
<launch>
<!-- 將 Urdf 文件的內(nèi)容加載到參數(shù)服務(wù)器 -->
<param name="robot_description" command="$(find xacro)/xacro $(find demo02_urdf_gazebo)/urdf/xacro/my_base_camera_laser.urdf.xacro" />
<!-- 啟動 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find demo02_urdf_gazebo)/worlds/hello.world" />
</include>
<!-- 在 gazebo 中顯示機器人模型 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>
啟動 launch 文件，使用 topic list 查看話題列表，會發(fā)現(xiàn)多了 /cmd_vel 然后發(fā)布 vmd_vel 消息控制即可
使用命令控制(或者可以編寫單獨的節(jié)點控制)
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'
接下來我們會發(fā)現(xiàn): 小車在 Gazebo 中已經(jīng)正常運行起來了
3.Rviz查看里程計信息
在 Gazebo 的仿真環(huán)境中，機器人的里程計信息以及運動朝向等信息是無法獲取的，可以通過 Rviz 顯示機器人的里程計信息以及運動朝向
里程計: 機器人相對出發(fā)點坐標系的位姿狀態(tài)(X 坐標 Y 坐標 Z坐標以及朝向)。
3.1啟動 Rviz
launch 文件
<launch>
<!-- 啟動 rviz -->
<node pkg="rviz" type="rviz" name="rviz" />
<!-- 關(guān)節(jié)以及機器人狀態(tài)發(fā)布節(jié)點 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
3.2 添加組件
執(zhí)行 launch 文件后，在 Rviz 中添加圖示組件:
示例結(jié)果：
2.2 雷達信息仿真及顯示
通過 Gazebo 模擬激光雷達傳感器，并在 Rviz 中顯示激光數(shù)據(jù)。
實現(xiàn)流程:
雷達仿真基本流程:
- 已經(jīng)創(chuàng)建完畢的機器人模型，編寫一個單獨的 xacro 文件，為機器人模型添加雷達配置；
- 將此文件集成進xacro文件；
- 啟動 Gazebo，使用 Rviz 顯示雷達信息。
1.Gazebo 仿真雷達
1.1 新建 Xacro 文件，配置雷達傳感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷達 -->
<gazebo reference="laser">
<sensor type="ray" name="rplidar">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>5.5</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3</min_angle>
<max_angle>3</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>laser</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
1.2 xacro 文件集成
將步驟1的 Xacro 文件集成進總的機器人模型文件，代碼示例如下:
<!-- 組合小車底盤與傳感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
<!-- 雷達仿真的 xacro 文件 -->
<xacro:include filename="my_sensors_laser.urdf.xacro" />
</robot>
1.3啟動仿真環(huán)境
編寫launch文件，啟動gazebo，此處略…
2.Rviz 顯示雷達數(shù)據(jù)
先啟動 rviz,添加雷達信息顯示插件
示例結(jié)果：
2.3 攝像頭信息仿真及顯示
通過 Gazebo 模擬攝像頭傳感器，并在 Rviz 中顯示攝像頭數(shù)據(jù)。
實現(xiàn)流程:
攝像頭仿真基本流程:
- 已經(jīng)創(chuàng)建完畢的機器人模型，編寫一個單獨的 xacro 文件，為機器人模型添加攝像頭配置；
- 將此文件集成進xacro文件；
- 啟動 Gazebo，使用 Rviz 顯示攝像頭信息。
1.Gazebo 仿真攝像頭
1.1 新建 Xacro 文件，配置攝像頭傳感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 被引用的link -->
<gazebo reference="camera">
<!-- 類型設(shè)置為 camara -->
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate> <!-- 更新頻率 -->
<!-- 攝像頭基本信息設(shè)置 -->
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<!-- 核心插件 -->
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
1.2 xacro 文件集成
將步驟1的 Xacro 文件集成進總的機器人模型文件，代碼示例如下:
<!-- 組合小車底盤與傳感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
<!-- 攝像頭仿真的 xacro 文件 -->
<xacro:include filename="my_sensors_camara.urdf.xacro" />
</robot>
1.3啟動仿真環(huán)境
編寫launch文件，啟動gazebo，此處略…
2.Rviz 顯示攝像頭數(shù)據(jù)
執(zhí)行 gazebo 并啟動 Rviz,在 Rviz 中添加攝像頭組件。
示例效果：
2.4 kinect信息仿真及顯示
通過 Gazebo 模擬kinect攝像頭，并在 Rviz 中顯示kinect攝像頭數(shù)據(jù)。
實現(xiàn)流程:
kinect攝像頭仿真基本流程:
- 已經(jīng)創(chuàng)建完畢的機器人模型，編寫一個單獨的 xacro 文件，為機器人模型添加kinect攝像頭配置；
- 將此文件集成進xacro文件；
- 啟動 Gazebo，使用 Rviz 顯示kinect攝像頭信息。
1.Gazebo仿真Kinect
1.1 新建 Xacro 文件，配置 kinetic傳感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<gazebo reference="kinect link名稱">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>kinect link名稱</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>
</sensor>
</gazebo>
</robot>
1.2 xacro 文件集成
將步驟1的 Xacro 文件集成進總的機器人模型文件，代碼示例如下:
<!-- 組合小車底盤與傳感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_head.urdf.xacro" />
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
<xacro:include filename="move.urdf.xacro" />
<!-- kinect仿真的 xacro 文件 -->
<xacro:include filename="my_sensors_kinect.urdf.xacro" />
</robot>
1.3啟動仿真環(huán)境
編寫launch文件，啟動gazebo，此處略…
2 Rviz 顯示 Kinect 數(shù)據(jù)
啟動 rviz,添加攝像頭組件查看數(shù)據(jù)
補充:kinect 點云數(shù)據(jù)顯示
在kinect中也可以以點云的方式顯示感知周圍環(huán)境，在 rviz 中操作如下:
問題: 在rviz中顯示時錯位。
原因: 在kinect中圖像數(shù)據(jù)與點云數(shù)據(jù)使用了兩套坐標系統(tǒng)，且兩套坐標系統(tǒng)位姿并不一致。
解決:
1.在插件中為kinect設(shè)置坐標系，修改配置文件的<frameName>標簽內(nèi)容：
<frameName>support_depth</frameName>
2.發(fā)布新設(shè)置的坐標系到kinect連桿的坐標變換關(guān)系，在啟動rviz的launch中，添加:
<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />
3.啟動rviz，重新顯示。
示例結(jié)果：
Reference
http://www.autolabor.com.cn/book/ROSTutorials/di-2-zhang-ros-jia-gou-she-ji/23-fu-wu-tong-xin/224-fu-wu-tong-xin-zi-ding-yi-srv-diao-yong-b-python.html文章來源地址http://www.zghlxwxcb.cn/news/detail-740252.html
到了這里，關(guān)于【ROS學(xué)習筆記17】ROS常用仿真組件URDF集成Gazebo的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
