搭建机器人模型进行slam过程记录

于 2022-12-08 10:07:19 发布
阅读量1k 收藏 9
点赞数 4
CC 4.0 BY-SA版权
文章标签： 机器人 ubuntu
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/magic_invaid/article/details/128231526

有需要的小伙伴可以在评论区留言获取完整的工作空间代码包

环境：ubuntu18.04， ROS melodic

机器人底盘URDF文件编写

首先在工作空间目录下创建一个功能包，用来放置机器人模型，以及对机器人模型进行可视化操作

catkin_creating_pkg mrobot_description urdf xacro

该功能包需要 urdf 和 xacro 这两个依赖

进入 mrobot_description 文件夹并创建一个 urdf 文件夹，用来放置机器人的模型文件

下面进入对机器人的urdf文件的编写。URDF即 United Robot Description Format，统一机器人描述格式，这是ROS中重要的机器人模型描述格式。完整代码如下所示，对于重要的部分，我进行了注释。

<?xml version="1.0" ?>
<robot name="mrobot_chassis">
<!--声明该文件使用XML描述，使用<robot>根标签，下面的内容就是对机器人模型的详细描述,机器人是由link和joint组成的-->
    <link name="base_link">
        <visual><!--visual子标签用来定义该部分的外观属性，根据该部分的内容，可以在rviz和gazebo中显示出来-->
            <origin xyz=" 0 0 0" rpy="0 0 0" /><!--定义起始位姿-->
            <geometry><!--用圆柱体显示-->
                <cylinder length="0.028" radius="0.13"/>
            </geometry>
            <material name="orange">
                <color rgba="1 0.4 0 1"/><!--这四个参数分别为该颜色的rgb值和透明度-->
            </material>
        </visual>
    </link>

    <joint name="base_left_motor_joint" type="fixed"><!--这里关节的类型是固定-->
        <origin xyz="-0.055 0.075 0" rpy="0 0 0" /> <!--相对于parent link的位姿-->       
        <parent link="base_link"/><!--joint用来定义两个link之间的运动关系-->
        <child link="left_motor" />
    </joint>

    <link name="left_motor">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" /><!--沿roll轴即x轴旋转pi/2-->
            <geometry>
                <cylinder radius="0.02" length = "0.08"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="left_wheel_joint" type="continuous"><!--这里关节的类型是转动-->
        <origin xyz="0 0.0485 0" rpy="0 0 0"/>
        <parent link="left_motor"/>
        <child link="left_wheel_link"/>
        <axis xyz="0 1 0"/><!--这里表示转轴是y轴-->
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.033" length = "0.017"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
    </link>
<!--下面是关于右轮的部分，和左轮对称-->
    <joint name="base_right_motor_joint" type="fixed">
        <origin xyz="-0.055 -0.075 0" rpy="0 0 0" />        
        <parent link="base_link"/>
        <child link="right_motor" />
    </joint>

    <link name="right_motor">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.02" length = "0.08" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="0 -0.0485 0" rpy="0 0 0"/>
        <parent link="right_motor"/>
        <child link="right_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.033" length = "0.017"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
    </link>
<!--这里是一个支撑轮-->
    <joint name="front_caster_joint" type="fixed">
        <origin xyz="0.1135 0 -0.0165" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_caster_link"/>
    </joint>

    <link name="front_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry><!--这里用球体显示-->
                <sphere radius="0.0165" />
            </geometry>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
    </link>

</robot>

完成了底盘urdf文件的编写之后，我们可以使用两个小工具对编写的正确性进行检验

通过命令check_urdf mrobot_chassis.urdf可以check语法错误，编写正确的运行结果如下所示：

 

还可以通过命令urdf_to_graphiz mrobot_chassis.urdf查看urdf文件的整体结构：

 

urdf文件编写没有问题的话，下面我们将机器人模型在rviz中进行可视化。

还在刚刚的 mrobot_desription文件夹下，创建一个 launch文件夹，编写一个 display_mrobot_chassis_urdf.launch文件

<launch>
    <param name="robot_description" textfile="$(find mrobot_description)/urdf/mrobot_chassis.urdf"/>
    <param name="use_gui" value="true"/>
    <!--发布机器人的关节状态-->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
    <!--发布机器人的tf关系-->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot_urdf.rviz" required="true"/>
