ROS机器人建模与仿真（二）——URDF模型添加传感器与仿真器

本博客是 ROS机器人建模与仿真 的第二讲，参考古月大神《机器人建模与仿真》一书。仅纪录本人的学习过程，欢迎大家一起学习讨论。

使用版本: ROS Melodic

上一讲创建的 Mrobot URDF 模型，机器人底板上安装了8根支撑柱，架起了两层支撑板，可以在这些支撑板上放置电池、控制板、传感器等硬件设备。通常室内移动机器人会装配彩色摄像头、RGB-D 摄像头、激光雷达等传感器，我们可以在虚拟的机器人模型世界里创造一切。

1 添加摄像头

1.1 建立摄像头模型

用一个长方体来代表摄像头模型，对应的模型文件 test_camera.xacro :

<robot xmlns:xacro="https://www.ros.org/wiki/xacro" name="camera">
    <xacro:macro name="usb_camera" params="prefix:=camera">
        <link name="${prefix}_link">
            <inertial>
                <origin xyz="0 0 0" />
                <mass value="0.1" />
                <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>
                    <box size="0.01 0.04 0.04" />
                geometry>
                <material name="black" />
            visual>
            <collision>
                <origin xyz="0.0 0.0 0.0"  rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                geometry>
            collision>
        link>
    xacro:macro>

robot>

1.2 将摄像头与机器人连接

建立camera 与 机器人平台plate_2_link的连接关系 joint，并将机器人与照相机拼接在一起，建立 test_mrobot_with_camera.urdf.camera:

<robot name="mrobot" xmlns:xacro="https://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_mrobot_body.urdf.xacro" />
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_camera.xacro" />
	  
    <xacro:property name="camera_offset_x" value="0.1" />
    <xacro:property name="camera_offset_y" value="0.0" />
    <xacro:property name="camera_offset_z" value="0.02" />

    
    <test_mrobot_body />

    
    <joint name="camera_joint" type="fixed">
        <origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0 " />
        <parent link="plate_2_link" />
        <child    link="camera_link" />
    joint>
    
    <xacro:usb_camera prefix="camera" />
robot>

由于代码与上一讲的几乎相同，不再写注释了，大家可以与前一讲代码比较学习。

1.3 模型在Rviz中显示

最后，写一个启动文件即可在rviz中进行显示：
test_display_mrobot_with_camera.launch

<launch>
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find test_mrobot_description)/urdf/test_mrobot_with_camera.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_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 test_mrobot_description)/config/mrobot.rviz" required="true" />
launch>

显示结果如下图所示，具体摄像头配置将在后续Gazebo配置中介绍：

2 添加Kinect

Kinect 是一种常用的 RGB-D 摄像头，三维模型文件kinect.dae 可以在TurtleBot功能包中找到（仅限kinetic和indigo中，目前在Melodic版本中由于只能下载TurtleBot3，中间没有这个文件，我在别的版本中下载了下来）。文件通过百度网盘分享给大家：，将文件下载下来放入 test_mrobot_description/meshes文件夹下即可。

2.1 创建Kinect模型描述文件

类似于摄像机的配置，建立 test_kinect.xacro 文件

<robot name="kinect_camera" xmlns:xacro="https://www.ros.org/wiki/xacro">
    <xacro:macro name="kinect_camera" params="prefix:=camera">
        <link name="${prefix}_link">
            <visual>
                <origin xyz="0 0 0  " rpy="0 0 ${M_PI/2}" />
                <geometry>
                    <mesh filename="package://mrobot_description/meshes/kinect.dae" />
                geometry>
            visual>
            <collision>
                <geometry>
                    <box size="0.07 0.3 0.07" />
                geometry>
            collision>
        link>
    xacro:macro>
robot>

其中唯一不同的是 ，使用 < mesh >标签将kinect模型文件导入，< collision >标签将此模型简化为一个长方体，精简碰撞检测的数学计算。

2.2 将kinect与机器人连接

建立 test_mrobot_with_kinect.xacro 文件：

