机器人系统仿真是通过计算机对机器人实体进行模拟的技术，基于ROS系统机器人仿真将涉及以下三个方面：创建机器人模型、搭建仿真环境、实现机器人与仿真环境之间的交互。在真实环境中，仅需要URDF结合Rviz可视化便可以将机器人的各项操作进行可视化，在仿真环境中，还需要使用Gazebo搭建仿真环境，其作用搭建一个虚拟的机器人运动环境，本文将对创建机器人模型做以总结。

创建机器人模型采用统一的机器人描述格式URDF语言，本案例将实现一款方形平板小车移动机器人。小车尺寸为3m*4m*0.5m由六个轮子，其中四个角为四个万向轮，几何中心线为两个驱动轮轴。

一、URDF基础总结

URDF只是一个文件，需要在Rviz或Gazebo中渲染成图形化的机器人模型，实现流程如下：

1、创建功能包

导入URDF，创建文件夹：存储urdf文件的目录（urdf），渲染机器人的文件（mesh），配置文件（config），存储launch启动文件（launch）

2、编写urdf文件

机器人由两个部分构成：link（连杆）和joint（关节），其中link表达的是机器人实体的部分，joint表达的是各连杆之间的关系。

整个urdf文件由以下四个标签表达：robot标签、link标签、joint标签以及Gazebo标签

（1）robot标签用于保持XML文件的语法完成性，表达这是一个机器人文件，其他标签必须包含在此标签之内。在该标签中可以通过name属性设置机器人模型的名称，其他标签都是它的子标签。

（2）link标签用于表达机器人的部件。

link标签用于描述机器人刚体部分的外观和物理属性，机器人的各个零件都对应一个link，在每一个部件的link标签内可以设计该部件的形状、尺寸、颜色、惯性矩以及碰撞参数等一系列属性。

属性name表示连杆的名称

子级标签geometry用于设置连杆的形状，该标签之下又有4个子级标签：
- 标签1: box(盒状)
  - 属性:size=长(x) 宽(y) 高(z)
- 标签2: cylinder(圆柱)
  - 属性:radius=半径 length=高度
- 标签3: sphere(球体)
  - 属性:radius=半径