</launch>

在工作空间下通过catkin_make 命令进行编译，在工作空间路径下执行以下命令

source ./devel/setup.bash

roslaunch mrobot_description display_mrobot_chassis_urdf.launch

可以看到机器人的模型成功展示出来了。

现在仅仅是将机器人的外形展示了出来，对于仿真是不足够的，还需要机器人的质量、惯性张量、碰撞体积等参数，下面以base_link 为例加入<inertial>和<collision>标签

    <link name="base_link">
        <visual>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.028" radius="0.13"/>
            </geometry>
            <material name="orange">
                <color rgba="1 0.4 0 1"/>
            </material>
        </visual>

        <inertial>
            <mass value="2"/>
            <origin xyz="0 0 0"/>
            <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                      iyy="0.01" iyz="0.0" izz="0.5"/>
        </inertial>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <cylinder length="0.028" radius="0.13"/>
            </geometry>
        </collision>
    </link>

需要强调的是惯性张量可以通过solidworks等三维建模软件进行计算得到，碰撞体积是对于外形的简化，以减少计算量，这里机器人本身的模型已经比较简单了，碰撞模型和本身模型进行了重叠。

机器人底盘XACRO文件编写

xacro是另一种精简化、可复用、模块化的描述形式，可以建立宏定义，也支持一些可编程接口，比如常量、变量、数学公式、条件语句等。

下面是机器人底盘的xacro文件。

<?xml version="1.0"?>
<robot xmlns:xacro="https://www.ros.org/wiki/xacro">
    
    <xacro:property name="M_PI" value="3.14159"/>
    <xacro:property name="wheel_radius" value="0.033"/>
    <xacro:property name="wheel_length" value="0.017"/>
    <xacro:property name="base_link_radius" value="0.13"/>
    <xacro:property name="base_link_length" value="0.028"/>
    <xacro:property name="motor_radius" value="0.02"/>
    <xacro:property name="motor_length" value="0.08"/>
    <xacro:property name="motor_x" value="-0.055"/>
    <xacro:property name="motor_y" value="0.075"/>


    <xacro:macro name="mrobot_body">
        <material name="orange">
            <color rgba="1 0.4 0 1"/>
        </material>
        <material name="black">
            <color rgba="0 0 0 1"/>
        </material>
        <material name="white">
            <color rgba="1 1 1 1"/>
        </material>

        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
            </visual>
        </link>

        <link name="base_link">
            <inertial>
                <mass value="2"/>
                <origin xyz="0 0 0.0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                        iyy="0.01" iyz="0.0" izz="0.5" />
            </inertial>            
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
                </geometry>
                <material name="orange" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
                </geometry>
            </collision>
        </link>
        
        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>

        <link name="left_motor">
            <inertial>
                <origin xyz="0.0 0 0"/>
                <mass value="0.1"/>
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                        iyy="0.001" iyz="0.0" izz="0.001"/>
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
                <geometry>
                    <cylinder radius="${motor_radius}" length="${motor_length}"/>
                </geometry>
                <material name="black"/>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${motor_radius}" length = "${motor_length}"/>
                </geometry>
            </collision>
        </link>

        <joint name="base_left_motor_joint" type="fixed">
            <origin xyz="${motor_x} ${motor_y} 0" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="left_motor"/>
        </joint>

        <link name="left_wheel_link">
            <inertial>
                <origin xyz="0 0 0"/>
                <mass value="0.01" />
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                        iyy="0.001" iyz="0.0" izz="0.001" />
            </inertial>    
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <material name="white"/>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
            </collision>        
        </link>

        <joint name="left_wheel_joint" type="continuous">
            <origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>
            <parent link="left_motor"/>
            <child link="left_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="right_motor">
            <inertial>
                <origin xyz="0.0 0 0"/>
                <mass value="0.1"/>
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                        iyy="0.001" iyz="0.0" izz="0.001"/>
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
                <geometry>
                    <cylinder radius="${motor_radius}" length="${motor_length}"/>
                </geometry>
                <material name="black"/>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${motor_radius}" length = "${motor_length}"/>
                </geometry>
            </collision>
        </link>
        
        <joint name="base_right_motor_joint" type="fixed">
            <origin xyz="${motor_x} -${motor_y} 0" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="right_motor"/>
        </joint>

        <link name="right_wheel_link">
            <inertial>
                <origin xyz="0 0 0"/>
                <mass value="0.01" />
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                        iyy="0.001" iyz="0.0" izz="0.001" />
            </inertial>    
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <material name="white"/>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
            </collision>        
        </link>

        <joint name="right_wheel_joint" type="continuous">
            <origin xyz="0 -${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>
            <parent link="right_motor"/>
            <child link="right_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="front_caster_link">
            <inertial>
                <origin xyz="0 0 0"/>
                <mass value="0.001" />
                <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
                            iyy="0.0001" iyz="0.0" izz="0.0001" />
            </inertial>

            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
                <geometry>
                    <sphere radius="${wheel_radius/2}" />
                </geometry>
                <material name="black" />
            </visual>

            <collision>
                <origin xyz="0 0 0.01" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <sphere radius="${wheel_radius/2}" />
                </geometry>
            </collision>
        </link>    
        
        <joint name="front_caster_joint" type="fixed">
            <origin xyz="${base_link_radius-wheel_radius/2} 0 -${wheel_radius/2}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="front_caster_link"/>
        </joint>
    </xacro:macro>