<robot name="mrobot" xmlns:xacro="https://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_mrobot_body.urdf.xacro" />
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_kinect.xacro" />

    <xacro:property name="kinect_offset_x" value="-0.06" />
    <xacro:property name="kinect_offset_y" value="0.0" />
    <xacro:property name="kinect_offset_z" value="0.035" />

    <test_mrobot_body />
    <joint name="kinect_frame_joint" type="fixed">
        <origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
        <parent link="plate_2_link" />
        <child link="camera_link" />
    joint>

    <xacro:kinect_camera prefix="camera" />
robot>

2.3 在Rviz中显示模型

建立 display_kinect.launch 文件：

<launch>
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find test_mrobot_description)/urdf/test_mrobot_with_kinect.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 test_mrobot_description)/config/mrobot_kinect.rviz" required="true" />
launch>

模型显示如下：

3 添加激光雷达

3.1 建立 Rplidar 激光雷达模型

创建 test_rplidar.xacro 模型文件：

<robot name="laser" xmlns:xacro="https://www.ros.org/wiki/xacro" >
    <xacro:macro name="rplidar" params="prefix:=laser">
        <link name="${prefix}_link">
            <inertial>
                <origin xyz="0 0 0" />
                <mass value="0.1" />
                <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.05" radius="0.05" />
                geometry>
            collision>
        link>
    xacro:macro>

robot>

3.2 连接Rplidar和机器人

创建 test_mrobot_with_rplidar.xacro :

<robot name="mrobot" xmlns:xacro="https://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_mrobot_body.urdf.xacro" />
    <xacro:include filename="$(find test_mrobot_description)/urdf/test_rplidar.xacro" />

    <xacro:property name="cplidar_offset_x" value="0" />
    <xacro:property name="cplidar_offset_y" value="0" />
    <xacro:property name="cplidar_offset_z" value="0.03" />

    <test_mrobot_body />

    <joint name="rplidar_joint" type="fixed">
        <origin xyz="${cplidar_offset_x} ${cplidar_offset_y} ${cplidar_offset_z} " rpy="0 0 0" />
        <parent link="plate_2_link" />
        <child link="laser_link" />
    joint>

    <xacro:rplidar prefix="laser" />

robot>

3.3 在Rviz中建立模型

建立 display_rplidar.launch 文件：

<launch>
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find test_mrobot_description)/urdf/test_mrobot_with_rplidar.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 test_mrobot_description)/config/mrobot.rviz" required="true" />
launch>

显示模型：

4 基于ArbotiX 和 rviz 的仿真器

在之前 rviz 的模型显示中使用了一个小插件来控制机器人的轮子转动，既然轮子可以转动，那么机器人应该可以在 rviz 中进行运动。

ArbotiX 是一款控制电机、舵机的控制板，并提供相应的 ROS 功能包，这个功能包不仅可以驱动真实的 Arbotix 控制板，还提供一个差速控制器，通过接收速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动。

本讲主要为机器人模型配置 ArbotiX 差速控制器，配合 rviz 创建一个简单的仿真环境。

4.1 安装 ArbotiX

在 Melodic 版本的 ROS 软件源中已经集成了 ArbotiX 功能包的二进制安装文件，可以使用如下命令进行安装：

sudo apt-get install ros-melodic-arbotix-*

也可以选择直接从 github 上拷贝源码：

git clone https://github.com/vanadiumlabs/arbotix_ros.git

4.2 配置 Arbotix 控制器

ArbotiX 功能包安装完成后，就可以针对机器人模型进行配置了。需要创建一个启动 ArbotiX 节点的 launch 文件，再创建一个相关的配置文件。

• 1 创建 launch 启动文件
  arbotix_mrobot_with_kinect.launch(以配置深度相机的机器人为例)

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

    <arg name="model" default="$(find xacro)/xacro --inorder '$(find test_mrobot_description)/urdf/test_mrobot_with_kinect.xacro'" />
    <arg name="gui" default="false" />
    <param name="robot_description" command="$(arg model)"/>

    
    <param name="use_gui" value="$(arg gui)" />

    
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find test_mrobot_description)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true" />
    node>

    
    <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 test_mrobot_description)/config/test_mrobot_arbotix.rviz" required="true" />
