ROS培训教程(第2期)¶
欢迎来到Tonyrobotics(汤尼机器人)推出的ROS系列教程！
准备工作¶
0.1.准备工作¶
配置ROS开发机器可以有两种方式：
- 在已有机器上直接安装Ubuntu系统，并安装ROS
- 使用虚拟机(由汤尼机器人提供，已安装好ROS环境)
1.机器配置¶
方式一：直接安装（熟悉Ubuntu系统的用户推荐用此种方式）¶
在此，以Ubuntu 16.04与ROS Kinetic为例。
- 首先，下载Ubuntu 16.04桌面版系统镜像，制作安装盘，安装系统到您的机器。
- 安装好Ubuntu 16.04系统后，可参照 本教程1.1节 步骤安装ROS。
方式二：使用虚拟机（不熟悉Ubuntu系统的用户推荐用此种方式）¶
第一步：安装虚拟机
虚拟机软件可使用VMWare软件或VirtualBox。在此，给出了VirtualBox虚拟机的安装方式。如果您熟悉VMWare，可略过此步骤，使用VMWare导入虚拟机。请参照以下教程安装：
Windows下安装VirtualBox可参考 http://drupalchina.cn/node/6391
第二步：导入装有ROS的Ubuntu系统镜像
到网盘下载汤尼机器人制作的虚拟机文件 Ubuntu 16.04 for ROS Kinetic.ova ，大小： 4.8G 。
下载完后，直接用虚拟机软件加载.ova文件即可。具体如何导入.ova文件，可参照 https://jingyan.baidu.com/article/870c6fc3551e21b03fe4be14.html
注意 ：如果出现加载失败现象，可能是BIOS没有开启虚拟化技术。 需要先进入BIOS，找到 Virtualization Technology 选项或 VT-x 选项，开启即可。
第三步：登录Ubuntu系统
系统导入成功后，可登录系统，用户名密码为：
用户名：ros
密码：ros
登录成功后，即可进入Ubuntu桌面系统。
2.ROS环境测试¶
Ubuntu系统装有ROS环境，以及此次课程所需的资料。所有资料均存放于Documents目录下。
为了测试ROS环境正常，执行以下步骤。
(1)Ctrl+Alt+T快捷键打开终端，输入roscore，回车运行。
(2)再使用快捷键打开一个新的终端，输入rosrun turtlesim turtlesim_node，回车运行，此时会弹出小乌龟界面。
(3)再打开第三个终端，输入rosrun turtlesim turtle_teleop_key，回车运行， 将光标保持在此窗口 ， 然后点击键盘方向键，完成对小乌龟的控制。
(4)执行快捷键Ctrl+C终止当前命令行程序。完成测试。
至此，则完成准备工作。
0.2.基础知识预习¶
在进行ROS学习时，需要具备基本的Linux知识，以及C++或Python编程知识。 可参照下列教程进行预习。如果已具备基本的Linux及编程相关知识，可略过此节。
1.Linux/Ubuntu基础知识及常用指令介绍¶
可参照下列教程进行入门： https://www.cnblogs.com/resn/p/5800922.html
2.C++编程基础知识介绍¶
可参照系列教程进行入门： http://www.runoob.com/cplusplus/cpp-tutorial.html
3.Python编程基础知识介绍¶
可参照简明Python教程： http://www.kuqin.com/abyteofpython_cn/
理论部分¶
目标：了解ROS的基础概念
ROS_Introduction¶
ROS简介¶
ROS的应用
PR2个人助理
Turtlebot移动机器人
PAL人形机器人
NASA太空机器人
ROS的发展历史
Powering the world’s robots—机器人软件的事实标准
ROS概念¶
ROS：Robot Operating System 机器人操作系统
- ROS是面向机器人的开源的元操作系统（meta-operating system）
- 提供类似传统操作系统的诸多功能：硬件抽象、底层设备控制、常用功能实现、进程间消息传递、程序包管理等。
- 提供相关工具和库，用于获取、编译、编辑代码以及在多个计算机之间运行程序完成分布式计算
ROS为机器人软件开发带来的优势：
- 分布式计算：点对点，解决进程间通讯问题
- 软件复用：算法，通信接口， 避免重复造轮子
- 快速测试：工具，模块化，数据记录与回放
- 免费开源：ROS软件的开发自始至终采用开放的BSD协议，开源社区
ROS安装¶
我们以ROS Kinetic版本为例看一下ROS的安装及配置。
- Ubuntu软件配置：勾选universe, restricted, multiverse选项
- 添加sources.list：将镜像添加到Ubuntu系统源列表中
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
- 添加keys：公钥是Ubuntu系统的一种安全机制，ROS安装不可缺少
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
- 系统更新：确保Debian软件包和索引是最新的
$ sudo apt-get update && sudo apt-get upgrade
- 安装ROS：四种安装方式，推荐桌面完整版
$ sudo apt-get install ros-kinetic-desktop-full
ROS配置¶
- 初始化rosdep:
- rosdep可在需要编译某些源码的时候为其安装一些系统依赖 rosdep也是某些ROS核心功能组件所必须用到的工具
$ sudo rosdep init && rosdep update
- ROS环境配置: 添加到bashrc文件中
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
- 安装rosinstall: ROS中独立分开的常用命令行工具，通过一条命令就可以给某个ROS软件包下载很多源码树。
$ sudo apt-get install python-rosinstall
- 更多ROS安装相关的信息可参考以下链接：
- http://wiki.ros.org/kinetic/Installation/Ubuntu
ROS File System¶
Catkin工作空间¶
ROS Workspace - catkin Workspace
- ROS Workspace - 组织ROS项目文件的文件夹
- catkin - 编译工具，将ROS项目的源码编译成可执行的二进制文件，Catkin是基于CMake的编译构建系统
- catkin Workspace - 使用catkin作为编译工具的ROS Workspace, 主要包含src空间、devel空间、build空间、log空间
创建catkin工作空间
在home目录下打开新的终端，依次输入以下指令创建catkin工作空间:
mkdir -p tr_tutorial_ws/src
cd tr_tutorial_ws
catkin init
catkin build
ROS Package¶
ROS Package
组织ROS应用中不同的功能模块
位于src空间中，存放用户代码
ROS Package必须包括两个文件：
package.xml ：包括了package的描述信息
name, description, version, maintainer(s), license
authors, dependencies, plugins, etc…
CMakeLists.txt ：构建package所需的CMake文件
调用Catkin的函数/宏
解析package.xml
找到其他依赖的catkin软件包
将本软件包添加到环境变量
创建ROS Package
进入src空间，使用以下命令创建ROS Package：
cd src
# catkin_create_pkg <new_package_name> <package_deps>
catkin_create_pkg topic_demo rospy std_msgs
通过以下指令可以安装整个src空间中所有的Package所需的依赖：
cd <src space>
rosdep install --from-paths . --ignore-src -y
Catkin编译的工作流程¶
Catkin编译的工作流程如下：
- 首先在工作空间catkin_ws/src/下递归的查找其中每一个ROS的package
- Catkin(CMake)编译系统依据CMakeLists.txt文件，生成makefiles(放在catkin_ws/build/中)
- 然后make刚刚生成的makefiles等文件，编译链接生成可执行文件（放在catkin_ws/devel中）
Catkin是将cmake与make指令做了封装从而完成整个编译过程的工具。
更多ROS文件层级相关的概念可以参考以下内容：
ROS Computation Graph¶
ROS计算图概念¶
ROS计算图级概念是理解ROS应用的最重要的概念，可以认为ROS计算图是对运行时的ROS应用的形象化表示。
在底层，ROS计算图由ros_comm库实现，包含Master、Node、Topic、Service、Parameter Server等概念的实现。。
下图是一个典型的ROS计算图，我们可以看到用ROS实现的机器人视觉应用项目数据流向。
Node:
- ROS的最小进程单元
- 从程序角度来说，就是一个可执行文件被执行，加载到了内存之中
- 从功能角度来说，通常一个node负责机器人的某一个单独的功能
- ROS Node通过Topic、Service、Action进行通信
- ROS Node通常由C++或Python实现
Master:
- 节点管理器，在整个网络通信架构里相当于管理中心，管理各个node
- Node首先在master处进行注册，然后进入整个ROS网络
- Master引导各个相关节点建立直接的连接（数据的传输不经过Master），实现分布式计算节点间的通信
- ROS程序启动时，第一步先启动master，随后启动node
ROS应用的启动¶
1. 启动Master
打开终端，输入:
roscore
运行roscore命令将启动ROS master，同时启动rosout和parameter server
rosout：负责日志输出的节点，告知用户当前系统的状态，输出系统的error、warning等，并将log记录于日志文件中
parameter server：参数服务器，并不是node，而是存储参数配置的一个服务器
注意： 如果Master在ROS工程启动之后停止或重启，之前启动的节点仍然能正常进行通信。但这些节点将在不会试图与重启之后Master重连，Master不会有之前已经启动的节点的信息，所以不能帮助后续启动的节点与之前启动的节点建立通信连接。
2. 启动Node
可通过以下方式启动节点:
rosrun pkg_name node_name
也可使用以下方式启动同一个节点的多个副本，否则，如果试图启动跟已启动的节点具有相同名字的新节点，会将原有的节点关闭。
rosrun pkg_name node_name __name:=new_name
注意： 如果使用Ctrl-C命令终止节点，通常不会在Master中注销该节点，这样已终止的节点信息依然可以通过rosnode命令查看，可以使用rosnode cleanup命令清除注销节点的残留信息；另外如果使用rosnode kill指令关闭节点，通常不会发生上述现象。
ROS计算图示例¶
- 首先启动Master
roscore
- 然后启动turtle模拟器节点
rosrun turtlesim turtlesim_node #启动turtle模拟器
- 接下来启动turtle控制节点
rosrun turtlesim turtle_teleop_key #通过turtle控制节点，我们可以控制模拟器中的turtle运动
- 通过rqt node graph查看运行时ROS应用的计算图
rqt
在终端中输入rqt，随后在弹出的界面的Plugins中选择Node Graph即可看到下图所示的ROS计算图。
我们可以看到/teleop_turtle Node和/turtlesim Node通过/turtle1/cmd_vel Topic进行通信。
ROS Node 命令行工具¶
ROS提供了一些命令行工具帮助我们查看Node的信息，最常用的是:
rosnode list #列出当前正在运行的Nodes
rosnode info <node_name> #查看指定Node的信息
更多ROS Node的命令行工具可通过在终端中输入 rosnode -h 查看
更多ROS计算图相关的概念可以参考以下内容：
ROS Topic¶
ROS Topic概念¶
ROS Node之间进行通信所利用的最重要的机制就是消息传递，在ROS中，消息有组织的放到Topic里进行传递。
Publisher
- 生成信息，通过ROS Topic与其它Node进行通信。
- 通常用于处理原始的传感器信息，如相机、编码器等。
Subscriber
- 接收信息，通过ROS Topic接收来自其它Node的信息，并通过回调函数处理
- 通常用于监测系统状态，如当机器人关节到达限位位置时触发运动中断
Topic通信过程为：
- Publisher节点和Subscriber节点分别在Master进行注册
- Publisher发布Topic
- Subscriber在Master指挥下订阅Topic，从而建立起Pub-Sub之间的通信
注意：消息是直接从发布节点传递到订阅节点，并不经过Master。
下图是ROS Node和ROS Topic概念的图形化表示，我们可以看到两个Node（圆形）通过Topic（长方形）实现通信。
Topic通信的特点为：
1. Topic通信是多对多的异步通信方式:
Topic Publisher调用publish()方法发布消息，发送完立即返回，不用等待反馈；Subscriber通过回调函数的方式来处理消息。
对于同一Topic，系统中可以同时存在多个Publisher和多个Subscriber；
另外，Publisher并不知道哪个节点会接收消息，而Subscriber也并不知道接收的消息来自哪个节点，节点之间是松耦合的，这也是ROS最关键的设计特性之一。
2. 对于实时性、周期性的消息，使用topic传输是最佳的选择
3. Topic通信方式充分体现了分布式系统通信的好处：扩展性好，软件复用率高
ROS Topic命令行工具¶
ROS提供了一些命令行工具帮助我们查看Topic的信息，最常用的是:
rostopic list #列出当前正在运行的Topics
rostopic info <topic_name> #查看指定Topic的信息
rostopic echo <topic_name> #打印指定Topic的内容
更多ROS Topic的命令行工具可通过在终端中输入 rostopic -h 查看
ROS Topic示例¶
Publisher Node Demo
#!/usr/bin/python2
# coding: utf-8
import rospy #导入ROS Python客户端
from std_msgs.msg import String
def simplePublisher():
rospy.init_node('publish_node', anonymous = False) #初始化ROS节点
simple_publisher = rospy.Publisher('simple_topic', String, queue_size = 10) #定义Publisher对象
rate = rospy.Rate(10) #设置Topic发布的频率（Hz）
simple_topic_content = "simple pub-sub demo" #填充Topic内容
while not rospy.is_shutdown():
simple_publisher.publish(simple_topic_content) #发布Topic
rate.sleep() #确保Topic以我们设定的频率发布
if __name__ == '__main__':
try:
simplePublisher()
except rospy.ROSInterruptException:
pass
Subscriber Node Demo
#!/usr/bin/python
# coding: utf-8
import rospy #导入ROS Python客户端
from std_msgs.msg import String
# 定义Subscriber回调函数，处理收到的信息
def stringSubscriberCallback(data): #data的数据类型与Subscriber接收的Topic对应的消息类型一致
rospy.loginfo('The contents of simple_topic: %s', data.data)
def stringSubscriber():
rospy.init_node('subscribe_node', anonymous = False) #初始化ROS节点
rospy.Subscriber('simple_topic', String, stringSubscriberCallback) #定义Subscriber对象
rospy.spin()
if __name__ == '__main__':
stringSubscriber()
自定义Message类型¶
Topic类型严格来说是一个抽象的概念，提到Topic类型，通常指的是Node通过Topic传递的Message的类型。
ROS提供了大量的标准Message类，同时也支持用户自定义Message类型。
Message文件
- ROS通过message文件定义Message
- Message文件通常位于<ros_package_name>/msg文件夹中
- Message文件的名字通常为<custom_message_type>.msg
自定义Message类型
现在，我们定义一个新的超声传感器类型： UltrasoundInformation
要求包含：
- ROS message type - 距离传感器接口类型
- String - 超声传感器制造商名称
- Unsigned Integer - 超声传感器编号
我们可以通过以下指令创建这个新的Message类型:
首先创建msg文件:
roscd topic_demo
mkdir msg
cd msg
touch UltrasoundInformation.msg
然后在新建的msg文件中输入以下内容:
sensor_msgs/Range distance
string maker_name
uint32 part_number
编译构建自定义的Message类型
在上一部分我们定义了新的Message文件，新定义的Message经过编译构建之后才能在程序中使用。
首先修改package.xml文件，在package.xml文件中添加以下内容：
<build_depend>sensor_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
然后修改CMakeLists.txt文件，在CMakeLists.txt文件中相应的位置添加以下内容：
# 在find_package中添加sensor_msgs, message_generation
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
sensor_msgs
message_generation
)
# 在add_message_files中添加UltrasoundInformation.msg
add_message_files(
FILES
UltrasoundInformation.msg
)
# 在generate_messages中添加sensor_msgs
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
随后切换到tr_tutorial_ws顶层目录执行编译构建：
catkin build
编译完成之后：
source devel/setup.bash
我们可以通过 rosmsg 命令行工具查看新定义的Message类型：
rosmsg show topic_demo/UltrasoundInformation
# 新定义的Message的内容
sensor_msgs/Range distance
uint8 ULTRASOUND=0
uint8 INFRARED=1
std_msgs/Header header
uint32 seq
time stamp
string frame_id
uint8 radiation_type
float32 field_of_view
float32 min_range
float32 max_range
float32 range
string maker_name
uint32 part_number
如果想查看更详细的描述，可以添加 -r 命令选项：
rosmsg show topic_demo/UltrasoundInformation -r
这样我们就可以在程序中使用自定义的Message类型了。
ROS Topic练习¶
我们已经完成了Publisher、Subscriber的构建及定义新的Message类型，接下来我们进行一个小练习，构建一个简单的ROS应用：
- 包含一个Publisher和一个Subscriber
- Publisher发布新定义的Message类型
- Subscriber接收到新定义的Message类型之后反馈接收结果
ROS Topic练习示例-Publisher:
#!/usr/bin/python2
# coding: utf-8
import rospy
from topic_demo.msg import UltrasoundInformation
def ultrasoundPublisher():
rospy.init_node('ultra_publish_node', anonymous = False)
ultra_publisher = rospy.Publisher('ultra_topic', UltrasoundInformation, queue_size =10)
rate = rospy.Rate(10)
ultra_information = UltrasoundInformation()
ultra_information.distance.header.stamp = rospy.Time.now()
ultra_information.distance.header.frame_id = 'ultrasound_frame'
ultra_information.distance.radiation_type = 0
ultra_information.distance.field_of_view = 0.05
ultra_information.distance.min_range = 0.01
ultra_information.distance.max_range = 0.1
ultra_information.maker_name = 'tony_robotics'
ultra_information.part_number = 100
while not rospy.is_shutdown():
ultra_publisher.publish(ultra_information)
rate.sleep()
if __name__ == '__main__':
try:
ultrasoundPublisher()
except rospy.ROSInterruptException:
pass
ROS Topic练习示例-Subscriber:
#!/usr/bin/python2
# coding: utf-8
import rospy
from topic_demo.msg import UltrasoundInformation
def ultrasoundSubscriberCallback(data):
rospy.loginfo('The distance form ultrasound sensor is: %f', data.distance.field_of_view)
def ultrasoundSubscriber():
rospy.init_node('ultra_subscribe_node', anonymous = False)
rospy.Subscriber('ultra_topic', UltrasoundInformation, ultrasoundSubscriberCallback)
rospy.spin()
if __name__ == '__main__':
ultrasoundSubscriber()
更多关于ROS Message的资料可参考以下链接：
关于如何创建自定义的Message类型的内容可参考以下链接：
ROS Service¶
ROS Service概念¶
ROS Topic适合实现状态监控类的任务，但Topic通信存在一些缺陷：
- 无法确认发布信息是否被收到。
- 有些类型的消息（例如相机节点发布的消息），按照一定频率发布，通常会占用比较大的带宽，造成资源浪费。
ROS Service可以解决以上问题。
Service是ROS Node的通信方式之一
Service通信的特点为：
- Service使用 请求-查询式 的通信模型，这样的通信模型没有频繁的消息传递，没有高系统资源的占用，只有接受请求才执行服务，简单而且高效。
- Service通信实现的是 一对一 通信，每一个服务由一个节点发起，对这个服务的响应返回同一个节点；Service的信息流是双向的，不仅可以发送消息，同时还会有反馈。所有service包含两部分，Client与Server。
- Service是 同步 通信方式，Client发布请求后会在原地等等reply，直到Server处理完了请求并且完成了reply，Client才会继续执行；Client等待过程中，处于阻塞状态。
下图是ROS Node Service通信的图形化表示，我们可以看到Client Node和Server Node通过request-response的方式进行通信。
ROS Service 命令行工具¶
ROS提供了一些命令行工具帮助我们查看Service的信息，最常用的是:
rosservice list #列出当前正在运行的Services
rosservice call <service_name> <service_arguments> #调用指定的Service
更多ROS Service的命令行工具可通过在终端中输入 rosservice -h 查看
ROS Service示例¶
首先新建service_demo package，并创建scripts文件夹，在src空间执行以下操作：
catkin_create_pkg service_demo rospy std_msgs rospy_tutorials
cd service_demo
mkdir scripts
Service Server Node Demo
#!/usr/bin/python2
# coding: utf-8
import rospy
from rospy_tutorials.srv import *
def handle_add_two_ints(req):
print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b))
return AddTwoIntsResponse(req.a + req.b)
def add_two_ints_server():
rospy.init_node('add_two_ints_server') # 初始化Node
s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) # 创建Service Server
print "Ready to add two ints."
rospy.spin()
if __name__ == "__main__":
add_two_ints_server()
Service Client Node Demo
#!/usr/bin/python2
# coding: utf-8
import sys
import rospy
from rospy_tutorials.srv import *
def add_two_ints_client(x, y):
rospy.wait_for_service('add_two_ints') # 阻塞直到server可用
try:
add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts) # 创建Service调用handle
resp1 = add_two_ints(x, y) # 调用Service
return resp1.sum
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def usage():
return "Usage: rosrun <client_node_name> [x y]"
if __name__ == "__main__":
if len(sys.argv) == 3:
x = int(sys.argv[1])
y = int(sys.argv[2])
else:
print usage()
sys.exit(1)
print "Requesting %s+%s"%(x, y)
print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))
使用ROS Service开发ROS应用需要注意的内容：
- ROS Service会阻塞程序流
- 在Service回调函数中只能进行能够快速完成的计算任务
- 进入Service回调流程后没有中途返回机制
自定义Service类型¶
Service文件
- ROS通过service文件定义Service
- Service文件通常位于<ros_package_name>/srv文件夹中
- Service文件的名字通常为<custom_message_type>.srv
- Service类型包含request、response两部分
自定义Service类型
现在，我们定义一个新的弧度角转换Service类型： RadianToAngle
要求包含：
- Request - 弧度值
- Response - 角度值
- Response - 转换成功标志位
我们可以通过以下指令创建这个新的Service类型:
首先创建srv文件:
roscd service_demo
mkdir srv
cd srv
touch RadianToAngle.srv
然后在新建的srv文件中输入以下内容:
float64 radian
---
float64 angle
bool success
编译构建自定义的Service类型
在上一部分我们定义了新的Service文件，新定义的Service经过编译构建之后才能在程序中使用。
首先修改package.xml文件，在package.xml文件中添加以下内容：
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>
然后修改CMakeLists.txt文件，在CMakeLists.txt文件中相应的位置添加以下内容：
# 在find_package中添加message_generation
find_package(catkin REQUIRED COMPONENTS
rospy
rospy_tutorials
std_msgs
message_generation
)
# 在add_service_files中添加RadianToAngle.srv
add_service_files(
FILES
RadianToAngle.srv
)
# 去掉generate_messages的注释
generate_messages(
DEPENDENCIES
std_msgs
)
随后切换到tr_tutorial_ws顶层目录执行编译构建：
catkin build
编译完成之后：
source devel/setup.bash
我们可以通过 rossrv 命令行工具查看新定义的Service类型：
rossrv show service_demo/RadianToAngle
# 新定义的Service的内容
float64 radian
---
float64 angle
bool success
如果想查看更详细的描述，可以添加 -r 命令选项：
rossrv show service_demo/RadianToAngle -r
这样我们就可以在程序中使用自定义的Message类型了。
ROS Service练习¶
我们已经完成了Service Server、Client的构建及定义新的Service类型，接下来我们进行一个小练习，构建一个简单的ROS应用：
- 包含一个Service Server和一个Service Client
- Service Server提供将弧度转换为角度的Service
- Service Client向Server发出转换请求
ROS Service练习示例-Server:
#!/usr/bin/python2
# coding: utf-8
import rospy
from math import pi
from service_demo.srv import RadianToAngle, RadianToAngleRequest, RadianToAngleResponse
def radian_to_angel_server_callback(req):
res = RadianToAngleResponse()
res.angle = 180.0 / pi * req.radian
res.success = True
print "request radian: %f, response angle: %f"%(req.radian, res.angle)
return res
def radian_to_angle_server():
rospy.init_node('radian_to_angle_server_node')
radian_to_angle_server = rospy.Service('radian_to_angle', RadianToAngle, radian_to_angel_server_callback)
print "Ready to convert radian to angle"
rospy.spin();
if __name__ == '__main__':
radian_to_angle_server()
ROS Service练习示例-Client:
#!/usr/bin/python2
# coding: utf-8
import sys
import rospy
from service_demo.srv import *
def radian_to_angle_client(rad):
rospy.wait_for_service('radian_to_angle')
try:
radian_to_angle = rospy.ServiceProxy('radian_to_angle', RadianToAngle)
resp = radian_to_angle(rad)
if resp.success:
return resp.angle
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if __name__ == '__main__':
if len(sys.argv) == 2:
r = float(sys.argv[1])
else:
print "Usage: rosrun <service_node_name> radian"
sys.exit(1)
print "radian: %f convert to angle is: %f"%(r,radian_to_angle_client(r))
关于如何创建自定义的Service类型的内容可参考以下链接：
ROS Action¶
ROS Action概念¶
ROS Service会阻塞程序流，程序无法进行其它的工作，有时我们需要同时进行多个任务。
ROS Action可以满足要求，ROS Action提供程序的非阻塞执行。
Action是ROS Node的通信方式之一
Action server
向ROS系统广播指定action的Node，其它Node可以向该Node发出action目标请求
Action client
发出action目标请求的Node
Action通信的特点为：
- Action是类似于Service的通信机制，也是一种请求响应机制的通信方式，ROS的action通信通过Actionlib库实现
- Action主要弥补了service通信的一个不足，就是当机器人执行一个长时间的任务时，假如利用service通信方式，那么publisher会很长时间收不到反馈的reply，致使通信受阻。
- Action适合实现长时间的通信过程，且可以随时查看过程进度，也可以终止请求
Action通信的原理为：
- Action的工作原理是client-server模式，也是一个双向的通信模式
- 通信双方在ROS Action Protocol下通过消息进行数据的交流通信
- client和server为用户提供一个简单的API在客户端请求目标或在服务器端通过函数调用和回调来执行目标
下图是ROS Node Action通信的原理示意图，我们可以看到通信双方通过ROS Action Protocol实现通信。
Action的内容格式包含三个部分：目标、反馈、结果
目标（goal）
机器人执行一个动作，应该有明确的移动目标信息，包括一些参数的设定，如方向、角度、速度等等。从而使机器人完成动作任务。
目标在完成之前可被占用或被取消。
目标可处于ACTIVE、SUCCEEDED、ABORTED等不同的状态。
反馈（feedbac）
在动作进行的过程中，应该有实时的状态信息反馈给客户端，告诉客户端动作完成的状态，可以使客户端作出准确的判断去修正命令。
结果（result）
当动作完成时，服务端把运动的结果数据发送给客户端，使客户端得到本次动作的全部信息，例如机器人的运动时长、最终位姿等。
自定义Action类型¶
Action文件
- ROS通过action文件定义Action
- Action文件通常位于<ros_package_name>/action文件夹中
- Action文件的名字通常为<custom_message_type>.action
- Action类型包含goal、feedback、result三部分
自定义Action类型
现在，我们定义一个新的计算斐波那契数列的Action类型： Fibonacci
要求包含：
- Goal - 数列包含元素个数
- Result - 求解得到的数列
- Feedback - 已经列出的数列
我们可以通过以下指令创建这个新的Action类型：
首先创建action_demo Package：
catkin_create_pkg action_demo rospy std_msgs #在tr_tutorial_ws的src空间中
cd action_demo
mkdir action
touch action/Fibonacci.action
然后在新建的action文件中输入以下内容:
#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence
编译构建自定义的Action类型
在上一部分我们定义了新的Action文件，新定义的Action经过编译构建之后才能在程序中使用。
首先修改package.xml文件，在package.xml文件中添加以下内容：
<build_depend>actionlib_msgs</build_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
然后修改CMakeLists.txt文件，在CMakeLists.txt文件中相应的位置添加以下内容：
# 在find_package中添加actionlib_msgs
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
actionlib_msgs
)
# 在add_action_files中添加Fibonacci.action
add_action_files(
FILES
Fibonacci.action
)
# 去掉generate_messages的注释，增加actionlib_msgs
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs
)
# 在catkin_package中添加actionlib_msgs
catkin_package(
CATKIN_DEPENDS rospy std_msgs actionlib_msgs
)
随后切换到tr_tutorial_ws顶层目录执行编译构建：
catkin build
编译完成之后：
source devel/setup.bash
我们可以在devel/lib/python2.7/dist-packages/action_demo/msg路径下看到生成的Fibonacci Action相关的文件。
ROS Action示例¶
定义完Action之后，我们来看一下怎样使用SimpleActionServer和SimpleActionClient写一个简单的ROS应用，包含：
- 一个Action Server，提供计算斐波那契数列的服务
- 一个Action Client，发出计算斐波那契数列的请求
首先创建脚本文件用于存放示例代码:
roscd action_demo
mkdir scripts
touch scripts/fibonacci_server.py scripts/fibonacci_client.py
Action Server Node Demo
#! /usr/bin/env python
import rospy
import actionlib
import actionlib_tutorials.msg
class FibonacciAction(object):
# create messages that are used to publish feedback/result
_feedback = actionlib_tutorials.msg.FibonacciFeedback()
_result = actionlib_tutorials.msg.FibonacciResult()
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)
self._as.start()
def execute_cb(self, goal):
# helper variables
r = rospy.Rate(1)
success = True
# append the seeds for the fibonacci sequence
self._feedback.sequence = []
self._feedback.sequence.append(0)
self._feedback.sequence.append(1)
# publish info to the console for the user
rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
# start executing the action
for i in range(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node('fibonacci')
server = FibonacciAction(rospy.get_name())
rospy.spin()
Action Client Node Demo
#! /usr/bin/env python
from __future__ import print_function
import rospy
# Brings in the SimpleActionClient
import actionlib
# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
import actionlib_tutorials.msg
def fibonacci_client():
# Creates the SimpleActionClient, passing the type of the action
# (FibonacciAction) to the constructor.
client = actionlib.SimpleActionClient('fibonacci', actionlib_tutorials.msg.FibonacciAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = actionlib_tutorials.msg.FibonacciGoal(order=20)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
# Prints out the result of executing the action
return client.get_result() # A FibonacciResult
if __name__ == '__main__':
try:
# Initializes a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
rospy.init_node('fibonacci_client_py')
result = fibonacci_client()
print("Result:", ', '.join([str(n) for n in result.sequence]))
except rospy.ROSInterruptException:
print("program interrupted before completion", file=sys.stderr)
我们可以尝试运行demo，在终端中会看到含20元素的斐波那契数列。
使用ROS Action开发ROS应用需要注意的内容：
- 我们当前使用的是Simple Action Server/Client，这是actionlib ROS package的通用action server的一种实现
- Simple Action Server/Client在同一时间只支持一个目标的非阻塞运行
- 新的目标到达会抢占已经激活的目标
- 我们可以实现自己的Action Server支持多目标的运行
ROS Action练习¶
我们已经完成了Action Server、Client的构建及定义新的Action类型，接下来我们进行一个小练习，构建一个简单的ROS应用：
- 包含一个Action Server，一个Action Client
- Action Server提供计数服务，计数完成之后反馈计数成功
- Action Client发出计数请求
Counter Action定义-示例:
uint32 num_counts
---
string result_message
---
uint32 counts_elapsed
ROS Action练习示例-Server:
#! /usr/bin/env python
import rospy
import actionlib
from action_demo.msg import CounterWithDelayAction, CounterWithDelayFeedback, CounterWithDelayResult
class CounterWithDelayActionClass(object):
# create messages that are used to publish feedback/result
_feedback = CounterWithDelayFeedback()
_result = CounterWithDelayResult()
def __init__(self, name):
# This will be the name of the action server which clients can use to connect to.
self._action_name = name
# Create a simple action server of the newly defined action type and
# specify the execute callback where the goal will be processed.
self._as = actionlib.SimpleActionServer(self._action_name, CounterWithDelayAction, execute_cb=self.execute_cb, auto_start = False)
# Start the action server.
self._as.start()
rospy.loginfo("Action server started...")
def execute_cb(self, goal):
# Define frequency
r = rospy.Rate(1)
# Variable to decide the final state of the action server.
success = True
# start executing the action
for counter_idx in range(0, goal.num_counts):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
# publish the feedback
self._feedback.counts_elapsed = counter_idx
self._as.publish_feedback(self._feedback)
# Wait for counter_delay s before incrementing the counter.
r.sleep()
if success:
self._result.result_message = "Successfully completed counting."
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
if __name__ == '__main__':
# Initialize a ROS node for this action server.
rospy.init_node('counter_with_delay')
# Create an instance of the action server here.
server = CounterWithDelayActionClass(rospy.get_name())
rospy.spin()
ROS Action练习示例-Client:
#! /usr/bin/env python
from __future__ import print_function
import rospy
# Brings in the SimpleActionClient
import actionlib
# Brings in the messages used by the CounterWithDelay action, including the
# goal message and the result message.
from action_demo.msg import CounterWithDelayAction, CounterWithDelayGoal, CounterWithDelayResult
def counter_with_delay_client():
# Creates the SimpleActionClient, passing the type of the action
# (CounterWithDelayAction) to the constructor.
client = actionlib.SimpleActionClient('counter_with_delay', CounterWithDelayAction)
# Waits until the action server has started up and started
# listening for goals.
rospy.loginfo("Waiting for action server to come up...")
client.wait_for_server()
num_counts = 10
# Creates a goal to send to the action server.
goal = CounterWithDelayGoal(num_counts)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.loginfo("Goal has been sent to the action server.")
# Waits for the server to finish performing the action.
# client.wait_for_result()
for count_idx in range(0, 10):
print("I am doing other things while the goal is being serviced by the server")
rospy.sleep(1.5)
# Prints out the result of executing the action
return client.get_result() # A CounterWithDelayResult
if __name__ == '__main__':
try:
# Initializes a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
rospy.init_node('counter_with_delay_ac')
result = counter_with_delay_client()
rospy.loginfo(result.result_message)
except rospy.ROSInterruptException:
print("program interrupted before completion", file=sys.stderr)
- 更多关于ROS Action的内容可以参考以下链接：
- http://wiki.ros.org/actionlib
ROS Launch Files¶
Launch文件介绍¶
在前面的练习中我们用rosrun命令在终端中单独启动一个节点，但一个ROS应用通常会含有多个节点，如果用rosrun启动会比较麻烦。
ROS提供了 roslaunch 工具帮助我们批量启动ROS Node。
Launch文件
- 通常位于launch文件夹中，命名为.launch文件
- Launch文件是xml格式的文件
- 批量启动ROS Nodes，并配置Node arguments, parameters, namespaces等
- 可include其它的launch文件
roslaunch命令行工具
roslaunch通过一个命令一次性启动master和多个node- 启动方式：
$ roslaunch pkg_name file_name.launch - roslaunch命令首先会自动检测系统的roscore有没有运行，如果master没有启动，那么roslaunch会首先启动master，然后再按照launch的规则执行
Launch文件示例¶
Launch文件启动单个节点示例:
<launch>
<node name="ultra_pub" pkg="topic_demo" type="ultra_pub.py"/>
</launch>
Launch文件启动多个节点示例:
<launch>
<node name="ultra_pub" pkg="topic_demo" type="ultra_pub.py"/>
<node name="ultra_sub" pkg="topic_demo" type="ultra_sub.py"/>
</launch>
Launch文件练习¶
请使用Launch文件启动前面的ROS Service和ROS Action部分构建的ROS应用。
- 更多ROS Launch相关的内容可以参考以下链接：
- http://wiki.ros.org/roslaunch
实践部分¶
目标：从零开始搭建一个基于ROS的移动机器人底盘，最终实现底盘的遥控、建图、避障和导航。
2.1.ROS基础知识回顾¶
ROS是什么¶
官方定义：ROS(Robot Operating System)是面向机器人的开源的元操作系统(meta-operating system)。它能够提供类似传统操作系统的诸多功能,如硬件抽象、底层设备控制、常用功能实现、进程间消息传递和程序包管理等。此外,它还提供相关工具和库,用于获取、编译、编辑代码以及在多个计算机之间运行程序完成分布式计算。
ROS = 管道+工具+能力+生态
ROS版本介绍¶
| ROS版本 | electric | fuerte | groovy | hydro | indigo |
|---|---|---|---|---|---|
| Ubuntu |
|
|
|
|
|
| ROS版本 | jade | kinetic | luna | melodic |
|---|---|---|---|---|
| Ubuntu |
|
|
|
|
ROS安装方法¶
官方教程具有十分详细的介绍。
步骤1 添加Ubuntu源地址：¶
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
注意： 有时使用系统默认源速度会较慢，并且缺少依赖。此时，可将Ubuntu源设置为中科大的，打开/etc/apt/sources.list文件：
$ sudo gedit /etc/apt/sources.list
改为以下内容(以Ubuntu 16.04为例)：
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-security main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-updates main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-proposed main restricted universe multiverse
deb http://mirrors.ustc.edu.cn/ubuntu/ xenial-backports main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-security main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-updates main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-proposed main restricted universe multiverse
deb-src http://mirrors.ustc.edu.cn/ubuntu/ xenial-backports main restricted universe multiverse
步骤2 设置安全公钥：¶
$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
步骤3 更新源：¶
$ sudo apt update
步骤4 安装：¶
$ sudo apt install ros-kinetic-desktop-full
或安装其它包：
$ sudo apt install ros-kinetic-ros-base
$ sudo apt install ros-kinetic-navigation
$ sudo apt install ros-kinetic-gmapping
步骤5 安装完后，导出环境变量：¶
$ source /opt/ros/kinetic/setup.bash
如果不想每次打开终端都导出环境变量，可将其添加到.bashrc文件中：
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
ROS通信机制¶
ROS整个通信机制的核心就是：发布订阅机制。例如下图：名为Talker的节点向/chatter主题发送字符串”hello”，另一个名为Listener的节点订阅/chatter主题，接收主题的字符串。
注意：每个主题跟消息类型是绑定的。比如，一个接收string类型消息的主题，是不能接收int数据类型消息的。
下面是一个更具体的例子，即底盘导航模块所用到的节点及主题。
ROS包、节点的创建¶
ROS中工作区、包、节点的概念类比：
| ROS | Visual Studio |
|---|---|
| 工作区(workspace) | 解决方案(solution) |
| 包(package) | 工程(project) |
| 节点(node) | 可执行文件(executable) |
创建工作区¶
工作区即一个目录，用mkdir命令创建即可：
$ cd ~
$ mkdir catkin_ws
在工作区目录下，创建名为src的文件夹：
$ cd ~/catkin_ws
$ mkdir src
此时的目录结构：
~
|--catkin_ws
|--src
创建ROS包¶
$ cd ~/catkin_ws/src
$ catkin_create_pkg trd_driver
此时的目录结构：
~
|--catkin_ws
|--src
|--trd_driver
|--CMakeLists.txt
|--package.xml
添加节点(C++源代码)¶
$ cd ~/catkin_ws/src/trd_driver
$ mkdir src
$ cd src
$ gedit hello_world_node.cpp
编辑hello_world_node.cpp内容:
#include "ros/ros.h"
int main() {
ROS_INFO("hello world!");
return 0;
}
修改~/catkin_ws/src/trd_driver/CMakeLists.txt内容：
$ gedit ~/catkin_ws/src/trd_driver/CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(trd_driver)
find_package(catkin REQUIRED
roscpp
)
catkin_package(CATKIN_DEPENDS
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# hello_world_node
add_executable(hello_world_node
src/hello_world_node.cpp
)
target_link_libraries(hello_world_node
${catkin_LIBRARIES})
此时的目录结构：
~
|--catkin_ws
|--src
|--trd_driver
|--CMakeLists.txt
|--package.xml
|--src
|--hello_world_node.cpp
编译运行节点¶
编译：
$ cd ~/catkin_ws
$ catkin_make
~
|--catkin_ws
|--lib #编译结果目录
|--devel #编译结果目录
|--src
|--trd_driver
|--CMakeLists.txt
|--package.xml
|--src
|--hello_world_node.cpp
- 运行(方式一)：
$ cd ~/catkin_ws
$ ./devel/lib/trd_driver/hello_world_node
- 运行(方式二)：
先将此工作区下的ROS包导出到环境变量，然后用ros run指令启动：
$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ rosrun trd_driver hello_world_node
- 运行(方式三)：
建立launch文件，用launch文件启动。
$ cd ~/catkin_ws/src/trd_driver
$ mkdir launch
$ cd launch
$ gedit trd_driver.launch
编辑launch文件内容：
<?xml version="1.0"?>
<launch>
<node pkg="trd_driver" type="hello_world_node" name="hello_world_node">
</node>
</launch>
启动launch文件（需要保证已经source当前工作区）：
$ roslaunch trd_driver trd_driver.launch
若需要打印节点的log输出，可在启动时加入 –screen 参数：
$ roslaunch trd_driver trd_driver.launch --screen
编写ROS订阅、发布节点(C++)¶
参照 hello_world_node 开发方式，创建一个名为 talker_node 的发布者节点 和一个名为 listener_node 订阅者节点。分别加入以下源代码并编译运行。
发布者节点¶
功能：以10Hz的频率，向/chatter主题发布”hello x”(x为计数)字符串。
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "talker"); //初始化节点,名称为"talker"
ros::NodeHandle nh; //创建节点句柄
ros::Publisher pub =
nh.advertise<std_msgs::String>("chatter", 1000); //创建发布者
ros::Rate loop_rate(10); //设置循环频率
int count = 0; //循环计数
while (ros::ok()){
std_msgs::String msg; //创建消息
std::stringstream ss;
ss << "Hello " << count;
msg.data = ss.str(); //消息赋值
ROS_INFO("send [%s]", msg.data.c_str());
pub.publish(msg); //发布消息
ros::spinOnce(); //非阻塞调用
loop_rate.sleep();
++count;
}
return 0;
}
订阅者节点¶
功能：订阅/chatter主题，并打印收到的内容。
#include "ros/ros.h"
#include "std_msgs/String.h"
void subCallback(const std_msgs::String::ConstPtr& msg) {
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char *argv[]) {
ros::init(argc, argv, "listener"); //初始化节点,名称为"listener"
ros::NodeHandle nh;//创建节点句柄
ros::Subscriber sub =
nh.subscribe("chatter", 1000, subCallback); //创建订阅者
ros::spin(); //阻塞调用
return 0;
}
CMakeLists.txt¶
修改 trd_driver/CMakeLists.txt 内容，添加两节点。
cmake_minimum_required(VERSION 2.8.3)
project(trd_driver)
find_package(catkin REQUIRED
roscpp
)
catkin_package(CATKIN_DEPENDS
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# hello_world_node
add_executable(hello_world_node
src/hello_world_node.cpp
)
target_link_libraries(hello_world_node
${catkin_LIBRARIES})
# talker_node
add_executable(talker_node
src/talker_node.cpp
)
target_link_libraries(talker_node
${catkin_LIBRARIES})
# listener_node
add_executable(listener_node
src/listener_node.cpp
)
target_link_libraries(listener_node
${catkin_LIBRARIES})
运行发布者和订阅者¶
为了避免每次启动终端，都要导出当前工作区环境变量， 可以将导出命令加入到当前用户的 ~/.bashrc 文件中：
$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
打开新的终端，启动 roscore ：
$ roscore
打开新的终端，启动 talker_node 节点：
$ rosrun trd_driver talker_node
打开新的终端，启动 listener_node 节点：
$ rosrun trd_driver listener_node
此时，可看到订阅者打印接收到的内容。
ROS常用指令¶
- roscore:启动ROS master主服务
- rviz:可视化工具(地图、图片、点云、机器人模型… …)
- rqt_reconfigure:配置参数界面
- rqt_graph:弹出所有节点/主题关系可视化界面
- rosnode list:列出正在运行的节点
- rosnode info node_name:打印节点信息
- rosnode kill node_name:停止某个节点
- rosrun package node:启动package包下的node节点
- rostopic list:列出正在运行的topic名字列表
- rostopic info:打印topic类型信息
- rostopic echo:打印topic实时数据信息
- roscd package:切换到package包所在路径
- rosed package abc.cpp:用编辑器打开package包下的abc.cpp文件
- rosservice list:列出正在运行的service名字列表
- rosservice info service_name:打印service类型信息
- roslaunch launch_file_name:启动launch文件
- roslaunch-deps abc.launch:列出launch文件中依赖的package
- rosmsg list:已安装msg名字列表
- rosmsg show msg_name:已安装msg类型信息
- rosmsg package 包名:包包含的所有msg名
- rospack list: 列出已安装的包
- rosstack list:列出已安装的元包
- rosbag record topic_name -O abc.bag:记录topic到abc.bag文件
- rosbag play abc.bag:播放bag文件
- roswtf:打印错误检查
- rossrv list:已安装srv名字列表
- rossrv show srv_name:已安装srv类型信息
- rossrv package 包名:包包含的所有srv名
ROS学习网站¶
- ROS维基网站： http://wiki.ros.org/
- ROS官方教程: http://wiki.ros.org/ROS/Tutorials
- ROS Answers论坛： answers.ros.org
- 创客智造： https://www.ncnynl.com/
ROS学习书籍¶
1. A Gentle Introduction to ROS¶
(英文版) 链接: https://pan.baidu.com/s/1WtsiWh7o542KCgSNMR8ugQ 密码: ci3k
(中文版) 链接: https://pan.baidu.com/s/1skP5o819hZE1OsXypFD_Gw 密码: p8f9
2. Learning ROS for Robotics Programming¶
链接: https://pan.baidu.com/s/1sjegN3w_ehvhxT00d1xBiw 密码: bnce
3. Programming Robots with ROS: A Practical Introduction to the Robot Operating System¶
链接: https://pan.baidu.com/s/1QkqD-Zu_i3KJNCn0xSsKng 密码: qd82
4. Robot Operating System (ROS)The Complete Reference¶
链接: https://pan.baidu.com/s/1d4UZd3Ast9SxPhz8HZh9gg 密码: ep42
2.2.RoboWare Studio安装及使用¶
RoboWare Studio介绍¶
RoboWare Studio是一个ROS集成开发环境。它使ROS开发更加直观、简单，并且易于操作。可进行ROS工作区及包的管理，代码编辑、构建及调试。
RoboWare Studio的主要特性有：
（1）易于安装及配置
下载后双击即可安装，RoboWare Studio可自动检测并加载ROS环境，无需额外配置。这种“开箱即用”的特性能够帮助开发者迅速上手。
（2）辅助ROS开发，兼容indigo/jade/kinetic版本
RoboWare Studio专为ROS (indigo/jade/kinetic)设计，以图形化的方式进行ROS工作区及包的创建、源码添加、message/service/action文件创建、显示包及节点列表。可实现CMakelists.txt文件和package.xml文件的自动更新。
（3）友好的编码体验
提供现代IDE的重要特性，包括语法高亮、代码补全、定义跳转、查看定义、错误诊断与显示等。支持集成终端功能，可在IDE界面同时打开多个终端窗口。支持Vim编辑模式。
（4）C++和Python代码调试
提供Release、Debug及Isolated编译选项。以界面交互的方式调试C++和Python代码，可设置断点、显示调用堆栈、单步运行，并支持交互式终端。可在用户界面展示ROS包和节点列表。
（5）远程部署及调试
可将本地代码部署到远程机器上，远程机器可以是X86架构或ARM架构。可在本地机器实现远程代码的部署、构建和实时调试。
（6）内置Git功能
Git使用更加简单。可在编辑器界面进行差异比对、文件暂存、修改提交等操作。可对任意Git服务仓库进行推送、拉取操作。
（7）遵循ROS规范
从代码创建、消息定义，到文件存储路径的创建及选择等，RoboWare Studio会引导开发者进行符合ROS规范的操作，协助开发者编写高质量、符合规范的ROS包。
RoboWare Studio软件安装¶
准备¶
安装前，请查看系统环境并确认：
（1）操作系统为Ubuntu。
（2）已完成ROS的安装配置。ROS安装步骤可参照官方网站：http://wiki.ros.org/kinetic/Installation/Ubuntu
（3）可使用catkin_make构建ROS包。（若无法构建，您可能需要运行
$ sudo apt install build-essential
来安装基本构建工具。）
（4）为支持Python相关功能，需要安装pylint。
$ sudo apt install python-pip
$ sudo python -m pip install pylint
（5）为支持clang-format相关功能，需要安装clang-format-3.8或更高版本。
$ sudo apt install clang-format-3.8
安装¶
下载RoboWare Studio最新版，在终端执行以下命令进行安装：
$ cd /path/to/deb/file/
$ sudo dpkg -i roboware-studio_<version>_<architecture>.deb
其中，<version>表示软件版本号，<architecture>表示机器的处理器架构（amd64为64位版本，i386为32位版本）。 将<version>和<architecture>替换为当前文件信息即可（小技巧：可在输入“sudo dpkg -i ”后按Tab键自动补全文件名）。 安装后，RoboWare Studio会自动检测并加载ROS环境，无需额外配置。
升级¶
下载最新的RoboWare Studio安装包deb文件，参照安装步骤直接安装即可，旧版本会被自动覆盖。
启动¶
方式一（推荐）： 点击屏幕左上角的Ubuntu图标，打开Dash，搜索“roboware-studio”，单击启动。
方式二： 通过终端启动，打开任一终端，执行：
$ roboware-studio
本地模式使用教程¶
(1)创建工作区¶
在欢迎界面，点击“新建工作区”按钮（或选择“文件 - 新建工作区”），选择路径并填写工作区名称，如“catkin_ws”，则会创建一个名为“catkin_ws”工作区，并显示在资源管理器窗口。
(2)打开/关闭工作区¶
在欢迎界面，点击“打开工作区”按钮（或选择“文件 - 打开工作区”），选择需要打开的ROS工作区，打开后即可显示在资源管理器窗口。
选择“文件 - 关闭工作区”，RoboWare Studio会关闭当前的工作区并返回欢迎界面。
(3)创建ROS包¶
右键点击ROS工作区下的“src”，选择“新建ROS包”，输入包名称及其依赖包的名称，如：
my_package roscpp std_msgs
回车后，会创建名为“my_package”、以“roscpp”和“std_msgs”为依赖的ROS包。
(4)添加新的动态链接库或可执行文件（ROS节点）¶
右键点击包名文件夹（如“my_package”），选择“新建Src文件夹”，会自动创建ROS标准的src源码目录，其它必要的目录也可通过此右键菜单来创建。
右键点击ROS包目录下的“src”，选择“新建CPP源文件”，输入文件名后，点击回车键，会弹出以下列表：
- 加入到新的库文件中
- 加入到新的可执行文件中
在列表中选择类型，则会创建一个与CPP文件同名的动态链接库或可执行文件（ROS节点），此时CMakeLists.txt文件会自动更新。
同理，右键点击ROS包目录下的“include/包名”，选择“新建头文件”，也可通过同样方式进行添加。
(5)添加C++源代码到动态链接库或可执行文件（ROS节点）¶
右键点击ROS包目录下的“src”，选择“新建CPP源文件”，输入文件名后，点击回车键，会弹出以下列表：
- my_library1
- my_library2
- my_executable1
- 加入到新的库文件中
- 加入到新的可执行文件中
其中my_library1、my_library2、my_executable1为已建立的库和可执行文件（节点），以列表的形式列出。 选择对应的条目（如my_executable1），CPP文件会添加到my_executable1可执行文件中。此时CMakeLists.txt文件会自动更新。
(6)编辑catkin ROS依赖包¶
右键点击包名文件夹（如“my_package”），选择“编辑依赖的ROS包列表”，加入新增的依赖包名称，如：
std_msgs
回车后，会自动修改CMakeLists.txt的依赖包列表，如依赖多个ROS包的时候需要用空格把每个依赖包隔开。
(7)添加message/service/action¶
右键点击包名文件夹（如“my_package”），选择“新建Msg文件夹”、“新建Srv文件夹”、 “新建Action文件夹”可分别创建message、service、action文件夹。 右键点击相应文件夹即可添加message、service、action文件。此时CMakeLists.txt文件会自动更新。
(8)构建工作区¶
RoboWare Studio支持catkin_make构建工具和catkin_tools构建工具。
选择菜单“文件 – 首选项 – 设置”可打开设置界面，点击“ROS - ros.buildTool”标签左侧的编辑标志，即可选择构建工具。
其中，Debug和Release选项分别表示构建调试版和发布版，默认构建方式为本地构建。 catkin make方式下，带有“isolated”的选项表示利用“catkin_make_isolated”命令进行构建。 带有“remote”的选项表示进行远程构建。“Remote Deploy”选项表示部署本地代码到远程计算机。 关于远程开发的具体步骤会在下一节“远程模式使用教程”介绍，在此以本地构建为例进行说明。
完成构建选项选择后，点击配置列表左侧的构建按钮，或选择“ROS”-“构建”即可构建对应版本的ROS包。构建完成后，资源管理器窗口下方的“ROS节点”子窗口会显示当前工作区下所有的ROS包及节点列表。
选择“查看 - 输出”可打开“输出”窗口，显示构建输出结果。若构建过程中出现错误，按住“CTRL”键并点击错误提示，即可跳转到源代码对应位置。
(9)构建工作区下的一个或多个包¶
默认情况下，点击“构建”按钮会构建当前工作区下的所有包。如果只想构建其中的一个或多个包，可右键点击包名，将其设置为活动状态。可同时将一个或多个包设置为活动状态。此时，不被编译的包即称为“非活动包”，在目录列表中将会以删除线标记出来。点击“构建”按钮，RoboWare Studio只会对处于活动状态的包进行构建。
(11)集成终端使用¶
选择“查看 - 集成终端”选项，可在编辑窗口下方打开集成终端窗口。集成终端默认打开路径为当前ROS工作区根目录。可在集成终端中执行任意命令行指令。
可以点击集成终端窗口右上方的“+”按钮打开新的集成终端，并在下拉列表中进入对应的集成终端。
(12)添加并启动launch文件¶
首先，右键点击包名文件夹（如“my_package”），选择“新建Launch文件夹”可创建launch文件夹。然后，右键点击launch文件夹，输入文件名添加launch文件。
编辑完后，右键launch文件，选择“运行Launch文件”即可，RoboWare Studio会自动打开集成终端并运行launch文件。
(13)编辑~/.bashrc文件¶
选择菜单“ROS – 打开~/.bashrc文件”即可打开并编辑.bashrc文件。
远程模式使用教程¶
(1)配置SSH公钥无密登录¶
首先，在本地计算机生成公钥和私钥。打开终端，执行命令：
$ ssh-keygen
一直按回车键选择默认选项，会在~/.ssh目录下生成id_rsa和id_rsa.pub两个文件。然后将id_rsa.pub文件复制到远程计算机：
$ scp ~/.ssh/id_rsa.pub username@ip_address:/home/username
其中username为远程计算机用户名，ip_address为远程计算机的IP地址，示例如下所示。
将公钥文件id_rsa.pub拷贝到远程计算机后，SSH登录到远程计算机：
$ ssh username@ip_address
其中username为远程计算机用户名，ip_address为远程计算机的IP地址。
登录后，将id_rsa.pub的文件内容追加写入到远程计算机的~/.ssh/authorized_keys文件中，并修改authorized_keys文件的权限：
$ cat id_rsa.pub >> ~/.ssh/authorized_keys
$ chmod 600 ~/.ssh/authorized_keys
配置完成后，再登录远程计算机就无需输入密码。接下来，即可配置RoboWare Studio的远程调试参数进行远程调试。
(2)修改远程计算机/etc/profile¶
首先，登录远程计算机：
$ ssh username@ip_address
其中username为远程计算机用户名，ip_address为远程计算机的IP地址。
登录后，切换到root用户权限，将ROS环境变量信息写入到/etc/profile文件中：
$ sudo su
$ echo "source /opt/ros/kinetic/setup.bash" >> /etc/profile
在此需要注意，示例中的ROS版本为“kinetic”，对于其它版本替换为对应名称即可。
(3)远程参数配置¶
启动RoboWare Studio后，点击“远程参数配置”按钮，依次配置远程计算机IP地址、远程计算机用户名、本地计算机密钥文件、远程计算机部署路径参数。
(4)远程部署¶
完成远程参数配置后，再进行远程部署。
首先，在资源管理器视图下，选择“Remote Deploy”远程部署选项，点击列表左侧的按钮进行远程部署。RoboWare Studio会将当前整个工作区的源代码部署到远程计算机的指定路径下（请参照上一节的“远程参数配置”进行远程部署路径的设置）。
在远程部署过程中，左下角状态栏图标会跳动。部署完成后，会在“输出”窗口显示部署成功的信息（Deploy Finished!）。
(5)远程构建¶
完成远程部署后，即可进行远程构建。远程构建与本地构建一样，可以选择catkin_make和catkin_tools两种构建方式，若选择catkin_tools构建方式，需要在远程机器上安装catkin_tools工具。在此仅以catkin_make构建方式进行说明。
首先，在资源管理器视图下，选择“Debug (remote)”构建选项，点击列表左侧的按钮进行远程构建。RoboWare Studio会将构建指令发送到远程计算机，并在“输出”窗口显示构建信息。
(6)远程清理¶
远程构建完成后，资源管理器窗口下方的“ROS节点”子窗口会显示远程工作区下所有的ROS包及节点列表。
点击“ROS节点”子窗口上的“清理”按钮，则会对远程构建结果进行清理。
在此需要注意，点击“清理”按钮，左上角的配置构建选项中如果为“remote”选项，则会清除远程构建结果，否则，则清除本地构建结果。
(7)远程部署/构建一个或多个包¶
默认情况下，部署时会将当前工作区下的所有包部署到远程主机。如果只想部署其中的一个或多个包，可右键点击包名，将其设置为活动状态。可同时将一个或多个包设置为活动状态。此时，进行部署时，RoboWare Studio只会将处于活动状态的包部署到远程主机。
与本地构建方式相同，当设置一个或多个活动包时，点击“构建”按钮，只会对处于活动状态的包进行构建。当清除所有包的活动状态后，则会回到初始状态（即所有包都处于非活动状态）。当所有包都处于非活动状态时，点击“构建”按钮，则会对当前工作区内的所有包进行构建。
(8)远程启动launch文件¶
右键launch文件，选择“在远程主机上运行Launch文件”，RoboWare Studio会在集成终端中启动远程主机的launch文件。在集成终端中使用“Ctrl + c”快捷键可终止运行。
软件首选项配置¶
RoboWare Studio软件可方便地进行的首选项配置，以点选的方式实现用户配置、工作区配置以及主题配置等。选择“文件 – 首选项 – 设置”即可打开配置界面。
FAQ¶
(1)如何导入已有的ROS工作区？
分两种情况：
- 对于普通的ROS工作区，直接在欢迎界面点击“打开工作区”按钮（或在菜单中选择“文件 - 打开工作区”），选择工作区路径打开即可。
- 对于旧版本RoboWare Studio打开过的ROS工作区，需要将工作区根目录下的“.vscode”文件夹删除，再打开工作区即可。
(2)如何进行软件升级？
下载最新的RoboWare Studio安装包deb文件，参照安装步骤直接安装即可，旧版本会被自动覆盖。
(3)如何修改界面语言？
在菜单栏中，选择“文件-首选项-语言设置”，打开配置文件。
"locale":"zh-CN" 表示设置为中文界面，
"locale":"en" 表示设置为英文界面，
可用“//”进行注释。修改完成后，重启RoboWare Studio即可生效。
(4)新建工作区时提示“路径不是ROS工作区”。
可能原因为ROS环境变量未导出。安装完ROS后，需要在当前用户家目录下的.bashrc文件中添加以下内容：
source /opt/ros/indigo/setup.bash #（indigo版）
或
source /opt/ros/kinetic/setup.bash #（kinetic版）
ROS的安装及配置说明可参照官网教程：
http://wiki.ros.org/indigo/Installation/Ubuntu （indigo版）
http://wiki.ros.org/kinetic/Installation/Ubuntu （kinetic版）
(5)提示错误“Linter pylint is not installed”。
需要安装pylint，请参照本手册“软件安装”-“准备”中的步骤。打开命令行终端，执行以下命令：
$ sudo apt install python-pip
在集成终端中安装python插件时会提示输入root密码，请按照提示输入。
(6)提示系统git版本低的问题。
需要升级git。打开命令行终端，执行以下命令：
$ sudo apt-add-repository ppa:git-core/ppa
$ sudo apt update
$ sudo apt install git
(7)名为“test”的节点无法生成问题。
不要将ROS节点名称命名为test，否则会无法生成节点。
(8)构建过程中卡住问题。
对于内存太小的机器，如果当前工作区内的ROS包数量太多，在构建整个工作区时，会因内存不足导致构建卡住。此时，可采用单独构建的方式依次对每个包构建。
(9)新建、删除文件时资源管理器不能自动刷新。
RoboWare Studio依靠ubuntu的文件系统监视功能实现资源管理器的自动刷新， 但ubuntu的文件监视数量限制可能设置的太小，导致资源管理器没有自动刷新。 为解决这个问题需要设置ubuntu的文件监视数量限制。方法如下：
使用root权限打开文件/etc/sysctl.conf进行编辑。
$ sudo gedit /etc/sysctl.conf
也可用其它文本编辑器。
找到fs.inotify.max_user_watches选项，适当增加等号后面的数值，比如改为100000。重启ubuntu即可。
(10)编辑器无法输入、选择、复制问题。
这是因为RoboWare Studio处于Vim编辑模式，切换到普通模式即可，在菜单栏选择“编辑-切换VIM编辑模式”可切换到普通编辑模式。
(11)编辑时如何进行前进、后退？能否自定义其快捷键？
默认的前进快捷键为“Ctrl+Shift+-”，后退快捷键为“Ctrl+Alt+-”。 可以进行快捷键自定义，点击菜单栏的“文件 - 首选项 – 键盘快捷方式”，找到“前进”、“后退”选项进行修改即可，也可对其它快捷键进行自定义。
(12)CMakelists.txt错误无法定位问题。
先删除工作区根目录.vscode文件夹下的tasks.json文件，然后重新打开工作区。
(13)Meta Package无法编辑依赖、无法新建节点问题。
目前尚不支持meta package编辑依赖和新建节点，将meta package下的所有包拷贝到src目录下即可。
2.3.让底盘动起来！差速控制模型及串口驱动编写¶
本节将编写一个差速底盘的驱动程序，采用串口通信方式，主要实现以下功能：
- 订阅ROS其它节点发布的速度指令，控制底盘运行；
- 读取底盘两个轮子的编码器数据，并解算成底盘的位置坐标。
用Python写一个简单的ROS发布者节点¶
首先，在trd_driver包的src目录下编写一个用Python写的ROS节点，命名为trd_driver.py，此节点以10Hz的频率，将ROS中nav_msgs包下的Odometry类型消息发布到名为/odom的主题上。
Odometry消息类型定义可查看ROS官方文档http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html。
注意：需将trd_driver.py文件添加执行权限后才可运行：
$ roscd trd_driver/src/
$ chmod +x trd_driver.py
#!/usr/bin/python2
import rospy
from nav_msgs.msg import Odometry
if __name__=='__main__':
rospy.init_node('trd_driver_node')
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
current_odom = Odometry()
odom_pub.publish(current_odom)
rate.sleep()
此时，先启动roscore，然后打开新的终端，执行：
$ cd ~/catkin_ws/src/trd_driver/src/
$ ./trd_driver.py
另一种执行方式是用rosrun指令，此命令可在任意路径下执行，无需进入到trd_driver.py所在路径。
$ rosrun trd_driver trd_driver.py
运行此节点后，利用rostopic命令查看节点发布的主题：
$ rostopic list
$ rostopic echo /odom
此时，可看到终端不停地打印出/odom主题，由于未对其赋值，所以默认值均为0。
在节点中添加速度订阅者，并接收键盘控制的速度指令¶
然后，在节点文件中加入一个订阅者，订阅名为/cmd_vel主题的消息，消息类型为geometry_msgs包下的Twist类型，为ROS中专门描述速度的消息。
Twist消息类型定义可查看ROS官方文档http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html。
#!/usr/bin/python2
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
def vel_callback(msg):
print(msg.linear.x, msg.angular.z)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
vel_sub = rospy.Subscriber('/cmd_vel', Twist, vel_callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
current_odom = Odometry()
odom_pub.publish(current_odom)
rate.sleep()
然后，在src目录下新建名为keyboard_teleop.py的ROS节点（记得修改执行权限），作用是将键盘控制信号发布到/cmd_vel速度主题。 （其源代码可到此处下载。)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Control Your Turtlebot!
---------------------------
Moving around:
u i o
j k l
m , .
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%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
moveBindings = {
'i':(1,0),
'o':(1,-1),
'j':(0,1),
'l':(0,-1),
'u':(1,1),
',':(-1,0),
'.':(-1,1),
'm':(-1,-1),
}
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.5
turn = 1
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('teleop_key')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
x = 0
th = 0
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()
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]
turn = turn * speedBindings[key][1]
count = 0
print vels(speed,turn)
if (status == 14):
print msg
status = (status + 1) % 15
elif key == ' ' or key == 'k' :
x = 0
th = 0
control_speed = 0
control_turn = 0
else:
count = count + 1
if count > 4:
x = 0
th = 0
if (key == '\x03'):
break
target_speed = speed * x
target_turn = turn * th
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.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)
#print("loop: {0}".format(count))
#print("target: vx: {0}, wz: {1}".format(target_speed, target_turn))
#print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))
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)
在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i
, j l键盘发布速度消息，观察trd_driver.py的输出。
$ rosrun trd_driver trd_driver.py
$ rosrun trd_driver keyboard_teleop.py
将上述代码封装到类中¶
为了便于下一步开发，将所有功能封装成类比较好。
#!/usr/bin/python2
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
class TrdDriver():
def __init__(self):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
current_odom = Odometry()
self.odom_pub.publish(current_odom)
rate.sleep()
def vel_callback(self,msg):
print(msg.linear.x, msg.angular.z)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
trd_driver = TrdDriver()
trd_driver.run()
在节点中加入串口驱动程序，控制底盘转动¶
TRD电机驱动板卡采用串口控制，其通信方式如下：
I.发送¶
| 字节 | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
|---|---|---|---|---|---|---|---|
| 内容 | 0xea | 0x05 | 0x7e | 速度1 | 速度2 | 校验 | 0x0d |
发送指令包含7个字节，头尾分别为0xea和0x0d，第4位、第5位分别表示两个电机的速度， 均用一个字节表示。数值为128时速度为零，大于128时电机正转，数值越大速度越快；小于 128时电机反转，数值越小速度越快。其中，第6位为前1~5位字节校验和（求异或）。
II.接收¶
每次发送速度控制指令后，电机驱动板卡都会回应以下信息。 接收到的数据包含23个字节，头尾分别为0xea和0x0d，4~21位描述了当前电机状态。 除了编码器均用一个字节来表示。两个电机编码器的数值分别用4个字节表示，为大端法表示的有符号整形。 其中，第20位为前1~19位字节校验和（求异或）。
| 字节 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 |
|---|---|---|---|---|---|---|---|---|---|
| 内容 | 0xea | 0x13 | 0x7e | 电压 | 电流1 | 电流2 | 错误码 | 速度1 | 速度2 |
| 10 | 11 | 12~15 | 16~19 | 20-21 | 22 | 23 |
|---|---|---|---|---|---|---|
| 实际速度1 | 实际速度2 | 编码器1(大端int) | 编码器2(大端int) | 未用 | 校验 | 0x0d |
将底盘TRD电机驱动板卡的串口转USB线连接到电脑的USB接口，此时会出现”/dev/ttyUSB0”的串口设备。
$ ls /dev/ttyUSB*
在节点中，引入python的serial串口驱动包，打开”/dev/ttyUSB0”串口，波特率为38400。 然后根据上述通信协议，向电机驱动板卡发送速度控制指令。（可修改源码中的速度值改变电机转速）
#!/usr/bin/python2
# coding: utf-8
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import serial
class TrdDriver():
def __init__(self):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
self.ser = serial.Serial('/dev/ttyUSB0', 38400)
def send(self, cmd):
self.ser.write(cmd)
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
for i in range(len(cmd)-2):
cmd[-2] ^= cmd[i]
print('send cmd:', cmd)
self.send(cmd)
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
current_odom = Odometry()
self.odom_pub.publish(current_odom)
rate.sleep()
def vel_callback(self, msg):
print(msg.linear.x, msg.angular.z)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
trd_driver = TrdDriver()
trd_driver.set_speed(128, 128)
trd_driver.run()
将当前用户添加到dialout用户组¶
通常，普通用户对串口设备没有读写权限，如果存在这样的情况，一种方式是通过chmod指令修改串口权限，但这样每次重启系统要重新执行。
sudo chmod 775 /dev/ttyUSB0
要想一劳永逸，最好将当前用户添加到dialout用户组：
sudo usermod -a -G dialout $USER
利用键盘控制底盘运动¶
在订阅/cmd_vel的回调函数中，根据接收到的速度数据控制底盘运动。
其中，msg.linear.x表示前进或后退速度，单位为m/s。msg.angular.z表示旋转速度，单位为rad/s。
#!/usr/bin/python2
# coding: utf-8
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import serial
import time
class TrdDriver():
def __init__(self):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
self.ser = serial.Serial('/dev/ttyUSB0', 38400)
self.v1 = 128
self.v2 = 128
self.linear_coef = 82.0
self.angular_coef = 14.6
def send(self, cmd):
self.ser.write(cmd)
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
for i in range(len(cmd)-2):
cmd[-2] ^= cmd[i]
print('send cmd:', cmd)
self.send(cmd)
def read_buffer(self):
result = ''
while self.ser.inWaiting() > 0:
result += self.ser.read(1)
return result
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.set_speed(self.v1, self.v2)
time.sleep(0.05)
result = self.read_buffer()
print('response:', result)
current_odom = Odometry()
self.odom_pub.publish(current_odom)
rate.sleep()
def vel_callback(self, msg):
v1 = self.linear_coef * msg.linear.x
v2 = self.linear_coef * msg.linear.x
v1 -= self.angular_coef * msg.angular.z
v2 += self.angular_coef * msg.angular.z
v1 += 128
v2 += 128
v1 = int(v1) if v1<255 else 255
v2 = int(v2) if v2<255 else 255
v1 = int(v1) if v1>0 else 0
v2 = int(v2) if v2>0 else 0
self.v1 = v1
self.v2 = v2
if __name__=='__main__':
rospy.init_node('trd_driver_node')
trd_driver = TrdDriver()
trd_driver.run()
运行脚本会打印如下消息：
('send cmd:', [234, 5, 126, 128, 128, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x80\x80\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 129, 129, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x81\x81\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 131, 131, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x83\x83\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 132, 132, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x84\x84\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 134, 134, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x86\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 134, 137, 158, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x89\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x16\r')
('send cmd:', [234, 5, 126, 134, 140, 155, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x8c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x13\r')
在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i
, j l键盘发布速度消息，观察底盘运动情况。
$ rosrun trd_driver trd_driver.py
$ rosrun trd_driver keyboard_teleop.py
基于双轮差速模型的底盘里程计计算¶
差速模型示意图：
根据此模型，实现update_odom方法，可根据获取到的编码器值，计算底盘实时里程，并发布到/odom主题。
#!/usr/bin/python2
# coding: utf-8
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
import serial
import time
import struct
import math
class TrdDriver():
def __init__(self, serialport, baudrate):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
self.ser = serial.Serial(serialport, baudrate)
self.v1 = 128
self.v2 = 128
self.linear_coef = 100.0
self.angular_coef = 10.0
self.wheel_diameter = 0.08
self.base_width = 0.21
self.encoder_ticks_per_rev = 1980
self.encoder1 = 0
self.encoder2 = 0
self.encoder1_prev = 0
self.encoder2_prev = 0
self.x = 0
self.y = 0
self.theta = 0
self.odom = Odometry()
self.odom.header.frame_id = 'odom'
self.odom.child_frame_id = 'base_link'
self.time_prev = rospy.Time.now()
def send(self, cmd):
self.ser.write(cmd)
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
for i in range(len(cmd)-2):
cmd[-2] ^= cmd[i]
print('send cmd:', cmd)
self.send(cmd)
def read_buffer(self):
result = ''
while self.ser.inWaiting() > 0:
result += self.ser.read(1)
return result
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.set_speed(self.v1, self.v2)
time.sleep(0.05)
result = self.read_buffer()
print('response:', result)
#提取编码器数值
self.encoder1, = struct.unpack('>i', bytearray(result[11:15]))
self.encoder2, = struct.unpack('>i', bytearray(result[15:19]))
print('encoder:', self.encoder1, self.encoder2)
#计算新的里程计并发布
self.update_odom()
def vel_callback(self, msg):
v1 = self.linear_coef * msg.linear.x
v2 = self.linear_coef * msg.linear.x
v1 -= self.angular_coef * msg.angular.z
v2 += self.angular_coef * msg.angular.z
v1 += 128
v2 += 128
v1 = int(v1) if v1<255 else 255
v2 = int(v2) if v2<255 else 255
v1 = int(v1) if v1>0 else 0
v2 = int(v2) if v2>0 else 0
self.v1 = v1
self.v2 = v2
def update_odom(self):
encoder1 = self.encoder1
encoder2 = self.encoder2
time_current = rospy.Time.now()
time_elapsed = (time_current - self.time_prev).to_sec()
self.time_prev = time_current
dleft = math.pi * self.wheel_diameter * \
(encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
dright = math.pi * self.wheel_diameter * \
(encoder2 - self.encoder2_prev) / self.encoder_ticks_per_rev
self.encoder1_prev = encoder1
self.encoder2_prev = encoder2
d = (dleft + dright) / 2
dtheta = (dright - dleft) / self.base_width
if d != 0:
dx = math.cos(dtheta) * d
dy = -math.sin(dtheta) * d
self.x += dx*math.cos(self.theta)-dy*math.sin(self.theta)
self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
self.theta += dtheta
self.odom.header.stamp = time_current
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
q = tf.transformations.quaternion_from_euler(0,0,self.theta)
self.odom.pose.pose.orientation.x = q[0]
self.odom.pose.pose.orientation.y = q[1]
self.odom.pose.pose.orientation.z = q[2]
self.odom.pose.pose.orientation.w = q[3]
self.odom.twist.twist.linear.x = d / time_elapsed
self.odom.twist.twist.angular.z = dtheta / time_elapsed
self.odom_pub.publish(self.odom)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
serialport = rospy.get_param('~serialport', default='/dev/ttyUSB0')
baudrate = rospy.get_param('~baudrate', default=38400)
trd_driver = TrdDriver(serialport, baudrate)
trd_driver.run()
在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i
, j l键盘发布速度消息。 并查看/odom消息内容更新情况。
$ rosrun trd_driver trd_driver.py
$ rosrun trd_driver keyboard_teleop.py
$ rostopic echo /odom
用launch文件启动并加载参数¶
在trd_driver包下launch文件夹中，修改trd_driver.launch文件，将启动的节点改为 trd_driver.py 。
另外，可以将串口名和波特率传入节点。
<?xml version="1.0"?>
<launch>
<node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
<param name="serialport" value="/dev/ttyUSB0" />
<param name="baudrate" value="38400" />
</node>
</launch>
考虑到trd_driver.launch文件可能会被传入参数，或被其它launch文件调用，所以最好采用arg标签指定。
<?xml version="1.0"?>
<launch>
<arg name="serialport" default="/dev/ttyUSB0" />
<arg name="baudrate" default="38400" />
<node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
<param name="serialport" value="$(arg serialport)" />
<param name="baudrate" value="$(arg baudrate)" />
</node>
</launch>
2.4.将程序放到底盘上以及运动模型的标定¶
本节介绍如何将电脑上的程序放到底盘的板卡操作系统上运行，并通过远程进行开发及调试，多个ROS主机的网络配置以及通信。然后在上一节实现的差速控制模型的基础上，介绍如何进行底盘运动的标定，让底盘按照指定的线速度及角速度进行准确的运动，以及对里程进行精确的计算。
网络架构¶
上一节中，通过笔记本电脑的USB口直接跟底盘控制器进行串口通信，发送并接受指令。架构如下：
在实际使用过程中，是不可能让笔记本拖着线跟底盘一起跑的，所以在底盘上安装一个嵌入式板卡–树莓派（Raspberry Pi 3）。 与我们笔记本电脑X86架构不同，这是一款基于ARM架构的板卡，也装有Ubuntu操作系统，并安装了ROS。 处理能力也比较强悍，1G内存，4核处理器。与X86架构相比优势是低功耗，体积小巧，方便安装在底盘上。
在开发和使用过程中，将笔记本电脑跟树莓派嵌入式板卡通过网络连接起来，进行远程控制和调试，架构如下：
远程开发模式¶
安装上图网络连接好后，在笔记本电脑上，连接到名为”Venus-05”的路由器上，密码为：12345678。
连接后，执行：
$ ifconfig
即可看到本机的IP地址，例如，此处我的笔记本电脑显示如下信息：
wlp3s0 Link encap:Ethernet HWaddr b8:ee:65:28:f3:ae
inet addr:192.168.5.100 Bcast:192.168.5.255 Mask:255.255.255.0
可见IP地址为192.168.5.100。接下来可使用nmap指令列出路由器所在网段下连接的所有设备的IP：
$ nmap -sP 192.168.5.0/24
如果提示 找不到nmap指令 ，则需要先安装nmap。联网后，执行以下指令：
$ sudo apt install nmap
会得到以下输出：
Starting Nmap 7.01 ( https://nmap.org ) at 2019-01-15 14:03 CST
Nmap scan report for 192.168.5.1
Host is up (0.048s latency).
Nmap scan report for 192.168.5.100
Host is up (0.00086s latency).
Nmap scan report for 192.168.5.108
Host is up (0.011s latency).
Nmap done: 256 IP addresses (3 hosts up) scanned in 16.81 seconds
此时会列出树莓派的IP地址为192.168.5.108，此时可通过SSH进行连接（登录用户名为：pi，密码为：1）。并在板卡上建立一个名为tutorial_ws的空文件夹。
$ ssh pi@192.168.5.108
(远程终端)$ mkdir tutorial_ws
首次连接时，会出现询问：
The authenticity of host '192.168.5.108 (192.168.5.108)' can't be established.
ECDSA key fingerprint is SHA256:EfOywkQj6mliIyg8KumdsZP/paXBmmdj00/NxPX0h6Y.
Are you sure you want to continue connecting (yes/no)?
则输入 yes 并回车确定。然后输入登录密码。
在多数情况下，我们可以在笔记本电脑上进行开发，然后使用rsync指令将结果部署到底盘板卡上。 例如在笔记本电脑的catkin_ws路径下，将src文件夹(包含了所有的ROS包源代码)部署到远程板卡的tutorial_ws目录下。
$ cd ~/catkin_ws
$ rsync -avz --delete --exclude="*.swp" src pi@192.168.5.108:/home/pi/tutorial_ws/
这段指令的意思将本机的当前路径的src文件夹，拷贝到远程板卡的/home/pi/tutorial_ws文件夹下。 使用rsync而不是scp的好处是，rsync只会拷贝差异，当只改变个别文件时，只会将改变的文件拷贝到远程，而不是全部拷贝。 这里–delete选项是指本地删除某个文件，同步后远程也会删除此文件，–exclude是指不会拷贝后缀名为swp的文件(swp为临时打开文件)。
部署完成后，可在远程板卡的tutorial_ws路径下执行catkin_make进行编译构建。
(远程终端)$ cd ~/tutorial_ws
(远程终端)$ catkin_make
利用树莓派板卡控制底盘运动¶
将源代码部署到远程板卡，构建成功后，还需要将工作区导出到环境变量，方法与在笔记本电脑相同，即：
(远程终端)$ echo "source ~/tutorial_ws/devel/setup.bash" >> ~/.bashrc
(远程终端)$ source ~/.bashrc
此时可在远程终端执行ROS节点。先启动roscore：
(远程终端)$ roscore
然后分别启动trd_driver.py底盘驱动节点，和keyboard_teleop.py键盘控制节点。
打开一个新终端，登录到树莓派，执行：
(远程终端)$ rosrun trd_driver trd_driver.py
打开一个新终端，登录到树莓派，执行：
(远程终端)$ rosrun trd_driver keyboard_teleop.py
此时可通过i , j l k按键分别控制底盘前进、后退、左转、右转和停止。
此处，keyboard_teleop.py节点控制底盘运动，会发现速度从0增加到最大值时间较长，也就是加速较慢，存在控制延迟。 仔细查看此节点代码，需要修改什么参数可以使加速度较快，或瞬间加速到最大值。
控制延迟问题¶
此时遥控底盘运动，当keyboard_teleop.py发送频率大于trd_driver循环频率是会发生什么情况？
当停止发送指令时，trd_driver订阅速度的话题队列为非空，会继续接收并执行队列中的消息。
可查看Subscriber方法中queue_size参数的定义。
@param queue_size: maximum number of messages to receive at
a time. This will generally be 1 or None (infinite,
default).
此时，需要将queue_size改为1，即可解决控制延迟问题。
编码器初始偏置问题¶
前提：除非重启运动控制板卡，否则编码器的读数是不会归零的。
这一前提会给调试带来不便，想想为什么。
下面我们在代码中加入两个变量来存储每次启动时的初始偏置，用读到的数值减去偏置即可得到相对编码器读数。
修改后的代码如下：
#!/usr/bin/python2
# coding: utf-8
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
import serial
import time
import struct
import math
class TrdDriver():
def __init__(self, serialport, baudrate):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
self.ser = serial.Serial(serialport, baudrate)
self.v1 = 128
self.v2 = 128
self.linear_coef = 100.0
self.angular_coef = 10.0
self.wheel_diameter = 0.08
self.base_width = 0.21
self.encoder_ticks_per_rev = 1980
self.encoder1 = 0
self.encoder2 = 0
self.encoder1_prev = 0
self.encoder2_prev = 0
self.x = 0
self.y = 0
self.theta = 0
self.odom = Odometry()
self.odom.header.frame_id = 'odom'
self.odom.child_frame_id = 'base_link'
self.time_prev = rospy.Time.now()
self.is_first_time = True
self.encoder1_offset = 0
self.encoder2_offset = 0
def send(self, cmd):
self.ser.write(cmd)
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
for i in range(len(cmd)-2):
cmd[-2] ^= cmd[i]
print('send cmd:', cmd)
self.send(cmd)
def read_buffer(self):
result = ''
while self.ser.inWaiting() > 0:
result += self.ser.read(1)
return result
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.set_speed(self.v1, self.v2)
time.sleep(0.05)
result = self.read_buffer()
print('response:', result)
#提取编码器数值
self.encoder1, = struct.unpack('>i', bytearray(result[11:15]))
self.encoder2, = struct.unpack('>i', bytearray(result[15:19]))
print('encoder:', self.encoder1, self.encoder2)
if self.is_first_time:
self.encoder1_offset = self.encoder1
self.encoder2_offset = self.encoder2
self.is_first_time = False
self.encoder1 -= self.encoder1_offset
self.encoder2 -= self.encoder2_offset
print('encoder-offset:', self.encoder1, self.encoder2)
#计算新的里程计并发布
self.update_odom()
def vel_callback(self, msg):
v1 = self.linear_coef * msg.linear.x
v2 = self.linear_coef * msg.linear.x
v1 -= self.angular_coef * msg.angular.z
v2 += self.angular_coef * msg.angular.z
v1 += 128
v2 += 128
v1 = int(v1) if v1<255 else 255
v2 = int(v2) if v2<255 else 255
v1 = int(v1) if v1>0 else 0
v2 = int(v2) if v2>0 else 0
self.v1 = v1
self.v2 = v2
def update_odom(self):
encoder1 = self.encoder1
encoder2 = self.encoder2
time_current = rospy.Time.now()
time_elapsed = (time_current - self.time_prev).to_sec()
self.time_prev = time_current
dleft = math.pi * self.wheel_diameter * \
(encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
dright = math.pi * self.wheel_diameter * \
(encoder2 - self.encoder2_prev) / self.encoder_ticks_per_rev
self.encoder1_prev = encoder1
self.encoder2_prev = encoder2
d = (dleft + dright) / 2
dtheta = (dright - dleft) / self.base_width
if d != 0:
dx = math.cos(dtheta) * d
dy = -math.sin(dtheta) * d
self.x += dx*math.cos(self.theta)-dy*math.sin(self.theta)
self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
self.theta += dtheta
self.odom.header.stamp = time_current
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
q = tf.transformations.quaternion_from_euler(0,0,self.theta)
self.odom.pose.pose.orientation.x = q[0]
self.odom.pose.pose.orientation.y = q[1]
self.odom.pose.pose.orientation.z = q[2]
self.odom.pose.pose.orientation.w = q[3]
self.odom.twist.twist.linear.x = d / time_elapsed
self.odom.twist.twist.angular.z = dtheta / time_elapsed
self.odom_pub.publish(self.odom)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
serialport = rospy.get_param('~serialport', default='/dev/ttyUSB0')
baudrate = rospy.get_param('~baudrate', default=38400)
trd_driver = TrdDriver(serialport, baudrate)
trd_driver.run()
硬件参数的测量¶
在位置解算时，需要知道轮子的直径，以及两个轮子之间的距离。 在代码中分别对应wheel_diameter和base_width参数。 测量后，轮子的直径为8cm，两轮子距离为21cm。修改以下代码：
self.wheel_diameter = 0.08
self.base_width = 0.21
另外，根据编码器型号，我们可以查到其每转1圈会有1980个脉冲，修改以下代码：
self.encoder_ticks_per_rev = 1980
底盘线速度标定¶
- 首先查看底盘行走一段距离，编码器计算的到的/odom里程数据准不准，如何查看？
- 记下实际运行距离，以及里程计输出的距离，需要修改那些参数使得两个距离一致？
- 如何标定线速度控制系数？
底盘角速度标定¶
- 参考线速度标定步骤，如何标定角速度控制系数？(提示：/odom里程消息中有解算得到的速度信息)
综合测试¶
- 如何测试标定结果是否准确？
2.5.标定结果测试¶
本节将对上节标定的运动参数进行多方位测试，将所有参数放到launch文件中进行修改调试。待参数满足要求精度后，即可作为底盘的驱动程序。
将所有参数放到launch文件中¶
修改trd_driver.py文件，将所有参数用ROS的get_param()方法获取：
# 使用ROS param设定参数值
self.linear_coef = rospy.get_param('~linear_coef', default=100.0)
self.angular_coef = rospy.get_param('~angular_coef', default=10.0)
self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.08)
self.base_width = rospy.get_param('~base_width', default=0.21)
self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=1980)
修改后内容如下：
#!/usr/bin/python2
# coding: utf-8
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
import serial
import time
import struct
import math
class TrdDriver():
def __init__(self, serialport, baudrate):
self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
self.ser = serial.Serial(serialport, baudrate)
self.v1 = 128
self.v2 = 128
# 使用ROS param设定参数值
self.linear_coef = rospy.get_param('~linear_coef', default=100.0)
self.angular_coef = rospy.get_param('~angular_coef', default=10.0)
self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.08)
self.base_width = rospy.get_param('~base_width', default=0.21)
self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=1980)
self.encoder1 = 0
self.encoder2 = 0
self.encoder1_prev = 0
self.encoder2_prev = 0
self.x = 0
self.y = 0
self.theta = 0
self.odom = Odometry()
self.odom.header.frame_id = 'odom'
self.odom.child_frame_id = 'base_link'
self.time_prev = rospy.Time.now()
self.is_first_time = True
self.encoder1_offset = 0
self.encoder2_offset = 0
def send(self, cmd):
self.ser.write(cmd)
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
for i in range(len(cmd)-2):
cmd[-2] ^= cmd[i]
print('send cmd:', cmd)
self.send(cmd)
def read_buffer(self):
result = ''
while self.ser.inWaiting() > 0:
result += self.ser.read(1)
return result
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.set_speed(self.v1, self.v2)
time.sleep(0.05)
result = self.read_buffer()
print('response:', result)
#提取编码器数值
self.encoder1, = struct.unpack('>i', bytearray(result[11:15]))
self.encoder2, = struct.unpack('>i', bytearray(result[15:19]))
print('encoder:', self.encoder1, self.encoder2)
if self.is_first_time:
self.encoder1_offset = self.encoder1
self.encoder2_offset = self.encoder2
self.is_first_time = False
self.encoder1 -= self.encoder1_offset
self.encoder2 -= self.encoder2_offset
print('encoder-offset:', self.encoder1, self.encoder2)
#计算新的里程计并发布
self.update_odom()
def vel_callback(self, msg):
v1 = self.linear_coef * msg.linear.x
v2 = self.linear_coef * msg.linear.x
v1 -= self.angular_coef * msg.angular.z
v2 += self.angular_coef * msg.angular.z
v1 += 128
v2 += 128
v1 = int(v1) if v1<255 else 255
v2 = int(v2) if v2<255 else 255
v1 = int(v1) if v1>0 else 0
v2 = int(v2) if v2>0 else 0
self.v1 = v1
self.v2 = v2
def update_odom(self):
encoder1 = self.encoder1
encoder2 = self.encoder2
time_current = rospy.Time.now()
time_elapsed = (time_current - self.time_prev).to_sec()
self.time_prev = time_current
dleft = math.pi * self.wheel_diameter * \
(encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
dright = math.pi * self.wheel_diameter * \
(encoder2 - self.encoder2_prev) / self.encoder_ticks_per_rev
self.encoder1_prev = encoder1
self.encoder2_prev = encoder2
d = (dleft + dright) / 2
dtheta = (dright - dleft) / self.base_width
if d != 0:
dx = math.cos(dtheta) * d
dy = -math.sin(dtheta) * d
self.x += dx*math.cos(self.theta)-dy*math.sin(self.theta)
self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
self.theta += dtheta
self.odom.header.stamp = time_current
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
q = tf.transformations.quaternion_from_euler(0,0,self.theta)
self.odom.pose.pose.orientation.x = q[0]
self.odom.pose.pose.orientation.y = q[1]
self.odom.pose.pose.orientation.z = q[2]
self.odom.pose.pose.orientation.w = q[3]
self.odom.twist.twist.linear.x = d / time_elapsed
self.odom.twist.twist.angular.z = dtheta / time_elapsed
self.odom_pub.publish(self.odom)
if __name__=='__main__':
rospy.init_node('trd_driver_node')
serialport = rospy.get_param('~serialport', default='/dev/ttyUSB0')
baudrate = rospy.get_param('~baudrate', default=38400)
trd_driver = TrdDriver(serialport, baudrate)
trd_driver.run()
然后，修改trd_driver.launch文件，加入对应参数数值。 注意，此处底盘的轮径为0.08米，轮距为0.21米，编码器为1980线，控制速度线速度系数和角速度系数分别为100.0和10.0。
<?xml version="1.0"?>
<launch>
<arg name="serialport" default="/dev/ttyUSB0" />
<arg name="baudrate" default="38400" />
<arg name="linear_coef" default="100.0" />
<arg name="angular_coef" default="10.0" />
<arg name="wheel_diameter" default="0.08" />
<arg name="base_width" default="0.21" />
<arg name="encoder_ticks_per_rev" default="1980" />
<node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
<param name="serialport" value="$(arg serialport)" />
<param name="baudrate" value="$(arg baudrate)" />
<param name="wheel_diameter" value="$(arg wheel_diameter)" />
<param name="base_width" value="$(arg base_width)" />
<param name="encoder_ticks_per_rev" value="$(arg encoder_ticks_per_rev)" />
<param name="linear_coef" value="$(arg linear_coef)" />
<param name="angular_coef" value="$(arg angular_coef)" />
</node>
</launch>
测试参数是否满足精度¶
启动驱动节点和键盘控制节点，遥控底盘运动，并查看/odom主题内容。
(远程终端)$ roslaunch trd_driver trd_driver.launch
(远程终端)$ rosrun trd_driver keyboard_teleop.py
(远程终端)$ rostopic echo /odom
观察/odom中的Position位置读数以及速度Twist读数，看看发现什么问题？如何调整参数进行校准？
如何测试，可以确保上述参数是准确的？
最终参数：
<arg name="linear_coef" default="100.0" />
<arg name="angular_coef" default="10.0" />
<arg name="wheel_diameter" default="0.08" />
<arg name="base_width" default="0.21" />
<arg name="encoder_ticks_per_rev" default="1980" />
何解决usb断线重连问题¶
由于底盘运行过程中会出现颠簸，难免导致USB接口出现断开的情况。因此需要在代码中加入重连机制。
提示 在TrdDriver类中定重连成员方法：
def reconnect(self):
print('{} reconnecting...'.format(time.time()))
while True:
try:
self.ser = serial.Serial(self.serialport, self.baudrate)
except:
time.sleep(1)
print('{} reconnecting...'.format(time.time()))
else:
print('{} reconnected!'.format(time.time()))
break
提示 发送指令出现异常时重连：
def send(self, cmd):
while True:
try:
self.ser.write(cmd)
break
except serial.serialutil.SerialException:
self.reconnect()
提示 接收指令出现异常时重连：
def read_buffer(self):
result = ''
try:
while self.ser.inWaiting() > 0:
result += self.ser.read(1)
except:
self.reconnect()
return result
采用汤尼机器人官方trbase包¶
至此，差速电机驱动包的开发完成。汤尼机器人官方包 trbase 中包含了所有开发所用到的包，不仅包括trd_driver电机驱动包， 还包含传感器驱动、所有底盘建图及导航包，可以下载此包进行部署编译。
将trd_driver包备份到其它位置，将trbase包克隆到本地~/catkin_ws/src下。
$ cd ~/catkin_ws/src
$ mv trd_driver ~/Documents/
$ git clone https://github.com/TonyRobotics/trbase
如果未联网，可暂时将树莓派下~/catkin_ws工作区中的trbase包复制过来：
$ cd ~/catkin_ws/src
$ mv trd_driver ~/Documents/
$ rsync -avz pi@192.168.5.108:/home/pi/catkin_ws/src/trbase ./
然后部署到树莓派的 tutorial_ws 工作区下：
$ cd ~/catkin_ws
$ rsync -avz --delete --exclude="*.swp" src pi@192.168.5.108:/home/pi/tutorial_ws/
编译运行即可。
2.6.激光传感器安装与显示¶
本节我们将列举几款常用的激光传感器，并对其中一款进行安装和显示。
激光传感器的介绍¶
激光传感器的作用是，扫描机器人周围的物体，并以点云的形式进行表示。 因此，激光传感器分为二维激光传感器和三维激光传感器，会分别提供二维点云和三维点云数据。
通常，三维激光传感器价格昂贵，移动底盘通常会采用二维激光传感器。常用的型号有：
- 日本北洋Hokuyo系列
ROS驱动：http://wiki.ros.org/urg_node
- 德国SICK系列
ROS驱动：http://wiki.ros.org/sick_tim
- 深圳镭神智能生产的激光雷达
ROS驱动：https://github.com/leishen-lidar/LS01D
- 上海思岚科技生产的RPLIDAR系列
ROS驱动：http://wiki.ros.org/rplidar
- 深圳EAI公司生产的YDLIDAR系列
示例一：安装YDLIDAR F4激光ROS驱动包¶
在此，采用激光传感器型号为深圳EAI公司推出的YDLIDAR F4型号。其ROS驱动包可在Github下载：
https://github.com/EAIBOT/ydlidar
将源代码下载到笔记本电脑catkin_ws工作区的src路径下，然后部署到树莓派上：
$ cd ~/catkin_ws/src
$ git clone https://github.com/EAIBOT/ydlidar
然后执行rsync指令同步代码。在此，可将rsync指令写到一个.sh脚本中，每次执行脚本文件就可以了。 在catkin_ws下新建deploy.sh文件，加入以下内容：
rsync -avz --delete --exclude="*.swp" src pi@192.168.5.108:/home/pi/tutorial_ws/
远程部署时在catkin_ws下执行即可。
$ cd ~/catkin_ws
$ sh deploy.sh
将代码部署到树莓派后，登陆上去进行构建、配置。
$ ssh pi@192.168.5.108
(远程终端)$ cd ~/tutorial_ws
(远程终端)$ catkin_make
构建完成后，即可运行测试。为了防止串口名称的冲突，可执行以下命令，将激光传感器映射为特定的名称。
(远程终端)$ roscd ydlidar/startup
(远程终端)$ sudo sh initenv.sh
上述指令会建立udev
rules文件，将激光串口名映射为/dev/ydlidar，方便后续调用。
注意：设置完udev rules后，需要重新连接激光传感器才会生效。
示例二：安装镭神ls01d激光ROS驱动包¶
同理，如果采用镭神生产的ls01d激光传感器，则需要下载安装其对应驱动。
首先到github下载驱动包，地址为https://github.com/leishen-lidar/LS01D
$ git clone https://github.com/leishen-lidar/LS01D
然后将其中对应的ROS驱动包重命名，即将ls01D Ros文件夹重命名为ls01d， 将重命名后的ls01d文件夹拷贝到工作区的~/catkin_ws/src文件夹下。
同样，将代码远程部署到树莓派后，登陆上去进行构建、配置。
$ ssh pi@192.168.5.108
(远程终端)$ cd ~/tutorial_ws
(远程终端)$ catkin_make
构建完成后，即可运行测试。为了防止串口名称的冲突，可执行以下命令，将激光传感器映射为特定的名称。
(远程终端)$ roscd ls01d/scripts
(远程终端)$ sudo sh create_udev_rules.sh
上述指令会建立udev
rules文件，将激光串口名映射为/dev/laser，方便后续调用。
注意：设置完udev rules后，需要重新连接激光传感器才会生效。
示例三：安装Hokuyo激光驱动包¶
由于Hokuyo激光传感器的ROS驱动包已发布到ROS的Ubuntu软件源中，所以可以直接二进制方式安装：
$ sudo apt install ros-kinetic-urg-node
同理，SICK的激光驱动包也可通过二进制方式安装：
$ sudo apt install ros-kinetic-sick-tim
启动激光驱动，查看发布的消息¶
将激光传感器连接到树莓派的USB端口上，用roslaunch启动驱动节点：
(远程终端)$ roslaunch ydlidar lidar.launch
或:
(远程终端)$ roslaunch ls01d ls01d.launch
在新打开终端，查看发布的主题及内容：
(远程终端)$ rostopic list
(远程终端)$ rostopic echo /scan
这样可看到打印出激光点云数据。请自行查看消息的类型，及其定义，各变量所表示的含义。
$ rostopic info /scan
ROS多机运行的配置¶
为了能够在笔记本电脑上也能接收到激光数据，需要配置两台机器的ROS环境变量参数。
以底盘树莓派作为ROS主节点。 在树莓派的~/.bashrc文件加入以下内容。
export ROS_MASTER_URI=http://192.168.5.108:11311
export ROS_HOSTNAME=192.168.5.108
然后，在笔记本电脑的~/.bashrc文件加入以下内容。
export ROS_MASTER_URI=http://192.168.5.108:11311
export ROS_HOSTNAME=192.168.5.100
这样，在树莓派上启动roscore后及相关节点后，即可在笔记本电脑上进行查看和通信，请自行尝试。
- 尝试在树莓派启动激光驱动，并在笔记本电脑打印其内容。
- 尝试在笔记本电脑发送某主题，并在树莓派上打印其内容。
使用RViz查看激光信号¶
用打印的方式查看激光信号很不直观，这时，用RViz可直观的查看激光信号。
在树莓派启动激光驱动后，在笔记本电脑运行RViz：
(远程终端)$ roslaunch ydlidar lidar.launch
或：
(远程终端)$ roslaunch ls01d ls01d.launch
$ rviz
在RViz界面中，选择Add按钮，在By Topic选项中找到 /scan 主题，选中并添加。
这时，RViz不会显示激光数据。需要将 Fixed Frame 改为 laser_frame 即可显示。
为了显示更清晰，将 LaserScan 下的 Style 改为 Points 。
或直接在启动就将 Fixed Frame 设置为 laser_frame ：
$ rviz -f laser_frame
注意： 不同传感器驱动所对应的 /scan 主题的frame_id是不一样的，在此以 laser_frame 为例。
查看消息的frame_id值可通过打印消息进行查看： $ rostopic echo /scan 。
自行修改其它选项，看RViz都提供了哪些功能？
2.7.机器人SLAM-建图与导航¶
发布里程计的TF变换¶
在trd_driver驱动的ROS主循环中，加入发布base_link–>odom之间的TF变换的代码。
# 发布里程的tf变换
self.tf_broadcaster.sendTransform(
(self.x,self.y,0),
tf.transformations.quaternion_from_euler(0, 0, self.theta),
rospy.Time.now(),
'base_link',
'odom')
利用激光进行建图¶
建图框架如下图所示：
现在，我们已经实现了电机驱动节点、激光驱动节点的编写和安装。接下来需要加入建图节点实现完整的建图功能。
- 电机驱动节点(trd_driver.py)
针对不同的驱动，需要启动不同的电机驱动。 在此，我们采用前几课开发的针对坦克底盘的驱动，trd_driver包下的trd_driver.py节点。
- 激光驱动节点(ls01d)
在此，激光传感器采用深圳镭神开发的ls01d型号，请参照之前课程驱动包的安装及启动方法。
- 建图节点(gmapping或hector_mapping)
建图节点读取电机驱动节点发布的/odom里程计消息，以及激光驱动节点发布的/scan激光数据消息，即可完成建图，输出/map地图消息。 ROS中常用的移动底盘建图算法有gmapping和hector_mapping，两者的ROS地址为：
http://wiki.ros.org/slam_gmapping
http://wiki.ros.org/hector_mapping
安装方式即通过apt install指令安装：
$ sudo apt install ros-kinetic-slam-gmapping
$ sudo apt install ros-kinetic-hector-mapping
可通过rosrun指令启动：
$ rosrun gmapping slam_gmapping
$ rosrun hector_mapping hector_mapping
当然，最好的方式是在launch文件中统一启动。
手动遥控方式建图¶
在使用底盘进行建图时，可通过键盘遥控的方式，控制底盘扫描周围环境，实现建图。 此时，只需启动电机驱动节点、激光驱动节点以及建图节点即可，将3个节点在一个launch文件中启动， launch文件内容如下：
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="false" />
<include file="$(find trd_driver)/launch/trd_control.launch">
<arg name="serialport" value="/dev/motor_trd" />
</include>
<include file="$(find ls01d)/launch/ls01d.launch" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" />
</launch>
在树莓派启动mapping.launch文件，然后在笔记本电脑启动RViz进行查看。
$ roslaunch trd_driver mapping.launch
$ rviz
地图的保存¶
在建图过程中，随时可以执行map_server包下的map_saver节点完成地图的保存。
$ rosrun map_server map_saver
默认会保存为map.yaml和map.pgm文件。
打开map.yaml文件，查看文件内容，了解其参数的意义。
运动规划节点¶
运动规划完成的功能为：接收到目标点后，结合当前地图信息，以及激光等传感器检测到的障碍物信息， 规划出如何到达目标点的路径（称为全局路径），同时避开沿途障碍物。并能根据全局路径， 计算出每一时刻底盘应该行进的角度及方向（称为局部路径）。
在ROS的navigation地面导航包中，其核心是运动规划节点，即move_base。
安装navigation超包(即一系列相关功能包组成的包)后，会安装move_base包及节点。
$ sudo apt install ros-kinetic-navigation
由于move_base节点启动时，需要加载很多算法参数，所以通常会在launch文件中启动， 并指定其yaml参数配置文件。
建图后，在已有地图上进行导航¶
与建图功能的launch文件相比，导航功能不需要建图节点了，但需要具备以下节点：
- 电机驱动节点（trd_driver.py）
- 激光驱动节点（ls01d）
- 地图文件发布节点（map_server）
- 机器人定位节点（amcl）
- 运动控制节点（move_base）
在trd_driver包的launch文件夹下，新建navigation.launch文件，并将各节点调用添加到launch文件中：
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="false" />
<include file="$(find trd_driver)/launch/trd_control.launch">
<arg name="serialport" value="/dev/motor_trd" />
</include>
<include file="$(find ls01d)/launch/ls01d.launch" />
<node pkg="map_server" type="map_server" name="map_server"
args="$(find trd_driver)/maps/map.yaml" />
<node pkg="amcl" type="amcl" name="amcl" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find trd_driver)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find trd_driver)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find trd_driver)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
launch文件以来4个配置文件，里面设定了算法配置等参数， 在trd_driver包下新建config文件夹，并在config文件夹下加入以下4个文件：
- costmap_common_params.yaml
bstacle_range: 2.5
raytrace_range: 3.0
#robot_radius: 0.32
footprint: [[0.3,0.25], [0.3,0], [0.3,-0.25],[-0.3,-0.25],[-0.3,0.25]]
max_obstacle_height: 0.6
min_obstacle_height: 0.0
obstacle_layer:
observation_sources: laser_scan_sensor
laser_scan_sensor:
data_type: LaserScan
topic: scan
marking: true
clearing: true
expected_update_rate: 0
- local_costmap_params.yaml
local_costmap:
inflation_radius: 0.25
global_frame: /odom
robot_base_frame: /base_link
update_frequency: 2.0
publish_frequency: 1.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.025
transform_tolerance: 1.0
map_type: costmap
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
# - {name: range_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}
# - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
- global_costmap_params.yaml
global_costmap:
inflation_radius: 0.25
global_frame: /map
robot_base_frame: /base_link
update_frequency: 1.0
publish_frequency: 1.0
static_map: true
rolling_window: false
resolution: 0.025
transform_tolerance: 1.0
map_type: costmap
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
- base_local_planner_params.yaml
planner_frequency: 0.3
controller_frequency: 2.0
recovery_behavior_enabled: true
clearing_rotation_allowed: true
controller_patience: 60
recovery_behaviors:
- name: 'conservative_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
- name: 'aggressive_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
conservative_reset:
reset_distance: 1.0
aggressive_reset:
reset_distance: 0.0
TrajectoryPlannerROS:
max_vel_x: 0.35
min_vel_x: 0.0
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 0.5
min_vel_theta: -0.5
min_in_place_vel_theta: 0.3
escape_vel: -0.1
acc_lim_x: 1.0
acc_lim_y: 0.0 # zero for a differential drive robot
acc_lim_theta: 3.2
holonomic_robot: false
yaw_goal_tolerance: 0.2 # about 6 degrees/0.1
xy_goal_tolerance: 0.3 #
latch_xy_goal_tolerance: false
pdist_scale: 0.8
gdist_scale: 0.4
meter_scoring: true
heading_lookahead: 1.0
heading_scoring: false
heading_scoring_timestep: 0.8
occdist_scale: 0
oscillation_reset_dist: 0.05
publish_cost_grid_pc: false
prune_plan: true
sim_time: 3.0
sim_granularity: 0.025
angular_sim_granularity: 0.025
vx_samples: 16
vy_samples: 0 # zero for a differential drive robot
vtheta_samples: 30
dwa: true
simple_attractor: false
在树莓派启动navigation.launch文件，然后在笔记本电脑启动RViz进行查看。
$ roslaunch trd_driver navigation.launch
$ rviz
点击RViz界面的Nav Goal按钮，并在地图上设置目标位置，底盘即可自动导航至指定位置。
move_base原理¶
- 全局规划算法
功能：已知起始点、目标点、地图，计算出起始点到目标点的路径。
输出：一条路径（/plan），消息类型nav_msgs/Path
- 局部规划算法
功能：已知全局规划路线，计算底盘的实时运动速度，保证能够按照路线行走。
输出：运动速度（/cmd_vel），消息类型geometry_msgs/Twist
另一种建图方式：自动探索式建图¶
只需要在mapping.launch文件中加入move_base节点即可。
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="false" />
<include file="$(find trd_driver)/launch/trd_control.launch">
<arg name="serialport" value="/dev/motor_trd" />
</include>
<include file="$(find ls01d)/launch/ls01d.launch" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find trd_driver)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find trd_driver)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find trd_driver)/config/base_local_planner_params.yaml" command="load" />
</node>
</launch>
点击RViz界面的Nav Goal按钮，并在地图上设置目标位置，底盘即可一边建图，一边自动导航至指定位置。
机械臂仿真部分¶
3.1 仿真环境 Gazebo 和 RViz¶
基于 ros 的仿真环境有多种，比如 Gazebo、RViz等。
Gazebo 和 RViz的区别：
rviz是三维可视化工具，强调把已有的数据可视化显示。
gazebo是三维物理仿真平台，强调的是创建一个虚拟的仿真环境。
rviz需要已有数据。rviz提供了很多插件，这些插件可以显示图像、模型、路径等信息，但是前提都是这些数据已经以话题、参数的形式发布，rviz做的事情就是订阅这些数据，并完成可视化的渲染，让开发者更容易理解数据的意义。
gazebo不是显示工具，强调的是仿真，它不需要数据，而是创造数据。我们可以在gazebo中创建一个机器人世界，不仅可以仿真机器人的运动功能，还可以仿真机器人的传感器数据。而这些数据就可以放到rviz中显示，所以使用gazebo的时候，经常也会和rviz配合使用。当我们手上没有机器人硬件或实验环境难以搭建时，仿真往往是非常有用的利器。
Gazebo 的仿真界面：
RViz 的仿真界面：
参考：
- http://www.guyuehome.com/2263
3.2 Moveit! 介绍¶
在实现机械臂的自主抓取中机械臂的运动规划是其中最重要的一部分，其中包含运动学正逆解算、碰撞检测、环境感知和动作规划等。Gauss机械臂的运动规划采用的是ROS系统提供的MoveIt! 划。
MoveIt! 是ROS系统中集合了与移动操作相关的组件包的运动规划库。它包含了运动规划中所需要的大部分功能，同时其提供友好的配置和调试界面便于完成机器人在ROS系统上的初始化及调试，其具体架构如下图所示：
（1）move_group: move_group是MoveIt!的核心部分，它的功能在于集成机器人的各独立模块并为用户提供一系列的动作指令和服务。其本身并不具备太多的功能，起到的是积分器的作用，完成各功能包和插件的集成。
（2）场景规划（Planning Scene）: 通过场景规划，用户可以创建一个具体的工作环境或者加入障碍物。
（3）运动规划（motion panning): 在MoveIt!中，运动规划器起的作用是实现运动规划算法，其以插件的方式通过ROS的pluginlib接口完成信息的通讯，根据需求可制作或选用不同的规划算法。
（4）运动学（Kinematics）： 运动学算法是机械臂控制算法中的核心内容，其中包括正运动学算法和逆运动学算法，在MoveIt!中，运动学算法同样以插件的形式完成于ROS的集成，因此可根据实际需求，编写自己的运动学算法来控制机器人。
（5）碰撞检测（collision checking）： 为避免机器人自身或者和环境发生干涉导致意外的发生，在进行运动规划时碰撞检测是必不可少的一部分，在MoveIt!中，采用FCL（Flexible Collision Library）进行对象碰撞的运算。
（6）开源运动规划库（open motion planning library）: OMPL是一个的开源的C++库，主要用于运动规划，其中包含了大量用于运动规划的先进算法。该库中的算法以采样规划算法为主，通过其可以实现不同目标点之间的路径规划。
moveit 助手：
参考：
- https://moveit.ros.org/documentation/planners/
- http://moveit.ros.org/documentation/
- http://moveit.ros.org/documentation/concepts/
- https://blog.csdn.net/kalenee/article/details/80818658
- http://ompl.kavrakilab.org/planners.html
- https://www.ncnynl.com/archives/201610/1030.html
- https://blog.csdn.net/wxflamy/article/details/79171337
- https://blog.csdn.net/wxflamy/article/details/79160781
3.3 moveit demo 节点¶
- 创建 tony_moveit_demo packege：
cd 到 ~/catkin_ws/src 目录，然后执行：
catkin_create_pkg tony_moveit_demo catkin interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs
执行完命令后，你会看到 tony_moveit_demo 目录下的文件结构如下：
├── CMakeLists.txt
├── include
│ └── tony_moveit_demo
├── package.xml
2、添加 c++ 代码：
创建 src 目录，然后在 src 目录下新建文件 tony_moveit_demo_node.cpp
修改 tony_moveit_demo_node.cpp 为如下内容：
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP = "gauss_arm";
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to add and remove collision objects in our "virtual world" scene
// moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// Raw pointers are frequently used to refer to the planning group for improved performance.
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it. This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
joint_group_positions[0] = -3.1415926/2; // radians
move_group.setJointValueTarget(joint_group_positions);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
move_group.move();
ros::shutdown();
return 0;
}
3、修改 CMakeLists.txt 文件为如下内容：
cmake_minimum_required(VERSION 2.8.3)
project(tony_moveit_demo)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
interactive_markers
moveit_core
moveit_ros_perception
moveit_ros_planning_interface
pluginlib
roscpp
std_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES tony_moveit_demo
CATKIN_DEPENDS interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/tony_moveit_demo.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/tony_moveit_demo_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
4、编译代码：
cd 到 ~/catkin_ws 目录下，然后执行：
catkin_make
5、运行代码：
rosrun tony_moveit_demo tony_moveit_demo_node
3.4 Rviz入门¶
使用 MoveIt! 最简单的方式是通过 RViz 插件。 Rviz 是 ROS 中调试时所用的主要的可视化和交互工具。 MoveIt! Rviz 插件使得你可以设置虚拟环境（屏幕），交互式地创建开始和目标状态，测试各种运动规划器和将输出可视化。
第 1 步：启动 Demo 并配置插件¶
启动 demo:
roslaunch gauss_moveit_config demo.launch
如果你第一次执行这条命令，你会在RViz中看到一个空的world。 你需要自行添加一个 Motion Planning 插件。
- 你会在 RViz 中看到一个空的世界:
- 在 RViz 的 Displays 标签上，点击 Add:
- 在 moveit_ros_visualization 目录上，选择 “MotionPlanning” 作为 the DisplayType。 然后点击 “OK”。
- You should now see the Panda robot in RViz:
- 你应该在 RViz 中可以看到Gauss 机械臂了：
- 加载 Motion Planning 完成后，我们需要配置它。在”Displays” 子窗口上的 “Global Options” 标签页，设置 Fixed Frame 为
groud_link。 - 现在， 你可以开始为你的 Gauss 机械臂配置插件了。点击 “Displays” 标签上的 “MotionPlanning”:
- 确认 Robot Description 设置为
robot_description。 - 确认 Planning Scene Topic 设置为
/planning_scene。 - 在 Planning Request，修改 Planning Group 为
gauss_arm。 - 在 Planned Path，把 Trajectory Topic 修改为
/move_group/display_planned_path。
- 确认 Robot Description 设置为
第 2 步：操作可视化的机器人¶
There are four different overlapping visualizations: 有四种不同的重叠的可视化窗口：
- 在
/planning scene的规划环境中的机械臂的配置（默认打开） - 机械臂的规划路径（默认打开）
- 绿色：运动规划的初始状态（默认关闭）
- 橙色：运动规划的目标状态（默认打开）
这些可视化窗口的显示状态可以使用复选按钮来切换打开和关闭：
- planning scene 使用 Scene Robot 标签页上的 Show Robot Visual 复选按钮。
- planned path 使用 Planned Path 标签页上的 Show Robot Visual 复选按钮。
- 初始状态使用 Planning Request 标签页上的 Query Start State 复选按钮。
- 目标状态使用 Planning Request 标签页上的 Query Goal State 复选按钮。
- 点击这些复选按钮来切换不同的可视化窗口的打开和关闭状态。
第 3 步：与 Gauss 机械臂交互¶
接下来，我们只需要scene robot、start state 和 goal state。
- 选中 Planned Path 标签页上的 Show Robot Visual 复选按钮。
- 取消选中 Scene Robot 标签页上的 Show Robot Visual 复选框。
- 选中 Planning Request 标签页上的 Query Goal State 复选框。
- 选中 Planning Request 标签页上的 Query Start State 复选框。
现在应该可以看到两个交互式的 maker（形状是圆球）。 一个maker对应的一个橙色的机械臂，你可以用来设置运动规划的 “Goal State”。另外一个maker对应的是绿色的机械臂，你可以用来设置运动规划的 “Start State” 。 如果你没有看到交互式的 maker，点击RViz顶部菜单 Interact**按钮。（注意，有些工具可能被隐藏了，可以点击顶部菜单栏的 **”+” 来增加 Interact）。
现在你可以使用这些 maker 来拖拽机械臂和改变它的姿态了。尝试一下吧！
第 4 步：使用 Gauss 机械臂做运动规划¶
- 现在，你可以在 MoveIt! RViz插件中 使用 Gauss 机械臂做运动规划了。
- 移动初始姿态到一个期望的位置。
- 移动目标姿态到另一个期望的位置。
- 确保两个姿态不会导致机械臂自碰撞。
- 确保规划的路径是可视化的。检查 Planned Path 标签页的 Show Trail 为选中状态。
- 在 Planning 标签页的 MotionPlanning 窗口上，点击 Plan 按钮。你可以看到机械臂开始运动，并显示了运动轨迹。
- 检查轨迹点：
你可以在RViz上面检查轨迹点：
- 在 “Panels”菜单上，选择 “MotionPlanning - Slider”。你会在RViz看到一个新的滑动条面板。
- 设置你的目标姿态，然后点击`Plan`。
- 移动滑块，点击 “Play” 按钮。
注意： 一旦你设置了末端到新的姿态，确保先按 Plan 然后再点击 Play。
- 保存配置
RViz 允许你保存当前的配置： File->Save Config。
3.5 关节空间规划¶
MoveGroup 类¶
在 MoveIt!，最简单的是通过使用 MoveGroup 类。它为用户的大多数操作提供了简单的函数接口，特别是设置关节目标和位姿目标、创建运动规划、移动机器人、增加物体到环境中、增加/减少机器人本身的物体。 这个接口允许你通过ROS 话题、服务和action与MoveGroup节点通信。
执行关节空间的运动规划¶
1、启动 RViz 仿真环境：
roslaunch gauss_moveit_config demo.launch
2、在新的终端执行：
rosrun tony_moveit_tutorials move_group_interface_joint
我们可以设置机械臂各个关节的角度，并执行运动规划。在RViz仿真环境中可以看到机械臂的 1 轴转过了90度。
代码解析¶
1、初始化 ros node：
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
2、设置move group：
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
3、 获取关节状态：
// Raw pointers are frequently used to refer to the planning group for improved performance.
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it. This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
4、使用 MoveIt! 做运动规划：
// Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
joint_group_positions[0] = -3.1415926/2; // radians
move_group.setJointValueTarget(joint_group_positions);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
5、 执行运动规划：
move_group.move();
3.6 笛卡尔空间规划¶
通过图形界面查看机械臂的当前的关节状态：
笛卡尔空间规划：
我们可以设置机械臂末端的位姿，并执行运动规划。 注意：机械臂的末端位姿不能随意设置，必须在机械臂的可达范围内。
代码如下：
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
// the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
// are used interchangably.
static const std::string PLANNING_GROUP = "gauss_arm";
// The :move_group_interface:`MoveGroup` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
// Start the demo
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
geometry_msgs::Pose target_pose1;
// target_pose1.orientation.w = 1.0;
// target_pose1.orientation.x = 0;
// target_pose1.orientation.y = 0;
// target_pose1.orientation.z = 0;
target_pose1.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,0);
target_pose1.position.x = (double)276/1000;
target_pose1.position.y = 0;
target_pose1.position.z = (double)435/1000;
move_group.setPoseTarget(target_pose1);
// Now, we call the planner to compute the plan and visualize it.
// Note that we are just planning, not asking move_group
// to actually move the robot.
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "plan 1 (pose goal) %s", success ? "" : "FAILED");
// Moving to a pose goal
// ^^^^^^^^^^^^^^^^^^^^^
//
// Moving to a pose goal is similar to the step above
// except we now use the move() function. Note that
// the pose goal we had set earlier is still active
// and so the robot will try to move to that goal. We will
// not use that function in this tutorial since it is
// a blocking function and requires a controller to be active
// and report success on execution of a trajectory.
/* Uncomment below line when working with a real robot */
move_group.move();
ros::shutdown();
return 0;
}