</robot>

xacro 文件与URDF文件很相似，类似于<xacro:property name="motor_y" value="0.075"/>，是xacro中定义的常量，需要提到的是，在xacro文件中根link并不是base_link而是base_footprint，引入base_footprint主要是解决机器人模型会有一部分处在rviz中的grid之下，引入base_footprint可以方便的更改机器人本体的位置。

如需改变机器人某个link的尺寸，直接修改前面的常量即可，简洁了许多，这里xacro文件中直接定义了一个宏定义<xacro:macro name="mrobot_body">，我们可以再编写一个xacro文件引用该文件，并直接使用该宏定义，增强了代码的复用性，而且后期完成slam任务时，我们还需要摄像头或者雷达等传感器，这样只需要更改上层的xacro文件即可，结构清晰。

下面来创建另一个xacro文件来引用 mrobot_body.urdf.xacro.

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="https://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find mrobot_description)/xacro/mrobot_body.urdf.xacro"/>
    <mrobot_body/>
</robot>

可以看到，这套代码相当简洁。

下面再编写一个launch文件对xacro文件的机器人模型在rviz中进行可视化。

<launch>
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/xacro/mrobot.urdf.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" pkg="joint_state_publisher" type="joint_state_publisher"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot.rviz" required="true"/>
</launch>

下面是运行结果。

 

通过ArbotiX和rviz控制机器人运动

ArbotiX是一款控制电机、舵机的控制板，并提供相应的ROS功能包，这个功能包不仅能驱动真实的ArbotiX控制板，它还提供相应的ROS功能包，这个功能包提供了一个差速控制器，通过接收速度控制指令更新机器人的joint状态，即通过监听/cmd_vel话题，将话题信息转化为joint的转动。

首先进行ArbotiX的安装，通过命令git clone GitHub - vanadiumlabs/arbotix_ros: Arbotix ROS drivers repository进行clone，由于网络限制的问题，可能会失败，可以使用科学上网的方式，down在windows中再copy到虚拟机中，目前（2022/12）github托管的功能包是适配ROS-Noetic版本，需要进行更改，使之适配于melodic版本。

melodic中默认python为2.7版本的，找到图中的py文件，将第一行的 #!/usr/bin/env python3 改为

#!/usr/bin/env python，然后在工作空间下进行catkin_make编译

 

接下来进行ArbotiX控制器的配置。

首先在mrobot_description文件下创建一个config文件夹，创建一个fake_mrobot_arbotix.yaml文件

controllers: {
  base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }  
}

注意yaml文件的语法，每个冒号后面都要跟一个空格。

接下来创建一个launch文件

<launch>
    <param name="/use_sim_time" value="false"/>

    <arg name="model" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/xacro/mrobot.urdf.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" pkg="joint_state_publisher" type="joint_state_publisher"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
        <param name="publish_frequency" type="double" value="20.0"/>
    </node>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot.rviz" required="true"/>
    <node name="arbotix_ros" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find mrobot_description)/config/fake_mrobot_arbotix.yaml" command="load"/>
        <param name="sim" value="true"/>
    </node> 