- 标签4: mesh(为连杆添加皮肤)
  - 属性: filename=资源路径(格式:package://<packagename>/<path>/文件)

- origin 设置偏移量与倾斜弧度
  - 属性1: xyz=x偏移 y便宜 z偏移
  - 属性2: rpy=x翻滚 y俯仰 z偏航 (单位是弧度)
- metrial 设置材料属性(颜色)
  - 属性: name
  - 标签: color
    - 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
collision ---> 连杆的碰撞属性
Inertial ---> 连杆的惯性矩阵

（3）joint标签用于表达机器人各部件之间的关系。

1.属性

name ---> 为关节命名
type ---> 关节运动形式
- continuous: 旋转关节，可以绕单轴无限旋转
- revolute: 旋转关节，类似于 continues,但是有旋转角度限制
- prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
- planer: 平面关节，允许在平面正交方向上平移或旋转
- floating: 浮动关节，允许进行平移、旋转运动
- fixed: 固定关节，不允许运动的特殊关节

2.子标签

parent(必需的)

parent link的名字是一个强制的属性：
- link:父级连杆的名字，是这个link在机器人结构树中的名字。
child(必需的)

child link的名字是一个强制的属性：
- link:子级连杆的名字，是这个link在机器人结构树中的名字。
origin
- 属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
axis
- 属性: xyz用于设置围绕哪个关节轴运动。

（4）Gazebo标签用于配置仿真环境，比如材料属性、gazebo插件，该标签不是机器人模型所必须的，只有在仿真时才需要配置。

3、URDF调试工具

（1）check_urdf命令可以检查复杂的 urdf 文件是否存在语法问题，进入urdf文件所属目录，

调用方法:<check_urdf><urdf文件名>，如果不抛出异常，说明文件合法,否则非法

wy@eric:~/ws_sim/src/urdf01/urdf/urdf$ check_urdf demo05_test.urdf 
robot name is: mycar
---------- Successfully Parsed XML ---------------
root Link: base_footprint has 1 child(ren)
    child(1):  base_link
        child(1):  back_wheel
        child(2):  front_wheel
        child(3):  left_wheel
        child(4):  right_wheel

（2）urdf_to_graphiz 结构查看

进入urdf文件所属目录，调用方法:<urdf_to_graphiz ><urdf文件名>当前目录下会生成 pdf 文件,文件名称为：机器人名称.pdf

文件查看方法：<evince><机器人名称.pdf>

wy@eric:~/ws_sim/src/urdf01/urdf/urdf$ urdf_to_graphiz demo05_test.urdf 
Created file mycar.gv
Created file mycar.pdf
wy@eric:~/ws_sim/src/urdf01/urdf/urdf$ evince mycar.pdf

二、平板机器人搭建

1、构建参考系（base_footprint）

2、构建机器人底盘（base_link）

3、构建机器人驱动轮（right_Dwheel/left_Dwheel）

4、构建机器人万向轮（right_front_Uwheel/left_front_Uwheel /right_rear_Uwheel/left_rear_Uwheel）

模型建立：

<robot name="mycar">
    <!-- 设置 base_footprint  -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>

    <!-- 添加底盘 -->
<!-- 
        参数
            形状:正方体 
            长:0.4
            宽:0.3
            高:0.025
            离地：0.0125+0.025=0.0375

    -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.4 0.3 0.025" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="yellow">
                <color rgba="0.8 0.3 0.1 0.5" />
            </material>
        </visual>
    </link>

    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link"/>
        <origin xyz="0 0 0.0375" />
    </joint>

    <!-- 添加驱动轮 -->
<!-- 添加驱动轮 -->
    <!--
        驱动轮是侧翻的圆柱
        参数
            半径: 0.025
            宽度: 0.015
            颜色: 黑色
        关节设置:
            x = 0
            y = 底盘宽/2 + 轮胎宽度 / 2：（0.3+0.015/2=0.315/2=0.1575
            z = 离地间距 + 底盘长度 / 2 - 轮胎半径 = 1.5 + 4 - 3.25 = 2.25(cm)
            axis = 0 1 0
    -->
    <link name="left_Dwheel">
        <visual>
            <geometry>
                <cylinder radius="0.025" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="left_Dwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_Dwheel" />
        <origin xyz="0 0.1575 -0.0125" />
        <axis xyz="0 1 0" />
    </joint>


    <link name="right_Dwheel">
        <visual>
            <geometry>
                <cylinder radius="0.025" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="right_Dwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_Dwheel" />
        <origin xyz="0 -0.1575 -0.0125" />
        <axis xyz="0 1 0" />
    </joint>


    <!-- 添加万向轮(支撑轮) -->

<!-- 添加万向轮(支撑轮) -->
    <!--
        参数
            形状: 球体
            半径: 底盘的离地半径0.0375
            颜色: 黑色

        关节设置:
            x = 自定义(底盘半径 - 万向轮半径) = 0.2 - 0.02 = 0.18(cm)
            y = 0.15-0.0.02
            z = 底盘长度 / 2 + 离地间距 / 2 = 0.025 / 2 + 0.0375 / 2 = 0.03125 
            axis= 1 1 1

    -->
    <link name="left_front_Uwheel">
        <visual>
            <geometry>
                <sphere radius="0.01875" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="left_front_Uwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_front_Uwheel" />
        <origin xyz="0.18 0.13 -0.03125" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="right_front_Uwheel">
        <visual>
            <geometry>
                <sphere radius="0.01875" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="right_front_Uwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_front_Uwheel" />
        <origin xyz="0.18 -0.13 -0.03125" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="left_rear_Uwheel">
        <visual>
            <geometry>
                <sphere radius="0.01875" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="left_rear_Uwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_rear_Uwheel" />
        <origin xyz="-0.18 0.13 -0.03125" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="right_rear_Uwheel">
        <visual>
            <geometry>
                <sphere radius="0.01875" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="right_rear_Uwheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_rear_Uwheel" />
        <origin xyz="-0.18 -0.13 -0.03125" />
        <axis xyz="1 1 1" />
    </joint>

</robot>

配置文件：

<launch>
    <param name="robot_description" textfile="$(find mobile_robot_description)/urdf/urdf/robot_description.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mobile_robot_description)/config/robot_description.rviz" />
    <!-- 启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" /> -->

    <!-- 启动图形化的控制关节运动节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

三、控制机器人模型运动

1、控制器配置

# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
          #类型: 差速控制器:机器人的运动模型
       type: diff_controller,
       #参考坐标：视机器人模型而定
       base_frame_id: base_footprint, 
       #两个轮子之间的间距：在转弯的时候计算两轮的速率
       base_width: 0.315,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制：限制小车的加速度
       accel_limit: 1.0 
    }
}

2、启动文件配置

<launch>
    <param name="robot_description" textfile="$(find mobile_robot_description)/urdf/urdf/robot_description.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mobile_robot_description)/config/robot_description.rviz" />
    <!-- 启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
    <!-- 集成arbotix运动控制节点并加载参数 -->
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
    <!-- 启动arbotix_python中的arbotix节点 -->
        <rosparam file="$(find mobile_robot_description)/config/robot_control.yaml" command="load" />
        <param name="sim" value="true" />
        <!-- 驱动仿真机器人，因此要用sim -->
    </node>
</launch>

3、键盘控制

wy@eric:~$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py 

Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:	speed 0.5	turn 1.0 
wy@eric:~$

4、在终端发布消息控制运动

wy@eric:~$ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0"

5、编写运动发布节点控制小车底盘运动

/*
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
*/

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"motion_control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist msg;
    msg.linear.x = 1.0;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;

    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 2.0;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        pub.publish(msg);

        ros::spinOnce();
    }


    return 0;
}

7、编写节点控制小车按规律运动（文件控制小车运动）

8、存储系统运行的日志（对小车各参数的记录）