本博客是 ROS机器人建模与仿真 的第二讲，参考古月大神《机器人建模与仿真》一书。仅纪录本人的学习过程，欢迎大家一起学习讨论。
使用版本: ROS Melodic
上一讲创建的 Mrobot URDF 模型，机器人底板上安装了8根支撑柱，架起了两层支撑板，可以在这些支撑板上放置电池、控制板、传感器等硬件设备。通常室内移动机器人会装配彩色摄像头、RGB-D 摄像头、激光雷达等传感器，我们可以在虚拟的机器人模型世界里创造一切。
1 添加摄像头
1.1 建立摄像头模型
用一个长方体来代表摄像头模型，对应的模型文件 test_camera.xacro :
<?xml version="1.0"?>
<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:
<?xml version="1.0"?>
<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" />
<!--camera position-->
<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" />
<!--Mrobot body-->
<test_mrobot_body />
<!--Camera-->
<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)"/>
<!--设置GUI参数，显示关节控制插件-->
<param name="use_gui" value="arg gui" />
<!--运行 joint_state_publisher_gui 节点，发布机器人的关节状态-->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!--运行 robot_state_publisher 节点，发布TF-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!--运行rviz可视化界面-->
<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 文件
<?xml version="1.0"?>
<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 文件：
<?xml version="1.0"?>
<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)"/>
<!--设置GUI参数，显示关节控制插件-->
<param name="use_gui" value="arg gui" />
<!--运行 joint_state_publisher_gui 节点，发布机器人的关节状态-->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!--运行 robot_state_publisher 节点，发布TF-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!--运行rviz可视化界面-->
<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 模型文件：
<?xml version="1.0"?>
<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 :
<?xml version="1.0"?>
<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)"/>
<!--设置GUI参数，显示关节控制插件-->
<param name="use_gui" value="arg gui" />
<!--运行 joint_state_publisher_gui 节点，发布机器人的关节状态-->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!--运行 robot_state_publisher 节点，发布TF-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!--运行rviz可视化界面-->
<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)"/>
<!--设置GUI参数，显示关节控制插件-->
<param name="use_gui" value="$(arg gui)" />
<!--配置arbotix_driver-->
<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>
<!--运行 joint_state_publisher_gui 节点，发布机器人的关节状态-->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!--运行 robot_state_publisher 节点，发布TF-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
<param name="publish_frequency" type="double" value="20.0" />
</node>
<!--运行rviz可视化界面-->
<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" />
<!--配置arbotix_driver-->
<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 环境下的控制和仿真，敬请关注！
2525
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