launch>

程序解析，相比于之前的显示lunch文件，这个文件中多了以下几行代码：

    <param name="/use_sim_time" value="false" />
    
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find test_mrobot_description)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true" />
    node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
        <param name="publish_frequency" type="double" value="20.0" />
    node>

< param name="/use_sim_time" value=“false” />

经测试，此行代码有无对程序皆无影响，作用目前还不清楚，且做一个标记，后续学习填坑。

< node name=“arbotix” pkg=“arbotix_python” type=“arbotix_driver” output=“screen”>
< rosparam file="$(find test_mrobot_description)/config/fake_mrobot_arbotix.yaml" command=“load” />
< param name=“sim” value=“true” />
< /node>

启动 arbotix_driver 节点，同时加载控制器相关的配置文件
< param name=“sim” value=“true” /> 的作用目前看来应该是打开串口“/dev/ttyUSB0”，作者也没有详细解释，反正删了会报错，姑且这么理解。

< node name=“robot_state_publisher” pkg=“robot_state_publisher” type=“robot_state_publisher” >
< param name=“publish_frequency” type=“double” value=“20.0” />
< /node>

前文我们讲过的，robot_state_publisher 的作用是将机器人的各个link、joint之间的关系，通过 TF 的形式整理成三维姿态信息发布出去，参数 publish_frequency 就是规定了信息发布的频率。

• 2 创建控制器相关的参数配置文件
  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}
}

控制器命名为 base_controller,类型是 diff_controller (差速控制器)
配置参考坐标系：base_fame_id
底盘尺寸：base_width
编码器精度：ticks_meter
PID 控制参数：kp, kd, ki, ko
加速度限制：accel_limit
可以将odom_frame_id:修改为自己想固定的坐标系，默认为odom

在分析arbotix功能包中arbotix_python的源文件diff_controller.py发现

    # parameters: rates and geometry
    self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
    self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
    self.t_delta = rospy.Duration(1.0/self.rate)
    self.t_next = rospy.Time.now() + self.t_delta
    self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
    self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))

    self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
    self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')

    # parameters: PID
    self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
    self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
    self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
    self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)

    # parameters: acceleration
    self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)

这些都是可在.yaml 文件中载入的数据。如果自己不去自定义会有相应的默认值。

运行仿真环境：

roslaunch test_mrobot_description arbotix_mrobot_with_kinect.launch

4.3 控制小车的运动

此时查看当前的话题信息：

rostopic list

可以发现其中的/cmd_vel 速度控制话题，此时可以选择多种速度控制方式。

4.3.1 命令行控制

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" 

此时小车以 1m/s的线速度，1 rad/s 的角速度持续进行圆周运动

4.3.2 键盘控制

此时引用一个已经编辑好的键盘控制命令 类似乌龟测试程序的 turtle_teleop_key

启动命令：

roslaunch mrobot_teleop mrobot_teleop.launch

接下来，让我们具体看一下键盘控制命令的实现过程：
mrobot_teleop.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#申明编码格式
import rospy
from geometry_msgs.msg import Twist
#在geometry_msgs功能包里引用 Twist 话题（用来发布速度指令）
import sys, select 
import tty #终端控制功能（tty在linux中就是终端的意思）
import termios #此模块提供了针对tty 的I/O 控制的 POSIX 调用接口

#这个 msg 是发布在终端里面的提示
msg = """
控制机器人的按键说明
---------------------------
移动按键：
7    8    9
4    5    6
1    2    3

q/z : 最大速度增加/减小 10%
w/x : 线速度增加/减小  10%
e/c : 角速度增加/减小  10%
空格/5 ： 强制停止
其余按键，缓慢停止

按 Ctrl + C 退出
"""

#控制机器人的移动
#括号里的第一个参数表示前后  （1 表示前进）
#括号里的第二个参数表示转向左右 （1 表示左转）
moveBindings = {
       '7':(1,1),
       '8':(1,0),
       '9':(1,-1),
       '4':(0,1),
       '6':(0,-1),
       '1':(-1,-1),
       '2':(-1,0),
       '3':(-1,1),
           }