</launch>

这个launch文件主要多了一个控制器节点。再完成这两部分之后，我们还需要对之前的xacro文件修改，因为控制器的作用是监听/cmd_vel话题，将信息更新到joint上，因此模型的joint和控制器joint名称应统一，把xacro中的 left_wheel_joint 和 right_wheel_joint 改为 base_l_wheel_link 和 base_r_wheel_link.

最后使用键盘控制节点，控制机器人在rviz中运动

通过命令sudo apt-get install ros-melodic-teleop-twist-board安装键盘控制节点

roslaunch mrobot_description arbotix_mrobot.launch

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

运行结果如下，通过键盘即可控制机器人在rviz中运动。

Gazebo运动仿真

首先在gazebo中建立一个仿真环境，通过命令

roscore

rosrun gazebo_ros gazebo

打开gazebo软件，如果出现如下所示的错误，一般是由于虚拟机的显卡加速设置导致的。

gedit ~/.bashrc

在末尾加入 export SVGA_VGPU10=0 从而禁用显卡加速。

在工作空间目录下创建一个新的包 用于gazebo仿真

catkin_creat_pkg mrobot_gazebo roscpp gazebo_ros gazebo_plugins gazebo_ros_control

该功能包依赖上面几个包，请确认这些功能包安装完整。

在 mrobot_gazebo下创建worlds文件夹，再通过前述命令打开gazebo，创建一个简单的仿真环境。

如果在gazebo运行过程中，报错

将~/.ignition/fuel/config.yaml中的 https://api.ignitionfuel.org 改为 https://api.ignitionrobotics.org即可

最终创建的实验环境如下图所示，保存至 mrobot_gazebo/worlds/my_world.world

下面通过launch文件启动gazebo并生成实验环境。

在 mrobot_gazebo/launch下创建view_mrobot_gazebo.launch

<launch>
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/my_world.world"/>
    <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="$(arg world_name)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>
</launch>

这其中的大部分参数都很好理解，这里主要说两个参数，paused：在暂停状态打开gazebo；headless：gazebo的日志记录。

通过命令roslaunch mrobot_gazebo view_mrobot_gazebo.launch,运行结果如下所示：

实验环境建立完毕，在gazebo中进行仿真，还进行对机器人模型的xacro文件的完善，将之前的完成的两个xacro文件粘贴到如下图所示的位置。

