文章目錄
- 前言
- 一、建立編譯功能包
- 二、xacro檔案
-
- 1、建立robot_base.xacro檔案
- 2、解釋robot_base.xacro檔案
- 3、建立robot_camera.xacro檔案
- 4、解釋robot_camera.xacro檔案
- 5、建立robot_lidar.xacro檔案
- 6、解釋robot_lidar.xacro檔案
- 7、建立robot.xacro檔案
- 8、解釋robot.xacro檔案
- 三、launch啟動檔案
-
- 1、建立robot.launch檔案
- 2、運作robot.launch
- 四、rviz顯示
- 總結
前言
首先,上一篇使用urdf檔案建立了一個四輪小車模型。這一篇想使用xacro檔案建立一個正真的機器人模型。
xacro比urdf更具可讀性和可維護性。它還允許我們建立模型并複用這些模型去建立相同的結構。在裡面我們可以進行宏定義、變量定義、頭檔案包含、數學運算等操作。
我們确定下機器人的形态:一個帶有攝像頭、雷射雷達的四輪移動機器人,其中兩個輪子是帶有電機的差動輪,另外兩個為支撐輪。我們需要确定的參數如下:機器人主體的尺寸、顔色、角度,機器人輪子(包括joint與link)的尺寸,顔色、角度,機器人攝像頭的尺寸、顔色、角度等。
一、建立編譯功能包
切換到catkin_ws/src目錄下,如下:
catkin_create_pkg mbot_sim urdf xacro
切換到catkin_ws目錄下,編譯該功能包,如下:
catkin_make mbot_sim
在該功能包下建立include、src、launch、urdf檔案夾,在urdf檔案下建立urdf檔案夾和xacro檔案夾。
二、xacro檔案
1、建立robot_base.xacro檔案
切換到xacro檔案下,建立robot_base檔案,檔案内容如下:
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- PROPERTY LIST -->
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_length" value="0.025"/>
<xacro:property name="wheel_joint_y" value="0.19"/>
<xacro:property name="wheel_joint_z" value="0.05"/>
<!-- -->
<xacro:property name="caster_radius" value="0.015"/>
<!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
<xacro:property name="caster_joint_x" value="0.18"/>
<!-- Defining the colors used in this robot -->
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
<material name="gray">
<color rgba="0.75 0.75 0.75 1"/>
</material>
<!-- Macro for robot wheel -->
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint " type="continuous">
<origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="gray" />
</visual>
</link>
</xacro:macro>
<!-- Macro for robot caster -->
<xacro:macro name="caster" params="prefix reflect">
<joint name="${prefix}_caster_joint" type="continuous">
<origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
<material name="black" />
</visual>
</link>
</xacro:macro>
<!-- Macro for robot base -->
<xacro:macro name="mbot_base">
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
</link>
<wheel prefix="left" reflect="-1"/>
<wheel prefix="right" reflect="1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
</xacro:macro>
</robot>
2、解釋robot_base.xacro檔案
檔案的前兩行是每個xacro檔案必須包含的。
接下來,我們定義機器人經常用到的值,使用變量進行定義,主要包括常用的參數和機器人屬性,比如機器人主體圓柱體的半徑大小、高度大小,差動輪圓柱體的半徑大小、高度大小以及支撐輪圓柱體的半徑大小、高度大小;
然後定義yellow、black、gray三種顔色;
通過宏定義機器人差動輪和支撐輪,友善代碼複用。
最後定義機器人主體。
3、建立robot_camera.xacro檔案
在xacro檔案下,建立robot_camera.xacro檔案,檔案内容如下:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<link name="${prefix}_link">
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>
</link>
</xacro:macro>
</robot>
4、解釋robot_camera.xacro檔案
目前,隻是定義了攝像頭的外觀,攝像頭屬性通過插件來進行定義的。
5、建立robot_lidar.xacro檔案
在xacro檔案下,建立robot_lidar.xacro檔案,檔案内容如下:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="lidar">
<xacro:macro name="hokuyo_lidar" params="prefix:=lidar">
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://mbot_sim/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
</xacro:macro>
</robot>
6、解釋robot_lidar.xacro檔案
定義了日本北洋hokuyo雷射雷達,可以看到其中使用了hokuyo.dae檔案。需要将usr/share/gazebo-9/models/hokuyo/目錄中的meshes檔案夾整體複制到我們的功能包mbot_sim下面。
7、建立robot.xacro檔案
在xacro檔案下,建立robot.xacro檔案,檔案内容如下:
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_sim)/urdf/xacro/robot_base.xacro" />
<xacro:include filename="$(find mbot_sim)/urdf/xacro/robot_camera.xacro" />
<xacro:include filename="$(find mbot_sim)/urdf/xacro/robot_lidar.xacro" />
<xacro:property name="camera_offset_x" value="0.17" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.10" />
<xacro:property name="lidar_offset_x" value="-0.17" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.10" />
<mbot_base/>
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="lidar_link"/>
</joint>
<xacro:hokuyo_lidar prefix="lidar"/>
</robot>
8、解釋robot.xacro檔案
該檔案直接調用了前面兩個檔案中定義的宏,對機器人進行顯示設定,包含主體、輪子、攝像頭和雷射雷達。
三、launch啟動檔案
1、建立robot.launch檔案
在launch檔案下,建立robot.launch檔案,檔案内容如下:
<launch>
<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_sim)/urdf/xacro/robot.xacro'" />
<arg name="gui" default="true" />
<param name="robot_description" command="$(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_sim)/cfg/robot.rviz" required="true" />
</launch>
2、運作robot.launch
指令如下:
roslaunch mbot_sim robot.launch
效果如下:
四、rviz顯示
同上一篇介紹,添加RobotModel後,效果如下:
拖動右側插件的滾動條,可以看到輪子的轉動。
總結
該篇建立了一個移動機器人模型,添加了雷射雷達,添加了攝像頭,但攝像頭隻有外觀沒有設定屬性,沒有添加實體屬性和碰撞屬性。下一篇我們添加這些屬性并嘗試利用gazebo實作移動機器人在ROS系統中的仿真。