#速度控制
#第一个参数控制 x 轴线速度，第二个参数控制 z 轴角速度，表示新速度是原速度的多少倍
speedBindings={
# 最大速度增加或者减小
        'q':(1.1,1.1),
        'z':(.9,.9),
#线速度增加或者减小
        'w':(1.1,1),
        'x':(.9,1),
#角速度增加或者减小
        'e':(1,1.1),
        'c':(1,.9),
          }
# 此函数的目的是监听键盘的按键输入
def getKey():
    #目前看不懂，大概是数据输入的意思吧
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    #判断是否接收到数据
    if rlist:
        key = sys.stdin.read(1) #读取终端上的交互输入
    else:
        key = ''
    #目前看不懂，和键盘输入有关
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key
#设置初始执行速度、初始角速度
speed =0.2
turn = 1

#在屏幕上打印当前线速度，角速度
def vels(speed,turn):
    return "当前速度为:\t  线速度为： %s\t 角速度为： %s " % (speed,turn)

if __name__=="__main__":
    #这个 settings和代码最后一句都是配合 getKey() 函数来获得键盘的输入用的
    settings = termios.tcgetattr(sys.stdin)
    
    # 初始化节点
    rospy.init_node('mrobot_teleop')
    # 创建 速度控制的话题 （话题名：cmd_vel ,消息类型：Twist）
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    
    #初始化速度、方向等等
    x = 0   #表示机器人是前进/后退            （1前进，-1后退）
    th = 0 #表示机器人是原地左转，右转（1左转，-1右转）

    status = 0 #
    count = 0
    acc = 0.1

    target_speed = 0          #目标直行速度
    target_turn = 0             #目标角速度
    control_speed = 0      #实际运行的直行速度
    control_turn = 0         #实际运行的角速度


    try:
        print msg #在终端打印操作教程
        print vels(speed,turn) #打印目前设置的直行速度和角速度
        while(1):
            #获取用户输入
            key = getKey()
            # 运动控制方向键（1：正方向，-1负方向）
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            # 速度修改键
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]  # 线速度增加0.1倍
                turn = turn * speedBindings[key][1]    # 角速度增加0.1倍
                count = 0

                print vels(speed,turn) #再次打印速度和角速度

                if (status == 14):   #当终端打印的速度信息过多，重新打印操作教程
                    print msg
                status = (status + 1) % 15
            # 停止键
            elif key == ' ' or key == '5' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:  #当按下其他键超过 4 次后，机器人会维持原来的方向缓慢减速
                    x = 0
                    th = 0
                if (key == '\x03'): #表示按下 Ctrl+c,此时退出循环
                    break

            # 目标速度=速度值*方向值
            target_speed = speed * x
            target_turn = turn * th

            # 速度限位，防止速度增减过快：0.02和0.1分别代表速度和角速度的变化率，相当于加速度大小
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed
            

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            # 创建并发布twist消息
            twist = Twist()
            twist.linear.x = control_speed; 
            twist.linear.y = 0; 
            twist.linear.z = 0
            twist.angular.x = 0; 
            twist.angular.y = 0; 
            twist.angular.z = control_turn
            pub.publish(twist)

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; 
        twist.linear.y = 0; 
        twist.linear.z = 0
        twist.angular.x = 0; 
        twist.angular.y = 0; 
        twist.angular.z = 0
        pub.publish(twist)
    #和键盘输入有关
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

生成 启动文件 mrobot_teleop.launch：

<launch>
  <node name="mrobot_teleop" pkg="mrobot_teleop" type="mrobot_teleop.py" output="screen">	
  
  <param name="scale_linear"  value="0.1" type="double"/>
  <param name="scale_angular" value="0.4" type="double"/>
  node>
launch>

运行结果：

本讲结束，下一讲将介绍 机器人在 Gazebo 环境下的控制和仿真，敬请关注！