mrobot.urdf.xacro无需修改，下面对mrobot_body.urdf.xacro文件进行修改，完整的文件如下所示：

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- Defining the colors used in this robot -->
    <material name="Black">
        <color rgba="0 0 0 1"/>
    </material>

    <material name="White">
        <color rgba="1 1 1 1"/>
    </material>

    <material name="Orange">
        <color rgba="1 0.4 0 1"/>
    </material>
    <!-- PROPERTY LIST -->
    <!--All units in m-kg-s-radians unit system -->
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- Main body length, width, height and mass -->
    <xacro:property name="base_mass"        value="0.5" /> 
    <xacro:property name="base_link_radius" value="0.13"/>
    <xacro:property name="base_link_length" value="0.028"/>

    <xacro:property name="motor_x" value="-0.055"/>
    <xacro:property name="motor_y" value="0.075"/>
    <!-- Caster radius and mass -->
    <xacro:property name="caster_radius"          value="0.016" /> 
    <xacro:property name="caster_mass"            value="0.01" /> 
    <xacro:property name="caster_joint_origin_x"  value="-0.12" />

    <!-- Wheel radius, height and mass -->
    <xacro:property name="wheel_radius" value="0.033" /> 
    <xacro:property name="wheel_height" value="0.017" />
    <xacro:property name="wheel_mass"   value="0.1" />
    
    <!-- Wheel radius, height and mass -->
    <xacro:property name="motor_radius" value="0.02" /> 
    <xacro:property name="motor_height" value="0.08" />
    <xacro:property name="motor_mass"   value="0.1" />


    <!-- 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>

    <!-- Macro for wheel joint -->
    <xacro:macro name="wheel" params="lr translateY">
        <!-- lr: left, right -->
        <link name="wheel_${lr}_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0  0 " /> 
                <geometry>
                    <cylinder length="${wheel_height}" radius="${wheel_radius}" />
                </geometry>
                <material name="Black" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0 " />
                <geometry>
                    <cylinder length="${wheel_height}" radius="${wheel_radius}" />
                </geometry>
            </collision>
            <cylinder_inertial_matrix  m="${wheel_mass}" r="${wheel_radius}" h="${wheel_height}" />
        </link>

        <gazebo reference="wheel_${lr}_link">
            <material>Gazebo/White</material>
        </gazebo>

        <link name="motor_${lr}_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0  0 " /> 
                <geometry>
                    <cylinder length="${motor_height}" radius="${motor_radius}" />
                </geometry>
                <material name="Black" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0 " />
                <geometry>
                    <cylinder length="${motor_height}" radius="${motor_radius}" />
                </geometry>
            </collision>
            <cylinder_inertial_matrix  m="${motor_mass}" r="${motor_radius}" h="${motor_height}" />
        </link>
        <gazebo reference="motor_${lr}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <joint name="base_to_wheel_${lr}_joint" type="continuous">
            <parent link="motor_${lr}_link"/>
            <child link="wheel_${lr}_link"/>
            <origin xyz="0 ${-1*translateY * (motor_height+wheel_height)/2} 0" rpy="0 0 0" /> 
            <axis xyz="0 1 0" rpy="0  0" />
        </joint>

        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="wheel_${lr}_joint_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="base_to_wheel_${lr}_joint" />
            <actuator name="wheel_${lr}_joint_motor">
                <hardwareInterface>VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
        
        <joint name="base_to_motor_${lr}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="motor_${lr}_link"/>
            <origin xyz="${motor_x} ${-1*translateY *motor_y} 0" rpy="0 0 0" /> 
        </joint>
    </xacro:macro>

    <!-- Macro for caster joint -->
    <xacro:macro name="caster" params="fb translateX">
        <!-- fb: front, back -->
        <link name="${fb}_caster_link">
            <visual>
                <origin xyz="0 0 0 " rpy="0 0 0" /> 
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="Black" />
            </visual>  
            <collision>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <origin xyz="0 0 0 " rpy="0 0 0" /> 
            </collision>      
            <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
        </link>

        <gazebo reference="${fb}_caster_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <joint name="base_to_${fb}_caster_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${fb}_caster_link"/>
            <origin xyz="${translateX*caster_joint_origin_x} 0 ${-caster_radius}" rpy="0 0 0"/>
        </joint>
    </xacro:macro>



    <!-- BASE-FOOTPRINT -->
    <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->
    <xacro:macro name="mrobot_body">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0" />
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <!-- BASE-LINK -->
        <!--Actual body/chassis of the robot-->
        <link name="base_link">
            <cylinder_inertial_matrix  m="${base_mass}" r="${base_link_radius}" h="${base_link_length}" />
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>

            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
                </geometry>
            </collision>   
        </link>
        <gazebo reference="base_link">
            <material>Gazebo/Orange</material>
        </gazebo>

        <!-- Wheel Definitions -->
        <wheel lr="right"  translateY="1" />
        <wheel lr="left"  translateY="-1" />

        <!-- Casters Definitions -->
        <caster fb="front"  translateX="-1" />



        <!-- controller -->
        <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>base_to_wheel_left_joint</leftJoint>
                <rightJoint>base_to_wheel_right_joint</rightJoint>
                <wheelSeparation>${base_link_radius*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>1</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
		<publishOdomTF>true</publishOdomTF>
		<odometrySource>world</odometrySource>
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
        </gazebo> 

    </xacro:macro>
</robot>

这个文件与之前的xacro主要不同之处：

（1）每个link都多了<gazebo>标签，使之能够在gazebo正确显示自身的颜色。

（2）对于转动的joint添加了<transmission>标签，使之弄够在gazebo中正确的运动。

（3）文件末尾对于整个机器人多了<gazebo>标签，添加了差分控制器以及一些所必须的参数。

修改view_mrobot_gazebo.launch文件，在gazeb中生成环境的同时生成机器人模型。

<launch>
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/my_world.world"/>
    <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="$(arg world_name)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>
    
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/xacro/mrobot.urdf.xacro'"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
        <param name="publisher_frequency" type="double" value="50.0"/>
    </node>
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model mrobot -param robot_description"/>

</launch>

这个launch文件和之前的很类似，多了最后一个node，这个node就是在gazebo中生成机器人模型文件。

roslaunch mrobot_gazebo view_mrobot_gazebo.launch

运行结果如下所示。

再次启动键盘控制节点 rosrun teleop_twist_keyboard teleop_twist_keyboard.py

成功通过键盘控制机器人在gazebo仿真环境中移动。

Gazebo中的slam与导航仿真

进行slam，首先应该对机器人模型添加相应的传感器，我们采用激光雷达进行slam，对机器人添加激光雷达的模型。编写一个激光雷达的xacro文件。

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">

    <xacro:macro name="rplidar" params="prefix:=laser">
        <!-- Create laser reference frame -->
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
                <material name="black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.06" radius="0.05"/>
                </geometry>
            </collision>
        </link>
        <gazebo reference="${prefix}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <sensor type="ray" name="rplidar">
                <pose>0 0 0 0 0 0</pose>
                <visualize>false</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>6.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_link</frameName>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

其中比较重要的部分就是文件末尾的<gazebo>标签，激光雷达的传感器类型是ray，rplidar的相关参数在产品手册中可以找到。为了获取尽量贴近真实的仿真效果，需要根据实际参数配置<ray>中的雷达参数：360°检测范围、单圈360个采样点，5.5Hz采样频率，最远6m检测范围等。最后使用<plugin>标签添加激光雷达插件libgazebo_ros_laser.so，所发布的激光雷达话题是/scan

下面再编写一个带有雷达的机器人模型的xacro文件。

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find mrobot_gazebo)/xacro/mrobot_body.urdf.xacro"/>
    <xacro:include filename="$(find mrobot_gazebo)/xacro/rplidar.xacro"/>

    <xacro:property name="rplidar_offset_x" value="0"/>
    <xacro:property name="rplidar_offset_y" value="0"/>
    <xacro:property name="rplidar_offset_z" value="0.039"/>

    <mrobot_body/>
    <joint name="rplidar_joint" type="fixed">
        <origin xyz="${rplidar_offset_x} ${rplidar_offset_y} ${rplidar_offset_z}" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>
    <xacro:rplidar prefix="laser"/>
