Ubuntu 18.04 ROS Melodic 多轮差速机器人 Gazebo仿真设计 1 URDF建模
1. 项目介绍
近期在做一个Gazebo机器人仿真项目,仿真对象是一台多轮差速机器人,名为MAP2。普通的差速机器人都是两轮,通过两轮的速度差控制机器人方向,多轮差速机器人可能有就有一些,其实这个机器人大概长下面这个样子。
不过这张图中只现实了机器人的一条腿,最后的成品预期机器人有四条腿,每条腿由一个双轮差速机器人组成,从而组成一个多轮差速机器人。也许你会想,对于这种类型的机器人,我在每条腿上装一个轮子,然后用伺服来控制轮子转向就行了,为什么要白费周折给每条腿上装上差速装置。这种设计的最主要原因是这个机器人设计的初衷是为建筑工地提供载货的移动设备,要求其有极高的负重能力。若采用单轮胎,大负重会对轮胎造成极大磨损,且伺服电机需要很大力矩来改变方向。而这两个设计缺点,可以通过使用双轮差速模型来解决。以上,为这个项目机器人的背景。
虽然之前已经陆陆续续做过不少ROS项目了,但其实并没有很熟悉。这篇文章将从建立工作站开始,记录我做这个项目的每一步,希望也能帮助到以后的学弟学妹。
1.1 本章内容
本文章主要介绍了多轮差速机器人MAP2的URDF模型与xacro模型,并实现了在Rivz与Gazebo平台上的机器人可视化。对应的控制器还没有开始写,所以目前机器人还不能移动,写机器人的控制器,让机器人动起来,这也是下一步的任务目标。
2. 准备工作
2.1 创建工作空间
在主文件夹下,创建一个名为 MAP2_ws
的文件夹,作为工作空间。然后在MAP2_ws
文件夹下,建立一个名为src
的文件夹。然后进入 src
文件夹,输入以下代码,以初始化工作空间。
catkin_init_workspace
2.2 创建硬件描述包
进入刚才创建的src
文件夹,然后输入以下代码从而创建硬件描述包。
roscreate-pkg MAP2_description urdf
然后终端中会跳出以上字样,提示中说明当前的工作路径不在
ROS_PACKAGE_PATH
上。但是往下做的时候目前好像还没有发现有影响。这个bug先马住,出了问题再会来看。至少现在,说明硬件描述包创建成功。
3.Rivz 模型可视化
3.1 创建URDF文件
打开MAP2_description
文件夹,在其中新建一个名为urdf
的文件夹,然后进入这个文件夹,在文件夹中新创建一个名为MAP2_prototype.urdf
的URDF文件。在该文件中输入以下代码。
<?xml version="1.0"?>
<!-- The name of the robot 'smartrcar' -->
<robot name="smartcar">
<!-- base_link, which is a box -->
<link name="map">
<visual>
<geometry>
<!-- box , length height width-->
<box size="0.25 .16 .05"/>
</geometry>
<!-- r p y, x y z, the change in positon-->
<origin rpy="0 0 1.57075" xyz="0 0 0.25"/>
<!-- The base is set to be blue-->
<material name="blue">
<color rgba="0 .5 .8 1"/>
</material>
</visual>
</link>
<link name="right_front_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<material name="white">
<!-- red green blue alpha-->
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="right_front_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="map"/>
<child link="right_front_leg"/>
<origin rpy="0 0 1.57075 " xyz="0.06 0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_front_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="right_front_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="right_front_leg"/>
<child link="right_front_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_front_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="right_front_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="right_front_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_front_subleg"/>
<child link="right_front_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_front_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_front_subleg"/>
<child link="right_front_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="right_back_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="map"/>
<child link="right_back_leg"/>
<origin rpy="0 0 1.57075 " xyz="0.06 -0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="right_back_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="right_back_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="right_back_leg"/>
<child link="right_back_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of right back wheels-->
<link name="right_back_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="right_back_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="right_back_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_back_subleg"/>
<child link="right_back_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_back_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_back_subleg"/>
<child link="right_back_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of right back wheels-->
<link name="left_front_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<material name="whilte">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="left_front_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="map"/>
<child link="left_front_leg"/>
<origin rpy="0 0 1.57075 " xyz="-0.06 0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_front_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<material name="whilte">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="left_front_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="left_front_leg"/>
<child link="left_front_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of left front wheels-->
<link name="left_front_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="left_front_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="left_front_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_front_subleg"/>
<child link="left_front_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_front_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_front_subleg"/>
<child link="left_front_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of left front wheels-->
<link name="left_back_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<material name="whilte">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="left_back_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="map"/>
<child link="left_back_leg"/>
<origin rpy="0 0 1.57075 " xyz="-0.06 -0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<link name="left_back_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<material name="whilte">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="left_back_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="left_back_leg"/>
<child link="left_back_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of left back wheels-->
<link name="left_back_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="left_back_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<joint name="left_back_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_back_subleg"/>
<child link="left_back_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_back_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_back_subleg"/>
<child link="left_back_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- a pair of left back wheels-->
<link name="head">
<visual>
<geometry>
<box size=".02 .03 .03"/>
</geometry>
<material name="whilte">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="map"/>
<child link="head"/>
<origin xyz="0 0.08 0.275"/>
</joint>
</robot>
3.2 URDF 解析
URDF的教程网上很多,在这儿就不详细讲了,有时间回来更这一块。
3.3 创建Launch文件
回到MAP2_description
文件夹,新建一个launch
文件夹。进入launch
文件夹,新建一个MAP2_prototype.launch
文件。在文件中输入以下代码。
<?xml version="1.0"?>
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<!-- param是要加载ros的一个参数:smartcar_description (描述机器人的具体模型) 参数的具体内容是.urdf文件 ,将路径下的.urdf文件内容让到robot_description -->
<param name="robot_description" textfile="$(find MAP2_description)/urdf/MAP2_prototype.urdf" />
<!-- 设置GUI参数,显示关节控制插件 -->
<param name="use_gui" value="true"/>
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 ,而且可以通过UI界面对joint进行控制 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点,发布tf ,整理成三维姿态信息发布 -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- 运行rviz可视化界面 通过 args加载参数 .rviz文件 作用:默认打开rviz没有任何显示插件 通过保存配置文件的形式保存设置好的插件 保存在config下 配置文件是可以通过rviz保存得到的 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
3.4 运行Rivz
回到MAP2_ws
文件夹下,输入以下代码从而运行文件。
source devel/setup.bash
roslaunch MAP2_description MAP2_prototype.launch
然后我们就可以在Rivz的界面,但此时我们还看不到模型。在Rivz界面左下角点击add,然后点击RobotModel,就可以载入模型。如果现实的模型是纯白色,在左上角框的Global Options中的Fixed Frame里,把Frame改为map,就可以看到模型全貌。同时,控制面板也会随Rivz显示,12个bar可以改变四条腿与八个轮子的转动角度。
4. Gazebo 模型导入
现在,我们要尝试把建立好的模型导入Gazebo当中。Gazebo方针控制好像不支持普通的URDF文件,需要导入的文件类型为xacro。两种文件格式十分相思,相比URDF,xacro多了一些Plugin参数。以下为生成步骤。
4.1 创建xacro文件
首先打开urdf
文件夹,创建一个名为MAP2_move_robot.xacro
的文件,输入以下代码。
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
name="robot1_xacro">
<xacro:property name="length_wheel" value="0.05" />
<xacro:property name="radius_wheel" value="0.05" />
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0" />
</inertial>
</xacro:macro>
<link name="base_footprint">
<visual>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<xacro:default_inertial mass="0.0001"/>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" />
<parent link="base_footprint" />
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
<origin rpy="0 0 1.57075" xyz="0 0 0.25"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.25 .16 .05"/>
</geometry>
</collision>
<xacro:default_inertial mass="10"/>
</link>
<link name="right_front_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_back_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_front_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_back_leg">
<visual>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.01" length="0.15"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_front_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_back_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_front_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_back_subleg">
<visual>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.075 0.02 0.02"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_front_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_front_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_back_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="right_back_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_front_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_front_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_back_wheel_right">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="left_back_wheel_left">
<visual>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.035" length="0.01"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<joint name="right_front_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="right_front_leg"/>
<child link="right_front_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_back_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="right_back_leg"/>
<child link="right_back_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_front_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="left_front_leg"/>
<child link="left_front_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_back_subleg_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="left_back_leg"/>
<child link="left_back_subleg"/>
<origin rpy="0 0 1.57075 " xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- WHEELS-->
<joint name="right_front_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_front_subleg"/>
<child link="right_front_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_front_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_front_subleg"/>
<child link="right_front_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_back_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_back_subleg"/>
<child link="right_back_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_back_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="right_back_subleg"/>
<child link="right_back_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_front_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_front_subleg"/>
<child link="left_front_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_front_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_front_subleg"/>
<child link="left_front_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_back_wheel_right_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_back_subleg"/>
<child link="left_back_wheel_right"/>
<origin rpy="0 1.57075 0 " xyz="-0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_back_wheel_left_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="left_back_subleg"/>
<child link="left_back_wheel_left"/>
<origin rpy="0 1.57075 0 " xyz="0.05 0 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- WHEELS-->
<joint name="right_front_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_front_leg"/>
<origin rpy="0 0 1.57075 " xyz="0.06 0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="right_back_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="right_back_leg"/>
<origin rpy="0 0 1.57075 " xyz="0.06 -0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_front_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_front_leg"/>
<origin rpy="0 0 1.57075 " xyz="-0.06 0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<joint name="left_back_leg_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_back_leg"/>
<origin rpy="0 0 1.57075 " xyz="-0.06 -0.1 0.135"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<!-- Drive controller -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace></robotNamespace>
<leftFrontJoint>left_front_wheel_left_joint</leftFrontJoint>
<leftFrontJoint>left_front_wheel_right_joint</leftFrontJoint>
<rightFrontJoint>right_front_wheel_left_joint</rightFrontJoint>
<rightFrontJoint>right_front_wheel_right_joint</rightFrontJoint>
<leftRearJoint>left_back_wheel_left_joint</leftRearJoint>
<leftRearJoint>left_back_wheel_right_joint</leftRearJoint>
<rightRearJoint>right_back_wheel_left_joint</rightRearJoint>
<rightRearJoint>right_back_wheel_right_joint</rightRearJoint>
<wheelSeparation>4</wheelSeparation>
<wheelDiameter>0.1</wheelDiameter>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_link</robotBaseFrame>
<odometryFrame>odom</odometryFrame>
<torque>1</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>1</broadcastTF>
</plugin>
</gazebo>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/robot</robotNamespace>
</plugin>
</gazebo>
</robot>
4.2 创建Launch文件
打开launch
,创建MAP2_start_to_move.launch
,输入以下代码:
<?xml version="1.0"?>
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find MAP2_description)/urdf/MAP2_move_robot.xacro'" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model robot1 -param robot_description -z 0.05"/>
</launch>
4.3 运行Gazebo
运行以下代码,就能打开Gazebo,并且看到做好的机器人出现在仿真平台了。
source devel/setup.bash
roslaunch MAP2_description MAP2_start_to_move.launch
使用RotationMode功能可以看到在模拟环境中机器人以及有了物理性能,但现在对应的控制器还没有写好,所以机器人还不能动。下一章将讲解如何编写Gazebo仿真平台下的控制器。