</robot>

该文件的内容主要就是包括了原有的机器人模型和雷达的模型，定义了雷达的相关参数以及确定了雷达相对于机器人的位置，即确定了rplidar_joint.然后我们再编写一个launch文件，在gazebo中启动带有雷达的机器人。

<launch>
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/my_world.world"/>
    <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="$(arg world_name)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>
    
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/xacro/mrobot_with_rplidar.urdf.xacro'"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
        <param name="publisher_frequency" type="double" value="50.0"/>
    </node>
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model mrobot -param robot_description"/>

</launch>

这个launch文件和之前在gazebo中启动机器人的launch非常相似，唯一不同的点就是包含的机器人模型不同。启动后的运行效果如下所示：

通过命令rostopic list,可以看到/scan话题

再通过命令rosrun rviz rviz启动rviz，添加LaserScan，话题选择/scan

可以看到激光雷达的点云信息都显示在了rviz界面中

下面通过开源的gmapping功能包进行slam。关于gmapping算法的原理，可以阅读下列链接中的相关论文。OpenSLAM.org

安装gmapping：sudo apt-get install ros-melodic-gmapping

在工作空间下创建一个新的功能包。

catkin_create_pkg mrobot_navigation geometry_msgs move_base_msgs roscpp tf visualization_msgs

在mrobot_navigation/launch下创建一个gmapping.launch文件，主要用于节点参数的配置。

<launch>
    <arg name="scan_topic" default="scan"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

这里比较重要的两个参数：一个是odom_frame参数要和机器人本身的里程计坐标系一致；另一个就是激光雷达的话题名。

下面再创建一个launch文件，同时启动上面的gmapping节点和rviz界面。

<launch>
    <include file="$(find mrobot_navigation)/launch/gmapping.launch"/>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/gmapping.rviz"/>
</launch>

下面启动gmapping_demo.launch 和 view_mrobot_with_laser_gazebo.launch，再启动键盘控制节点，即可控制小车在gazebo仿真环境中移动，并且rviz中会同时建立相应地图，如下图所示。

在仿真环境中完整的跑一遍，建立完整的地图。

先安装一个地图管理的功能包sudo apt-get install ros-melodic-map-server

通过命令rosrun map_server map_saver保存当前建立好的地图

 

完成建图之后，下面进行机器人的导航。首先安装相关依赖

sudo apt-get install ros-melodic-navigation

我们使用move_base进行导航，以及利用ACML进行定位，使用move_base进行导航的时候需要对代价地图和本地规划器进行配置，首先是代价地图配置中的通用配置文件。

obstacle_range: 2.5
raytrace_range: 3.0
#footprint: [[0.15, 0.15], [0.15, -0.15], [-0.15, -0.15], [-0.15, 0.15]]
#footprint_inflation: 0.01
robot_radius: 0.15
inflation_radius: 0.02
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

obstacle_range即检测障碍物的最大范围，raytrace_range即检测自由空间的最大范围，footprint机器人在二维地图上的占用面积，inflation_radius即障碍物的膨胀参数，observation_sources参数列出了代价地图需要关注的所有传感器信息，每个传感器信息都会在后面列出详细内容。

然后是代价地图的全局规划配置文件。

global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   resolution: 0.01
   transform_tolerance: 1.0
   map_type: costmap

global_frame用来表示全局代价地图需要在哪个参考系下运行，robot_base_frame用来表示代价地图可以参考的机器人本体的坐标系。

然后是代价地图的本地规划配置文件。

local_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 1.0

最后是本地规划器配置

controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

TrajectoryPlannerROS:
   max_vel_x: 0.5
   min_vel_x: 0.1
   max_vel_y: 0.0  # zero for a differential drive robot
   min_vel_y: 0.0
   max_vel_theta: 1.0
   min_vel_theta: -1.0
   min_in_place_vel_theta: 0.4
   escape_vel: -0.1
   acc_lim_x: 1.5
   acc_lim_y: 0.0  # zero for a differential drive robot
   acc_lim_theta: 1.2

   holonomic_robot: false
   yaw_goal_tolerance: 0.1 # about 6 degrees
   xy_goal_tolerance: 0.05  # 5 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 0.4
   gdist_scale: 0.8
   meter_scoring: true

   heading_lookahead: 0.325
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0.05
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 1.0
   sim_granularity: 0.05
   angular_sim_granularity: 0.1
   vx_samples: 8
   vy_samples: 0  # zero for a differential drive robot
   vtheta_samples: 20
   dwa: true
   simple_attractor: false

该配置文件声明机器人本地规划采用Trajectory Rollout算法，并且设置算法中需要用到机器人速度、加速度等阈值信息。

创建launch文件启动move_base节点。mrobot_navigation/launch/move_base.launch

<launch>
    <node pkg="move_base" type="move_base" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find mrobot_navigation)/config/costmap_common_params.yaml" command="load"
        ns="global_costmap"/>
        <rosparam file="$(find mrobot_navigation)/config/costmap_common_params.yaml" command="load"
        ns="local_costmap"/> 
        <rosparam file="$(find mrobot_navigation)/config/local_costmap_params.yaml" command="load"/>
        <rosparam file="$(find mrobot_navigation)/config/global_costmap_params.yaml" command="load"/>
        <rosparam file="$(find mrobot_navigation)/config/base_local_planner_params.yaml" command="load"/>
    </node>               
</launch>

然后再创建一个运行所有导航功能节点的顶层launch文件mrobot_navigation/launch/navigation.launch

<launch>

    <node name="map_server" pkg="map_server" type="map_server" args="$(find mrobot_navigation)/maps/map.yaml"/>

    <include file="$(find mrobot_navigation)/launch/move_base.launch"/>

    <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />

    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100"/>

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>

</launch>

fake_localization兼容虚拟定位，tf用来发布/odom和/map之间的静态坐标变换。

通过命令roslaunch mrobot_gazebo view_mrobot_with_laser_gazebo.launch

roslaunch mrobot_navigation navigation_demo.launch

运行结果如下所示。

 

在rviz界面中点击 2D Nav Goal，即可看到机器人正在自动导航到相应的位置。

搭建机器人进行slam完成。

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值