如果满足A条件：
那么执行a指令
如果满足B条件：
那么执行b指令
否则：
执行c指令
智能无人车系统实践基础
智能无人车系统实践基础
课程内容：学习机器人操作系统（ROS）以及ubuntu系统的基础，在课程和竞赛中完成从感知、决策到执行，整个机器人系统环节的理论和实践，建立对机器人系统的基本框架的理解。提高Python编程、ROS系统、智能车避障、目标检测和路径规划等方面的实践技能和科学思维能力。
实践竞赛：可自由操作的真实卡丁车竞速障碍赛道场景、智能车模型高仿真度的虚拟场景。即学即练，快速实践，线上竞赛。
开始学习
智能无人机技术设计实践
智能无人机技术设计实践
课程内容：学习基于机器人操作系统(ROS)的智能无人机操作、基于CNN的物体识别、有限状态机的无人机系统集成方案等。综合运用多学科知识，设计并完成一个完整可用的信息系统 。
实践竞赛：培养青少年对于机器人系统的兴趣，启蒙青少年对机器人系统的概念，培养青少年用数学、物理、编程的思想解决人工智能系统任务的逻辑思维方式，培养动手解决实际问题的习惯。
开始学习
智能无人车系统实践基础
.png)
目录
课程知识点：
1、认识并能用ubuntu、虚拟机、命令行完成课程所需的ros指令
2、ROS系统的概念和几个基本元素：topic、node、launch文件
3、ROS系统基本操作方法：发送和接收前进转向指令、摄像头数据调用和传输
4、数字仿真环境gazebo的概述和调用
5、RVIZ、rqt调试工具的介绍和简单使用方法
6、计算机视觉的基础概念：图像在计算机中的存储方法、如何提取简单的图像信息
7、基础的数据处理：获取图像颜色信息、获取雷达距离信息、利用信息进行简单的路径规划和权重决策任务
8、调参体验：体验调整系统参数，优化任务完成度的过程
课程概述
﹀课程概述
快速开始
1、请在在线机器人仿真系统中，点击终端
.png)
2、输入指令
roslaunch nics_bringup nics_gazebo_simple.launch
3、按下回车键，进入3D机器人仿真环境中
4、右键终端，开启一个新终端
5、输入指令
roslaunch nics_line_follower nics_line_follower.launch
6、回车键，根据屏幕提示按下键盘任意键，3D仿真环境中的智能车引擎启动，智能车开始沿着跑道自动驾驶跑圈了！
这整个智能车自动驾驶的过程是如何完成的呢，这门课程将带你逐渐了解这个充满乐趣和知识领域。
第一章 机器人系统概述
﹀第一章 机器人系统概述
1.1 机器人系统运作方式
欢迎大家来到课程主线内容，相信现在你一定对机器人系统充满了好奇。
机器人是如何感知外界环境并且能够自主行动起来的呢？
这一小节，我们将带你走进机器人领域，对智能车这样简单的机器人系统运作方式产生初步的认知。
1.2 认识三个好朋友：Ubuntu、虚拟机、命令行
﹀1.2 认识三个好朋友：Ubuntu、虚拟机、命令行
1.2.1 Ubuntu
Ubuntu是一个以桌面应用为主的Linux操作系统，从前人们认为Linux难以安装、难以使用，在Ubuntu出现 后这些都成为了历史。Ubuntu也拥有庞大的社区力量，用户可以方便地从社区获得帮助。作为全球最流行且 最有影响力的Linux开源系统之一，Ubuntu自发布以来在应用体验方面：有较大幅度的提升，即使对比Windows、MacOS等操作系统，最新版本的Ubuntu也不逊色。
.png)
1.2.2 虚拟机
虚拟机（Virtual Machine）指通过软件模拟的具有完整硬件系统功能的、运行在一个完全隔离环境中的完整 计算机系统。在实体计算机中能够完成的工作在虚拟机中都能够实现。在计算机中创建虚拟机时，需要将实 体机的部分硬盘和内存容量作为虚拟机的硬盘和内存容量。
.png)
1.2.3 命令
一般说的“命令行”是指linux命令，linux命令是对Linux系统进行管理的命令。对于Linux系统来说，无论是中 央处理器、内存、磁盘驱动器、键盘、鼠标，还是用户等都是文件，Linux系统管理的命令是它正常运行的核 心，与之前的DOS命令类似。
命令行是用户通过键盘输入字符指令、计算机输出字符结果的一种人机交互的方式。与图形界面（GUI）主 要使用鼠标、图像等直观视觉交互方式不同，命令行通常需要用户记忆操作的命令。在熟记命令的前提下， 使用命令行往往要较使用图形用户界面的操作速度要快，并且更容易执行批量操作。
Tips 基于ubuntu的机器人在线仿真系统使用指南
﹀Tips 基于ubuntu的机器人在线仿真系统使用指南
一、点击网页导航栏的“在线仿真”模块。
二、如未注册或登陆，请先注册网站用户并登陆。
三、登陆后，可以使用公共仿真环境。请同学们点击去填写，补充完整资料，以获得独立的仿真空间IP和密码。
四、补充姓名、学校、专业和班级信息后点击提交（请使用真实信息以便助教审核同学们的信息）。
五、信息补充完后，页面最下会出现在线仿真系统的登陆密码，记录密码。
六、点击页面导航栏“在线仿真”，将跳转到个人的机器人系统仿真空间。
七、请在登陆界面点击连接后，输入后台分配的登陆密码。
如果出现页面停留在“连接中....”的情况，请刷新网页，如果问题仍存在，请联系助教。
八、登陆后，即可进入已经配置完成机器人操作系统环境的ubunt18.04系统。
tips：网页左边栏的工具栏可以点击拓展，包括键盘指令、剪贴板、放缩等设置，其中缩放模式请勿选择“远程调整大小”，避免网页仿真分辨率出现异常，建议选择“本地缩放”。请勿修改高级设置中的内容。
九、在线仿真系统的操作和ubuntu系统相同，需要注意复制粘贴指令不能直接输入。在线仿真系统和本地端间的文件不能直接传递，推荐同学们使用清华云盘（或微信等文件传输工具）上传下载文件。
十、请勿更改系统中的用户名和密码，避免忘记密码丢失数据。
十一、请勿随意删改系统中非自主（带锁）创建的文件，避免数据丢失。
十二、在线仿真系统每人独享环境，数据可以保存在在线云端服务器中，但强烈建议在每次调试后做好代码的备份和本地下载，避免由于云端受到攻击导致数据丢失的损失。
十三、在线仿真系统中对ROS的基本操作请参考课程内容学习，如发生误删代码、无法登录、无响应等问题，请联系助教。
请勿使用在线仿真系统服务器进行非法上网操作。
实验：启动ROS!
打开终端有两种方法：
1、按快捷键"ctrl+alt+T"打开终端（terminal）
2、在桌面点击右键，点击terminal
按"ctrl+alt+T"打开终端（terminal），在当前路径下输入
$ roscore
若能看到下面这个界面，就成功启动ros啦。
按"ctrl + c" 可以退出当前的ROS进程。
1.3 认识node和topics
﹀1.3 认识node和topics
1.3.1 Node简介
在ROS的世界里，最小的进程单元就是节点（node）。一个软件包里可以有多个可执行文件，可执行文件在 运行之后就成了一个进程(process)，这个进程在ROS中就叫做节点。
从程序的角度来说，node就是一个可执行文件。从功能角度来说，通常一个node负责者机器人的某一个单 独的功能。由于机器人的功能模块非常复杂，我们往往不会把所有功能都集中到一个node上，而会采用分布 式的方式，把鸡蛋放到不同的篮子里。例如有一个node来控制底盘轮子的运动，有一个node驱动摄像头获 取图像，有一个node驱动激光雷达，有一个node根据传感器信息进行路径规划……这样做可以降低程序发 生崩溃的可能性，试想一下如果把所有功能都写到一个程序中，模块间的通信、异常处理将会很麻烦。
1.3.2 启动Node
当我们要启动ROS时，首先输入命令:
代码
1$ roscore
此时ROS master启动。
ROS中它被赋予了专用的名字——node。我们知道一个package中存放着可执行文件，可执行文件是静态的， 当系统执行这些可执行文件，将这些文件加载到内存中，它就成为了动态的node。具体启动node的语句是：
rosrun + 包名+节点名
翻译成代码就是
代码
1$ rosrun pkg_name node_name
通常我们运行ROS，就是按照这样的顺序启动，有时候节点太多，我们会选择用launch文件来启动，下一小 节会有介绍。 Master、Node之间以及Node之间的关系如下图所示：
.png)
1.3.3 ROS的四种通信方式
在我们上节课讲到，ROS为机器人系统提供了通信架构，所以通信方式是ROS最为核心的概念。
1、Topic话题
2、service服务
3、Parameter Service参数服务器
4、Actionlib动作服务器
1.3.4 Topic简介
我们这门课，只需要用到topic（话题）这种通信方式。
ROS中的通信方式中，topic是常用的一种。对于实时性、周期性的消息，使用topic来传输是最佳的选择。 topic是一种点对点的单向通信方式，这里的“点”指的是node，也就是说node之间可以通过topic方式来传递 信息。topic要经历下面几步的初始化过程：首先，publisher节点和subscriber节点都要到节点管理器进行注 册，然后publisher会发布topic，subscriber在master的指挥下会订阅该topic，从而建立起sub-pub之间的 通信。注意整个过程是单向的。其结构示意图如下
.png)
我们以摄像头画面的发布、处理、显示为例讲讲topic通信的流程。在机器人上的摄像头拍摄程序是一个 node（圆圈表示,我们记作node1），当node1运行启动之后，它作为一个Publisher就开始发布topic。比如 它发布了一个topic（方框表示），叫做/camera_rgb，是rgb颜色信息，即采集到的彩色图像。
.png)
同时，node2假如是图像处理程序,它订阅了/camera_rgb这个topic，经过节点管理器的介绍，它就能建立和 摄像头节点（node1）的连接。 是不是很简单~~！
这里我要给大家讲一个知识点，topic的通信方式是异步的！
那么怎么样来理解“异步”这个概念呢？
在node1每发布一次消息之后，就会继续执行下一个动作，至于消息是什么状态、被怎样处理，它不需要了 解；而对于node2图像处理程序，它只管接收和处理/camera_rgb上的消息，至于是谁发来的，它不会关心 。所以node1、node2两者都是各司其责，不存在协同工作，我们称这样的通信方式是异步的。
ROS是一种分布式的架构，一个topic可以被多个节点同时发布，也可以同时被多个节点接收。比如在这个场 景中用户可以再加入一个图像显示的节点node3，我们在想看看摄像头节点的画面，则可以用自己的笔记本 连接到机器人上的节点管理器，然后在自己的电脑上启动图像显示节点。
这就体现了分布式系统通信的好处：扩展性好、软件复用率高。
总结两点：
1. topic通信方式是异步的，发送时调用publish()方法，发送完成立即返回，不用等待反馈。
2. topic可以同时有多个subscribers，也可以同时有多个publishers。
1.3.5 操作命令
在实际应用中，我们应该熟悉topic的几种使用命令，下表详细的列出了各自的命令及其作用。
| A | B | |
| 1 | 命令 | 作用 |
| 2 | rostopic list | 列出当前所有的topic |
| 3 | rostopic info topic_name | 显示某个topic的属性信息 |
| 4 | rostopic echo topic_name | 显示某个topic的内容 |
| 5 | rostopic pub topic_name ... | 向某个topic发布内容 |
| 6 | rostopic bw topic_name | 查看某个topic的带宽 |
| 7 | rostopic hz topic_name | 查看某个topic的频率 |
| 8 | rostopic find topic_type | 查找某个类型的topic |
| 9 | rostopic type topic_name | 查看某个topic的类型(msg) |
1.4 认识launch文件
﹀1.4 认识launch文件
1.4.1 简介
机器人是一个系统工程，通常一个机器人运行操作时要开启很多个node，对于一个复杂的机器人的启动操作 应该怎么做呢？当然，我们并不需要每个节点依次进行rosrun，ROS为我们提供了一个命令能一次性启动 master和多个node。该命令是：
$ roslaunch pkg_name file_name.launch
如果master没有启动，那么roslaunch就会首先启动master，然后再按照launch的规则执行。launch文件里 已经配置好了启动的规则。所以 roslaunch 就像是一个启动工具，能够一次性把多个节点按照我们预先的配 置启动起来，减少我们在终端中一条条输入指令的麻烦。
1.4.2 写法与格式
launch文件是一种标签文本，它的格式包括以下标签：
XML
参考链接:http://wiki.ros.org/roslaunch/XML
1.4.3 示例
launch文件的写法和格式看起来内容比较复杂，我们先来介绍一个最简单的例子如下：
XML
这是ros官网给出的一个最小的例子，文本中的信息是，它启动了一个单独的节点"talker",该节点是包" rospy_tutorials "软件包中的节点。
然而实际中的launch文件要复杂很多，同学们只需了解我们提供的launch文件即可
1.4.4 小结
对于初学者，我们不要求掌握每一个标签是什么作用，但至少应该有一个印象。如果我们要进行自己写 launch文件，可以先从改launch文件的模板入手，基本可以满足普通项目的要求。
1.4.5 小实验：搭建驾驶环境
PS:若想在终端里粘贴代码，需要"ctrl+shift +v"而不是"ctrl+v"
第一步：进入仿真环境
打开终端，输入
roslaunch nics_bringup nics_gazebo_simple.launch
看到这个界面，我们就成功启动launch文件，进入到智能车仿真驾驶界面啦。
第二步：退出
在终端里按"ctrl+c"退出仿真界面
1.5 单元测试一
﹀1.5 单元测试一
1.机器人操作系统的全称是?
提交
2.下列哪个不是ROS的特点?
提交
3.ROS最早诞生于哪所学校的实验室?
提交
4.（多选）下列哪几个命令可以启动ROS Master？
提交
5.关于ROS Node的描述，哪一项是错误的?
提交
6.关于.launch文件的描述，以下哪一项是错的?
提交
7.想要查看`/odom`话题发布的内容，应该用哪个命令?
提交
8.(多选)关于Topic通信的描述，正确的选项有：
提交
第二章 让智能车跑起来
﹀第二章 让智能车跑起来
2.1 智能车数字仿真—Gazebo
2.1.1 简介
ROS中的工具就是帮助我们完成一系列的操作，使得我们的工作更加轻松高效。ROS工具的功能大概有以下 几个方向：仿真、调试、可视化。本节课我们要学习的Gazebo就是实现了仿真的功能，而调试与可视化由 Rviz、rqt来实现。
2.1.2 认识Gazebo
对于Gazebo,大家可能比较陌生，它是机器人仿真工具。在topic通信中，我们的demo都是在Gazebo中实现。 Gazebo是一个机器人仿真工具，模拟器，也是一个独立的开源机器人仿真平台。当今市面上还有其他的仿 真工具例如V—Rep、Webots等等。但是Gazebo不仅开源，也是是兼容ROS最好的仿真工具。
Gazebo的功能很强大，最大的优点是对ROS的支持很好，可以进行机器人的运动学、动力学仿真，能够模 拟机器人常用的传感器（如激光雷达、摄像头、IMU等），也可以加载自定义的环境和场景。
2.1.3 仿真的意义
仿真不仅仅只是做出一个很酷的3D场景，更重要的是给机器人一个逼近现实的虚拟物理环境，比如光照条 件、物理距离等等。设定好具体的参数，让机器人完成我们设定的目标任务。比如一些有危险因素的测试， 就可以让机器人在仿真的环境中去完成，例如无人车在交通环境复杂的交通要道的效果，我们就可以在仿真 的环境下测试各种情况无人车的反应与效果，如车辆的性能、驾驶的策略、车流人流的行为模式等，又或者 各种不可控因素如雨雪天气，突发事故，车辆故障等，从而收集结果参数指标信息等等，只有更大程度的逼 近现实，才能得出车辆的真实效果。直到无人车在仿真条件下做到万无一失，才能放心的投放到真实环境中 去使用，这即避免了危险因素对实验者的威胁，也节约了时间和资源，这就是仿真的意义。
通常一些不依赖于具体硬件的算法和场景都可以在Gazebo上仿真，例如图像识别、传感器数据融合处理、 路径规划、SLAM等任务完全可以在Gazebo上仿真实现，大大减轻了对硬件的依赖。
2.1 实验:在Gazebo中手动驾驶智能车
﹀2.1 实验:在Gazebo中手动驾驶智能车
2.1.1 gazebo操作说明
•平移：鼠标左键
•旋转：鼠标滚轮中键
•放缩：鼠标滚轮
•界面左侧是控制面板
•导入模型就在控制面板的insert,可以直接拖入模拟空间，也可以按需自制模型拖入。
2.1.2 实验：搭建驾驶环境——用键盘驾驶智能车
那么如何在Ubuntu系统中，打开驾驶仿真环境，并且用键盘驾驶智能车呢？
我们需要用到2个launch文件。
1、nics_gazebo_simple.launch，它是我们写好的的gezebo仿真器；
2、keyboard_teleop.launch ，它是用键盘控制智能车的launch文件。
接下来我们开始实验
PS:若想在终端里粘贴代码，需要"ctrl shift +v"而不是"ctrl+v"!!!
第一步 ：打开仿真界面
和我们上一章节的实例测试一样，我们打开终端，输入
roslaunch nics_bringup nics_gazebo_simple.launch
可以看到gezebo仿真界面
第二步 ：启动键盘控制节点
按快捷键"ctrl+alt+T"另外打开一个新的终端，输入
roslaunch akm_control keyboard_teleop.launch
启动键盘控制的launch文件
在终端里我们可以看到键盘控制的键位
i是前进 u是向左前进 o是向右前进 k是刹车 ,是倒车
现在我们就用键盘来控制小车前后左右行走吧！
注意！！！我们需要在键盘控制的终端界面，来按"i"、"u"、"o"等指令。在其他界面按前后左右的指令,小车动不了！
你会发现小车动得特别慢！
怎么让小车走的快一点呢？这个时候你可以看一眼下面的红框
按一次"q"可以让小车提速10%！
第三步 ：在终端中查看车辆的线速度和角速度
再新打开一个终端，输入
roslaunch akm_control keyboard_teleop.launch
我们可以看到返回的线速度和角速度
2.1.3 小结
虽然Gazebo目前的功能还称不上强大，同时还存在着一些BUG，但是对于我们的入门学习也已经是足够了， 随着版本的更新，Gazebo也在越来越强大。
2.2 实验：ROS的调试工具—RViz
﹀2.2 实验：ROS的调试工具—RViz
2.2.1 简介
本节课介绍的是我们在ROS开发中非常常用的一个工具，基本上调试和开发都离不开这个工具——RViz(the Robot Visualization tool)机器人可视化工具，可视化的作用是直观的，它极大的方便了监控和调试等操作。 例如能在窗口中看到机器人模型和轨迹、激光雷达、点云地图。
2.2.2 实验：在RViz中添加智能车的第一视角
第一步 ：进入仿真环境
依然打开教材的模拟场景，输入
roslaunch nics_bringup nics_gazebo_simple.launch
第二步 ：打开RViz
之后打开新的终端，直接输入 " $ rviz "
打开rviz工具。
第三步 ：添加智能车的第一视角
在这里我们想看智能车摄像头的第一视角，需要点击“Add”
在By topic里添加image，点"ok"
RViz的插件种类繁多功能强大，非常适合我们开发调试ROS程序。
2.2.3 差异
虽然从界面上来看，RViz和Gazebo非常相似，但实际上两者有着很大的不同，Gazebo实现的是仿真，提供一 个虚拟的世界，RViz实现的是可视化，呈现接收到的信息。左侧的插件相当于是一个个的subscriber,RViz接收 信息，并且显示。所以RViz和Gazebo有本质的差异。
2.2.4 小结
RViz和Gazebo是我们常用的ROS工具，更好的利用这些工具是我们ROS进阶的基础。具体的操作和使用可以 参考我们的官方演示视频，跟着视频去实战演练，熟悉这两个工具。
2.3 实验：ROS的调试工具—RViz
﹀2.3 实验：ROS的调试工具—Rqt
2.3.1 简介
rqt是一个基于qt开发的可视化工具，拥有扩展性好、灵活易用、跨平台等特点，主要作用和RViz一致都是可 视化，但是和RViz相比，rqt要高级一个层次。
2.3.2 命令
rqt_graph :显示通信架构
rqt_graph是来显示通信架构，也就是我们上一章所讲的内容节点、主题等等，当前有哪些Node和topic在运 行，消息的流向是怎样，都能通过这个语句显示出来。此命令由于能显示系统的全貌，所以非常的常用。
2.3.3 实验：查看智能车的通信架构
1. 首先打开我们教材的模拟场景，输入
roslaunch nics_bringup nics_gazebo_simple.launch
2.输入命令语句" rqt_graph ",显示出了当前环境下运行的Node和topic，十分直观的看到通信结构以及消息流 向。注意在椭圆形的代表节点，矩形代表topic。（可以用鼠标滚轮放大缩小画面）
2.3.4 小结
rqt_graph这个功能是强大的，它使得我们初学者可以直观的看到ROS的通信架构和信息流，方便我们理解的同时，也使得我们能够最快的纠错等等。
2.4 单元测试二
﹀2.4 单元测试二
1.Gazebo是一款什么工具?
提交
2.rqt_graph可以用来查看计算图，以下说法错误的是：
提交
3.(多选)RViz可以图形化显示哪些类型的数据？
提交
4.(多选)关于仿真环境的描述，以下说法正确的是：
提交
第三章 python编程起步—用程序和智能车对话
﹀第三章 python编程起步—用程序和智能车对话
3.1 认识Python
3.2 实验：智能车喊话：Hello world !
﹀3.2 实验：智能车喊话：Hello world !
3.2.1 实验：智能车喊话：hello world ！
print语句
如果要让Python打印出指定的文字，可以用print()函数，然后把希望打印的文字用单引号或者双引号括起来，但不能混用单引号和双引号：
实战开始：
1、首先我们打开桌面的Visual Studio Code，它是我们今后要用到的代码编辑器！
新建一个文件
输入 print("hello world")
保存到桌面
重命名为 test.py
接着打开终端，输入
代码
1 cd Desktop
进入到桌面路径（因为代码刚刚保存在桌面；按“tab”键可以补全Desktop的拼写哦）
输入
代码
1python test.py
回车，我们可以看到“hello world”字符，在终端打印出来啦！
3.3 实验：如果红灯，请停车！
﹀3.3 实验：如果红灯，请停车！
每天的生活中，我们都在不断地做出一些判断。例如，每天有出门的时候，我们会决定今天穿什么衣服吃什 么样的早餐的。同时，每个判断背后都有一个理由，比如考虑到天气太冷了，就穿厚一点的衣服，所以理由 是用来做判断的条件。
如果要让小车能够在面对不同的路况时，做出一些优化的操作，就需要学习条件语句来做一些判断判断。判 断的条件来自小车对周围路况的“感知”。
3.3.1 if条件语句
当传感器获得的数据符合某种条件时，就会让小车执行相应策略的指令。这样的过程在计算机中可以理解为：
这就是一条简单的条件结构转化为计算机语句就是：
if 条件A：
指令a
elif 条件B：
指令b
else：
指令c
3.3.1 if条件语句
我们实现以下任务：
当小车检测到红灯时打印"stop"，检测到绿灯打印"go"，若既不是红灯也不是绿灯，则输出"no signal"
这样的要求可以通过下面的语句来实现：
Python
在写代码之前，建议不要用“复制”-“粘贴”把代码从页面粘贴到你自己的电脑上。写程序也讲究一个感觉， 你需要一个字母一个字母地把代码自己敲进去，在敲代码的过程中，初学者经常会敲错代码：拼写不对，大 小写不对，混用中英文标点，混用空格和Tab键，所以，你需要仔细地检查、对照，才能以最快的速度掌握 如何写程序。
建议同学们亲自敲打一遍
我们在vscode里新建文件，输入上面那段代码
保存在桌面，命名为test2.py
和之前test1.py一样，在终端运行
输入red
会输出stop
输入green
会输出go
输入其他的字符串
则会输出no signal
3.4 单元测试三
﹀3.4 单元测试三
1.（多选）以下关于Python的说法正确的是?
提交
第四章 初级视觉智能—完成基础寻迹关卡
﹀第四章 初级视觉智能—完成基础寻迹关卡
欢迎同学们来到赛道实践。
经过前面章节的铺垫学习，这一章节我们将带大家动手控制智能车完成寻迹任务。
我们首先来思考一下如何完成这样一个任务。
在这个任务中，任务的输入是环境信息，输出是智能车的ROS控制信号，我们需要做的就是处理环境信息，并决策，将ROS控制信号发送出去。
那么如何拿到完成任务需要参考的环境信息呢，采用视觉的方案是一种经典的方案。
欢迎同学们来到赛道实践。
4.1 计算机视觉
计算机视觉是研究如何使机器学会“看”的科学，也是人工智能和大量机器人系统的基础。这门科学体量庞 大、原理多样、应用领域很广泛。
但是请不要担心，这一章节我们会带你从零开始了解计算机视觉并动手操作计算机视觉的处理过程，经过这 一章节的学习，我们就能够让智能车自动完成跑圈任务。
本章共包含五个小节的内容，在第五小节我们会教大家手把手在Gazebo仿真器中调试车子。
由摄像头拍摄的周围环境图像，经过ROS系统通信传输之后，到达智能无人车的大脑（计算机）进行处理， 这是智能车获取周围环境信息和自身相对位置的重要方式。下面的视频将为你简单介绍计算机中处理图像的 基本知识，帮助你基本了解计算机是如何从图像中提取、处理信息的。
对计算机视觉及进行图像处理技术感兴趣的同学，欢迎关注我们后续推出的视觉方面的课程。
4.2 学会提取图像颜色信息
﹀4.2 学会提取图像颜色信息
上一节中我们了解了计算机中对图像信息的存储，大多采用矩阵存储0~255这些数字的形式。那么如何利用 这些矩阵中数字呢？
我们通过完成一项简单的智能车任务——寻迹，即无人车自动检测位置，并围绕赛道行驶一圈的任务，理解 提取图像信息的过程。
首先，来看一下咱们无人车跑圈的赛道场地。
第一步分析一下场地的特点：场地由绿色、黑色、白色、橘黄色组成，在这之中，我们希望无人车能够一直 沿着场地中橘黄色的线行驶。那么我们可以将任务可以分解为几个容易完成的任务链了：
1.找到图像中橘黄色的部分
2.确定橘黄色部分和目前无人车之间的相对位置
3.发送控制信号让无人车的中心靠近橘黄色区域并向前移动
视频中我们将教大家如何逐步完成这三个小任务链。
ok，我们已经完成了任务链的前两项任务了，距离完成智能车寻迹只剩下最后一个小任务了。
4.3 发出前进指令
﹀4.3 发出前进指令
上一小节中，我们将整体任务拆分成了小任务链，并完成了前两项小任务。
1.找到图像中橘黄色的部分
2.确定橘黄色部分和目前无人车之间的相对位置
3.发送控制信号让无人车的中心靠近橘黄色区域并向前移动
现在我们已经通过颜色信息提取出智能车偏离车道线的偏移量bias了，如何根据偏移量控制智能车运动呢？
在前面的课程中我们学到了控制智能车车前行：通过ROS topic发送线速度和角速度指令。因此，这里我们 要设计一个关于偏移量bias（y1-y2）和topic指令中线速度、角速度的函数。最后一步把topic发送出去即可 控制智能车按照我们的设计自动前进。
下面的视频将为你介绍如何设计关于偏移量bias和智能车线速度、角速度的函数。
三个小任务我们都完成了，距离动手实践只有一步之遥了。我们还需要设计一个应急预案，保证智能车在赛 道上的稳定性，应对丢失车道线等突发情况。
4.4 安全驾驶
﹀4.4 安全驾驶
经过前面三个小节的学习，相信同学们已经对完成任务胸有成竹，跃跃欲试想要操纵智能车开始竞速赛了。 别着急，还有最后一个问题需要注意，智能车驾驶的稳定性。
在自动驾驶的过程中，我们需要对可能遇到的异常驾驶情况做出提前的预案。这一小节内容非常简短，我们 将从实战的角度为大家讲解两个需要注意的事项。
4.5 开车调试——基础关卡
﹀4.5 开车调试——基础关卡
前面的四个小节中，我们已经掌握如何让智能车自动提取橙色的赛道线信息，并设计智能车前进的指令。 这一小节，我们将在实际场景的孪生仿真环境中调整和测试智能车，让它能够完成自动寻迹的基础任务。 需要提示大家非常重要的一点：
在调试的过程中，请注意做实验记录，记录不同参数带来的实验结果；同时，请将可以正常运行的程序文件 另存到另一个安全的目录下，制造一个可供恢复的原始time machine，以便在误操作时能够还原到之前正常 状态的代码。
ok，让我们开始吧！
视频版教程：
重要提示：如果代码报错，请同学们一定要仔细检查python语句中的缩进和逻辑关系。
文字版教程：
欢迎同学们来到赛道，在基础任务中我们将调用到两个launch文件。
第一个launch文件是打开仿真器环境的文件，请同学们打开一个终端，输入代码：
Python
1roslaunch nics_bringup nics_gazebo_simple.launch
并回车，静待几秒之后，我们将进入仿真环境中。
.jpg)
要让智能车动起来，我们还需要发出动作命令，首先是车道线检测节点，其次前进指令节点，这两个节点可 以通过另一个launch文件启动。
请再打开一个新的终端，右键点击终端，选择新终端，输入代码
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
.jpg)
并回车，静待几秒后，会发现命令行中出现了红字报错，这是因为目前的程序中缺少了几段代码。
.jpg)
不过不要着急，经过这一小节的学习，你将能够完成这些代码，并让智能车动起来。
Start
首先，我们在刚才报错的终端中键入ctrl+c终止这段出错的文件运行并关闭它。打开这个launch文件查看一 下它所运行的节点文件。这个launch文件位于主目录下的catkin_ws/src/nics_gzb/nicsrobot_line_follower 下的launch文件夹中。打开launch文件可以看到，启动的节点分别是detection_node和motion_node，分别 对应上层目录中scripts文件夹下的.py文件。
那么让我们从起点开始，以寻迹的过程为线索，寻找节点文件中缺少了哪部分代码。
首先智能车调用摄像头的图像，
.jpg)
接收到图像后，遍历图像找到图像中橙色像素点的坐标,并存入空间中。遍历图像后，计算空间中所有橙色像 素点坐标的均值。这部分操作可以在detection_node.py文件中查阅到（这里给一个打开文件的过程录屏）
(该文件位于Home/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_1).
这段操作中不需要更改代码的内容,但希望同学们可以学习代码内容尝试理解编写的逻辑和其中的函数操作。 我们可以直接利用求得的坐标均值center进行下一步操作.
.jpg)
.jpg)
接下来，橙色点坐标均值与智能车的位置横坐标相减，得到位置偏移量bias。请将这一操作的代码输入motion_node.py 第46行，motion_node.py文件位于
主目录下的catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_1文件夹下
输入
Python
注意保持输入的本行语句和上下文之间的缩进相同
.jpg)
接下来，将bias与偏移量阈值进行比较，当bias大于阈值时，向智能车发出转向角速度指令同时让线速度减 小；当bias小于阈值时，智能车不发出转向角速度指令，只发出正常线速度指令。那么我们需要先设置一个 偏移阈值。
在motion_node.py文件的第24行输入self.center_band=50，这里我们将偏移阈值设置为50个单位的大小。
Python
1 self.center_band=50 #车道线偏移在center_band内不用修正
根据bias计算出无人车运动的线速度和角速度,向无人车发出指令。智能车线速度和角速度的计算，我们在第三节中为大家介绍了一种设计方案，你也可以发挥自己的智慧，改变函数的形式和参数大小。
在motion_node.py文件的第61 62行中，输入计算线速度和角速度的代码.
希望初学的同学们可以手动输入代码，加深理解。对编程熟练的同学也可以从教案中复制粘贴代码。
Python
1velocity.linear.x =0.10+ 0.20*(1.0- abs(bias)/(self.image_width/2))
2velocity.angular.z = 0.002*bias
使用键盘Tab键和空格键调整缩进，注意保持本行语句和上下文间的缩进关系，保持这样的代码缩进。
.jpg)
这个映射关系函数中包含了很多参数，我们来看下参数的意义。
- bias
是横坐标的差值
- velocity.linear.x
是智能车向前方x行驶的线速度
- velocity.angular.z
表示智能车转弯的角速度（逆时针为正）
- self.image_width
是摄像头采集图像的宽度
.jpg)
接下来，将线速度和角速度指令topic发送出去，底盘接收到线速度角速度方向指令后，执行动作，这样我们 就完成了一个自动计算行驶的过程。
在motion_node.py文件的第71行中输入发送线速度和角速度的指令。
Python
1self.velocityPub_.publish(velocity)
随后，智能车的程序中循环上述过程，就能够完成整个赛道的行驶，到达终点。
这样我们就完成了对代码的完善工作
键盘按下ctrl+s，或直接保存刚才我们改写的motion_node.py文件。
请再次打开一个终端，输入之前的roslaunch指令
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
并回车，静待几秒后，命令行中出现下面的提示，这时，在键盘输入任意键即可开始运行整个程序。
.jpg)
成功了，智能车是不是动起来了！
恭喜！到此为止，你已经成功启动并完成了一次无人车寻迹行驶任务。
ok，现在你已经完成了基础寻迹关卡的任务了！
是不是很简单。不过，小e觉得还可以跑得更快。请尝试让小e更快地完成赛道任务。
回顾一下给小e发出线速度和角速度指令的设计过程
linear.x=0.1+0.28×( 1— |bias| ⁄ imagewidth/2)
angular.z=0.0023×bias
这里，线速度的计算中存在两个常量数值，尝试改变这两个常量，观察无人车的行为。我们可以先将常量改 变得尺寸略大一些，以方便观察智能车的行为变化，例如，我们将线速度改为
linear.x=0.1+0.2×( 1— |bias| ⁄ imagewidth/2)
如何在代码中调试呢？
接下来我们尝试调试代码的过程，如果智能车正在运行，请先在运行检测前进节点的nicsrobot_line_follower.launch文件的终端中键入ctrl+c，终止正在运行的launch文件，然后再调试。
首先，我们要让智能车回到起点，
在终端中输入
Python
1rosrun akm_control set_model_state.py AKM_1 0.5 -1.7 0
这个指令表示运行了一个设置智能车位置的节点
智能车就可以瞬间移动回起跑线啦。
我们打开motion_node.py文件，修改线速度的代码参数：
Python
1velocity.linear.x =1.0+ 0.20*(1.0- abs(bias)/(self.image_width/2) )
这样就将线速度的最小值限定为了1.0，保存motion_node.py文件，再在终端输入
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
再次启动launch文件，我们可以看到智能车前进的速度变快了。
同样的，每次调试程序前我们需要将智能车运行的launch文件程序中止，再发送指令让智能车回到起跑线。 然后修改角速度指令的权重参数，例如，我们将角速度代码参数改为：
angular.z=0.02×bias
Python
1self.center_band=100#车道线偏移在center_band内不用修正
需要注意的一点是，调整参数的过程中有可能会出现丢失车道线的情况。
请同学们打开rviz，调用智能车的摄像头image图像，查看以智能车第一视角观察到的图像。
.jpg)
如果智能车的视野中丢失了车道线，可以采用第三节中我们提供的解决方案，这个方案的代码在motion_node.py文件第57-59行中已经为同学们提供了，通过代码让智能车以0.15的线速度和0.8的角速度转圈 直到视野中检测到橙色像素点。
如果你的智能车出现了丢失车道线的问题，也可以尝试改变这部分代码的参数值，观察智能车不同的行为。
Python
同学们可以自行调整线速度和角速度的参数和常量，以及bias的偏移阈值，感受不同参数组合对智能车行为 的影响，调整参数到你认为最科学的组合，在最短时间内完成竞速赛吧！
帮助小工具：ubuntu目录以及文件操作方法
﹀帮助小工具：ubuntu目录以及文件操作方法
在使用ubuntu系统时，我们会需要对其中的文件进行操作。那如何找到在某个目录下的文件并进行操作呢？ 这是ubuntu的桌面界面，我们有两种进入目录管理文件的方法。
第一种是通过命令行，可以查阅1.4小节中关于命令行的"cd"操作，在命令行中进入某个文件夹。
第二种是通过图形化界面，像我们平日在windows系统中进行文件操作一样，ubuntu中的文件可以通过
进入到文件管理界面，默认的路径是主目录文件夹下
然后按照目录的路径，逐级双击打开文件夹就可以操作啦，和windows的操作几乎相同。
第五章 高级视觉智能—障碍赛实战
﹀第五章 高级视觉智能—障碍赛实战
5.1 从检测障碍物开始
欢迎来到课程的中级关卡，恭喜你已经完成了本课程的基础任务，相信到目前为止的内容对你来说完全是游 刃有余。那么在这一章节，我们将挑战更高的课程难度和逻辑能力，不过不用担心，我们仍会手把手带你了 解和学习完成障碍赛任务所需的所有资料。准备好了就开始吧！
完成中级任务所需的避障环境如下：
.png)
赛道还是原来的赛道，车子也是原来的车子，只不过，赛道上出现了一些红色的路障，路障的位置甚至挡到 了部分车道线，这种情况下，如何让车子既能完成车道线寻迹跑圈任务同时还能有效避开障碍物呢。解决这 个问题最重要的部分就是：检测障碍物，并规划避障路径。需要注意的是，为避免同学们冲出赛道，赛道两 侧的白色边线上设置有透明挡板。
5.2 避障路径规划
﹀5.2 避障路径规划
欢迎回到课程，这一小节我们开始一个简单的路径规划。
在前面基础关卡的寻迹任务中，我们给智能车发送的命令只是让其不要偏离车道线，一旦远离车道线超过阈 值，就发送指令让车子赶紧转向回来。在障碍赛阶段，这一策略在大部分时间内仍然是有效的，但是需要结 合实际的路况，做出更新的指令。
.png)
例如，在雷达探测范围内出现了路障时，应该如何设计智能车前进的指令呢？
.png)
5.3 接收雷达数据
﹀5.3 接收雷达数据
上一小节中我们为大家介绍了激光雷达，那么在程序中如何调用雷达的数据并处理呢？
这时，ROS系统的优势就体现出来了，我们不需要在硬件层面进行复杂的雷达驱动编写、程序烧录、调试等 工作，只需要在ROS系统中调用节点发送的雷达topic就可以了。而硬件层面的驱动等工程，在ROS系统中已 经预先封装过了。
这里，我们需要用到ROS系统的Subscriber函数，开启订阅雷达话题的功能。(英文subscribe意为订阅)
关于订阅话题接收数据的功能，在前面基础任务部分中，我们已经用subscribe函数订阅过摄像头的数据并进行了处理，可以参考detection_node_2.py文件的第24行。
(detection_node_2.py文件位于主目录/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_2)
这句话中参数的意义分别为:
.png)
- self.imageSub_ Subscriber函数的返回值
- rospy.Subscriber 调用ROS中名为Subscriber的函数
- /AKM_1/camera/rgb/image_raw
订阅名为'/AKM_1/camera/rgb/image_raw'的话题，话题名需要加单引号
- Image 话题的数据类型为Image
- self.imageCallback 订阅话题的同时，执行imageCallback函数
这句话表示，我们订阅了一个名为/AKM_1/camera/rgb/image_raw的话题，这个话题的数据类型为Image，订阅topic之后，执行imageCallback函数，Subscriber函数的返回值保存到self.imageSub_这个变量中。
（在编程中，大部分的函数都有一个“返回值”，例如：0或-1；用来表示处理完的数据或判断函数执行是否有误，所以我们可以定义一个变量来接收这个返回值）
目前我们知道它们的的调用用法就可以了。
请依样画葫芦，自己来写一句订阅激光雷达数据的语句。
这里，请同学们在detection_node_2.py文件的第26行自行用python编写一句代码，订阅名为/AKM_1/scan的话题，话题的数据类型为LaserScan,订阅话题后，执行lidarCallback函数，将Subscriber函数的返回值保存到self.lidarSub_这个变量中
完成后Subscriber函数的调用后，我们需要处理接收到的雷达数据，也就是lidarCallback函数。
我们这里的雷达返回的数据中包含了720个距离信息：它是将雷达水平360°逆时针方向上探测到的障碍物距离，以每0.5度为一个单位，保存的720个数据。
雷达数据的起始是从小车正后方开始，则我们关心的前方180°范围内障碍物信息位于表格的索引为(180~540)。
我们通过lidarCallback函数处理，将720个雷达数据保存为了一个720*1大小的列表，方便我们继续处理数据。（这里看能不能加一个从上图雷达的信息对应到表格数据的动画）
ok，雷达数据我们现在已经处理完了，最后一步操作：发送出去表格，让需要接收雷达数据的节点能够接收到我们处理的720维表格。
发送的方案非常简单，是ROS系统的基本用法，我们在程序里预先为同学们设定好了，不需要大家编写，但请理解并能迁移到其他的用法。
1. 第一步：定义一个Publisher
2. 第二步：定义Publisher函数发送的内容
3. 第三步：定义发送内容的格式和数据源，格式为Float32MultiArray（32位浮点数的数组）
好了，现在雷达返回的数据已经变成了一个尽在掌控的720*1维的表格，通过话题的收发操作，我们随时可以接收到它。下一小节中，我们就要利用雷达的数据列表和车道线信息，完成障碍赛路径规划的实践和操作了。
准备好了吗？！
5.4 开车调试——中级关卡
﹀5.4 开车调试——中级关卡
恭喜同学们来到课程的最后一步了——挑战障碍赛关卡！
这一章节的内容稍微晦涩了一些，不过还好，我们已经完成了一大部分。
现在我们的手中，有车道线偏移信息以及雷达返回的全方位障碍物位置信息，让我们做出路径规划吧！
这一章最终的任务，需要同学们自行创造一些代码，我们会给大家代码的目的和逻辑，请同学们用python编 写出来。
首先，回顾一下在路径规划那一小节思考的路径方案，课程教案里给同学们提供了两个方案做参考，但相信 你一定想到了更好的方案，那么如何在程序里执行我们的方案呢，以示例方案为例，我们将教你如何编写代 码实现。
我们以第二种方案为例：
.gif)
在小车行驶的过程中，持续关注前方障碍物信息，永远选择障碍物最远的角度作为避障方向。但同时应引入 对障碍物距离的阈值判定，如果障碍物已经非常接近了，要加大转弯角度，紧急避障。
在此基础上，关注车道线的偏移量，最终转向弧度的计算方法为
angularnew = angular + Kp*dir
这个公式我们也在5.2小节为大家解释了意义。
接下来，我们需要把这部分想法写到motion_node_2.py文件中。
（文件位于主目录/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_2）
motion_node_2.py文件中大部分的基础设置代码我们也为同学们完成了，希望同学们能根据代码和教案的提 示，自行完成89行的lidar_avoid函数内容的编写。
.png)
我们为大家讲解一下代码编写的一种思路，希望同学们能理解后，自行创造代码。
lidar_avoid的功能为：接收了车道线偏移计算的angular弧度参数，操作雷达数据列表，进行判断和赋值计 算，计算出避障的弧度，最终根据权重公式，输出angular_new（智能车的转向弧度）。
首先，我们定义一些需要用到的参数包括：
K_p 为权重公式中障碍物转向值的权重
lidar_cut 为裁剪了雷达数据列表后得结果
裁剪雷达数据是为了删除小车身后的障碍物信息干扰，将表格索引和转向角度值相互对应。请同学们编写一 句代码，裁剪出self.lidar_data中前方障碍物的信息保存到lidar_cut中。这样，表格索引就和转向角度相互对 应起来了。
max_idx lidar_cut 列表中数值最大值的位置，也就是列表中的序号
max_idx参数找到列表中最大值的位置，也就是障碍物位置最远的角度信息，因为雷达列表中每0.5°为一个 单位，所以列表中的序号就表示方向。这里，请同学们编写代码，找出lidar_cut中，数据最大值所在的坐标 位置，并赋值给max_idx。
right_min_idx 从列表中起始位置到最大值位置间的索引中，找到最小值位置的索引序号
这句话比较绕口，让我们通过一张图来理解：
假设障碍物距离最远的方向为max_idx，由于车道线权重的存在，难以保证小车行驶中能完全躲开障碍物， 这时，我们设定一个检测，如果在水平方向上距离最近的障碍物索引right_min_idx的值小于一定阈值时，我 们就认为小车快要撞上了，必须转向。
这里，需要同学们编写一个判断语句。如果这个最近障碍物的距离小于阈值了，则需要额外加大转向角度；
如果最小障碍物的距离大于阈值，就保持最大障碍物方向的角度转向。
.png)
dir_idx 转向角度
当距离障碍物过近时，转向角应尽量远离障碍物；当距离障碍物较远，转向角朝向障碍物最远的方向。
dir 转向弧度（角度—弧度变换） 请将刚才计算出来的转向角度，转换为转向弧度
angular_new 最终的输出转向值
ok，我们已经将示例代码的逻辑呈现给同学们了，相信同学们一定有自己的想法并且跃跃欲试了。
请按照自己的逻辑和路径设计方案，任意修改并创造代码吧。完成编写后，需要保存代码才可以运行roslaunch，请注意python语言编写中的缩进关系。如果出现了报错提示，请尝试自己寻找解决方案。
你也可以定义自己需要的变量，请记得备份有效的代码文件，尝试更高效的避障跑圈方案吧。
比比看，谁可以在最短时间内完成避障寻迹任务！
智能机器人专题无人车障碍竞速赛指南
﹀智能机器人专题无人车障碍竞速赛指南
选手需将竞速用到的工作空间代码（zip格式压缩包）上传至竞赛平台。
上传的代码跑分结果可在“个人中心”-“我的竞赛”中查看，代码上传完成后需等待几分后台测试用时
如果代码运行失败，将会产生报错提示（例如编译失败、由于撞墙等导致用时过长等）
可以点击查看文件报错提示（这里的提示只提示裁判的反馈，不提示代码运行错误）
初级和中级以及高级赛关卡的赛道，可通过对应的ROS系统launch文件中调用。
初级赛道环境调用命令为：
roslaunch nics_bringup nics_gazebo_simple.launch
中级赛道环境调用命令为：
roslaunch nics_bringup nics_gazebo_block.launch
高级赛道环境调用命令为：
roslaunch nics_bringup nics_gazebo_actor_new.launch
初中级赛道周边有透明围栏，避免智能车冲出赛道，透明围栏可以被激光雷达检测到；高级赛道周边无透明围栏，不允许冲出赛道行驶。
竞赛只采纳个人的历史最优成绩，可以多次上传代码，跑圈用时过多将不显示。
各赛道每人每天有三次平台上传代码机会，其余时间同学可以在仿真空间本地中进行代码调试计时，计时基于 ros time ,与仿真器完全一致，而非真实时间。
仿真空间本地裁判计时方法如下：
同学们请先在ROS系统中launch工作空间成功后，再将代码提交给裁判计时，裁判只负责按照设计的指令运行计时，不负责指出ROS指令和代码的错误。
仿真空间中
打开终端，进入timekeeper目录下，运行：（并回车，静待返回结果）
python3 main_debug.py --track_id 1 --workspace_path
工作空间.zip的路径
linux系统中, “~”表示当前用户的主目录，也就是当前用户文件管理内的Home目录；“/”表示根目录，也就是整个系统的最顶层目录；“./”表示当前目录；“...”表示上层目录
参数表示:
track_id：仿真赛道编号，1、2、3分别对应初级、中级、高级赛道
workspace_path：用户的ROS工作空间路径，压缩为zip格式
工作空间结构说明:
所上传的ROS工作空间名称随意，格式必须为zip压缩包
工作空间至少需要包含名为“nicsrobot_line_follower”的功能包，如下所示：
本程序将自动重新编译整个ROS空间，并根据所选择的赛道，对应执行如下命令，开始计时：
初级赛道：roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
中级赛道：roslaunch nicsrobot_line_follower nicsrobot_line_follower_2.launch
高级赛道：roslaunch nicsrobot_line_follower nicsrobot_line_follower_3.launch
同学务必保证程序启动指令与上面一致。
裁判机会自动运行跑圈代码并计时。
输出结果：
code：状态码，结果为0代表成功，非0代表错误
msg：状态描述，结果为 success 或者timeout 或者具体报错信息
castTime：花费时间，单位：秒
以上调试可参考视频在仿真空间环境中直接使用，无需配置。
结语
﹀课程结语
智能机器人实践的入门课程到此就结束啦，同学们可以在课程的“在线机器人仿真系统”中调试代码和智能车，每次调试完成后请将代码备份，避免云端服务器丢失数据。
对智能机器人系统感兴趣的同学，欢迎持续关注我们后续推出的课程和竞赛。
欢迎智能机器人相关的课程和赛事合作。
出品：
清华大学电子工程系
制作：
清华大学电子工程系协同智能团队
指导老师：
汪玉沈渊
课程设计：
夏妍余金城杨天翔向云飞唐佳昊郭中贺
鸣谢：
颜钰陈加聪白鑫鑫
合作和联系：
Email：nics_robo@mail.tsinghua.edu.cn
Tel：+86 15652764391
地址：清华大学电子工程系罗姆楼4F
智能无人机技术设计实践
目录
课程概述
﹀课程概述
无人机是近几年来的热门研究领域，商业上，涌现了一批创业公司，四旋翼飞行器已经逐渐在各个领域中投入使用； 军事上，无人作战平台在未来作战模式中占据主导地位。科研上，无人机本身的控制、通信、定位等问题有着较高的研究价值。
清华大学智能无人机挑战赛（全校本科生编程类赛事）是由校团委发起，由校学生科协、电子工程系学生科协、航天航空学院学生科协主办，旨在鼓励对无人机智能操控感兴趣的同学发展兴趣爱好，通过课程学习掌握相关理论知识，参与创新实践，在赛事中挑战自我，做出优秀作品。
快速开始
1、请在在线机器人仿真系统中，点击终端
2、输入指令
roslaunch nics_bringup nics_gazebo_simple.launch
3、按下回车键，进入3D机器人仿真环境中
4、右键终端，开启一个新终端
5、输入指令
roslaunch nics_line_follower nics_line_follower.launch
6、回车键，根据屏幕提示按下键盘任意键，3D仿真环境中的智能车引擎启动，智能车开始沿着跑道自动驾驶跑圈了！
这整个过程是如何完成的呢，这门课程将带你逐渐了解这个充满乐趣和知识领域。前5个章节中，我们通过智能车的仿真模型带大家学习ROS机器人操作系统和ubuntu的基础；通过ROS任务链挑战，熟悉ROS的概念和操作；第6-8章节，为同学们介绍更高级的无人机理论和实践内容。
第一章 机器人系统概述
﹀第一章 机器人系统概述
1.1 机器人系统运作方式
欢迎大家来到课程主线内容，相信现在你一定对机器人系统充满了好奇。
机器人是如何感知外界环境并且能够自主行动起来的呢？
这一小节，我们将带你走进机器人领域，对智能车这样简单的机器人系统运作方式产生初步的认知。
1.2 认识三个好朋友：Ubuntu、虚拟机、命令行
﹀1.2 认识三个好朋友：Ubuntu、虚拟机、命令行
1.2.1 Ubuntu
Ubuntu是一个以桌面应用为主的Linux操作系统，从前人们认为Linux难以安装、难以使用，在Ubuntu出现 后这些都成为了历史。Ubuntu也拥有庞大的社区力量，用户可以方便地从社区获得帮助。作为全球最流行且 最有影响力的Linux开源系统之一，Ubuntu自发布以来在应用体验方面：有较大幅度的提升，即使对比Windows、MacOS等操作系统，最新版本的Ubuntu也不逊色。
1.2.2 虚拟机
虚拟机（Virtual Machine）指通过软件模拟的具有完整硬件系统功能的、运行在一个完全隔离环境中的完整 计算机系统。在实体计算机中能够完成的工作在虚拟机中都能够实现。在计算机中创建虚拟机时，需要将实 体机的部分硬盘和内存容量作为虚拟机的硬盘和内存容量。
1.2.3 命令
一般说的“命令行”是指linux命令，linux命令是对Linux系统进行管理的命令。对于Linux系统来说，无论是中 央处理器、内存、磁盘驱动器、键盘、鼠标，还是用户等都是文件，Linux系统管理的命令是它正常运行的核 心，与之前的DOS命令类似。
命令行是用户通过键盘输入字符指令、计算机输出字符结果的一种人机交互的方式。与图形界面（GUI）主 要使用鼠标、图像等直观视觉交互方式不同，命令行通常需要用户记忆操作的命令。在熟记命令的前提下， 使用命令行往往要较使用图形用户界面的操作速度要快，并且更容易执行批量操作。
Tips 基于ubuntu的机器人在线仿真系统使用指南
﹀Tips 基于ubuntu的机器人在线仿真系统使用指南
一、点击网页导航栏的“在线仿真”模块。
二、如未注册或登陆，请先注册网站用户并登陆。
三、登陆后，可以使用公共仿真环境。请同学们点击去填写，补充完整资料，以获得独立的仿真空间IP和密码。
四、补充姓名、学校、专业和班级信息后点击提交（请使用真实信息以便助教审核同学们的信息）。
五、信息补充完后，页面最下会出现在线仿真系统的登陆密码，记录密码。
六、点击页面导航栏“在线仿真”，将跳转到个人的机器人系统仿真空间。
七、请在登陆界面点击连接后，输入后台分配的登陆密码。
如果出现页面停留在“连接中....”的情况，请刷新网页，如果问题仍存在，请联系助教。
八、登陆后，即可进入已经配置完成机器人操作系统环境的ubunt18.04系统。
tips：网页左边栏的工具栏可以点击拓展，包括键盘指令、剪贴板、放缩等设置，其中缩放模式请勿选择“远程调整大小”，避免网页仿真分辨率出现异常，建议选择“本地缩放”。请勿修改高级设置中的内容。
九、在线仿真系统的操作和ubuntu系统相同，需要注意复制粘贴指令不能直接输入。在线仿真系统和本地端间的文件不能直接传递，推荐同学们使用清华云盘（或微信等文件传输工具）上传下载文件。
十、请勿更改系统中的用户名和密码，避免忘记密码丢失数据。
十一、请勿随意删改系统中非自主（带锁）创建的文件，避免数据丢失。
十二、在线仿真系统每人独享环境，数据可以保存在在线云端服务器中，但强烈建议在每次调试后做好代码的备份和本地下载，避免由于云端受到攻击导致数据丢失的损失。
十三、在线仿真系统中对ROS的基本操作请参考课程内容学习，如发生误删代码、无法登录、无响应等问题，请联系助教。
请勿使用在线仿真系统服务器进行非法上网操作。
实验：启动ROS!
打开终端有两种方法：
1、按快捷键"ctrl+alt+T"打开终端（terminal）
2、在桌面点击右键，点击terminal
按"ctrl+alt+T"打开终端（terminal），在当前路径下输入
$ roscore
若能看到下面这个界面，就成功启动ros啦。
按"ctrl + c" 可以退出当前的ROS进程。
1.3 认识node和topics
﹀1.3 认识node和topics
1.3.1 Node简介
在ROS的世界里，最小的进程单元就是节点（node）。一个软件包里可以有多个可执行文件，可执行文件在 运行之后就成了一个进程(process)，这个进程在ROS中就叫做节点。
从程序的角度来说，node就是一个可执行文件。从功能角度来说，通常一个node负责者机器人的某一个单 独的功能。由于机器人的功能模块非常复杂，我们往往不会把所有功能都集中到一个node上，而会采用分布 式的方式，把鸡蛋放到不同的篮子里。例如有一个node来控制底盘轮子的运动，有一个node驱动摄像头获 取图像，有一个node驱动激光雷达，有一个node根据传感器信息进行路径规划……这样做可以降低程序发 生崩溃的可能性，试想一下如果把所有功能都写到一个程序中，模块间的通信、异常处理将会很麻烦。
1.3.2 启动Node
当我们要启动ROS时，首先输入命令:
代码
1$ roscore
此时ROS master启动。
ROS中它被赋予了专用的名字——node。我们知道一个package中存放着可执行文件，可执行文件是静态的， 当系统执行这些可执行文件，将这些文件加载到内存中，它就成为了动态的node。具体启动node的语句是：
rosrun + 包名+节点名
翻译成代码就是
代码
1$ rosrun pkg_name node_name
通常我们运行ROS，就是按照这样的顺序启动，有时候节点太多，我们会选择用launch文件来启动，下一小 节会有介绍。 Master、Node之间以及Node之间的关系如下图所示：
1.3.3 ROS的四种通信方式
在我们上节课讲到，ROS为机器人系统提供了通信架构，所以通信方式是ROS最为核心的概念。
1、Topic话题
2、service服务
3、Parameter Service参数服务器
4、Actionlib动作服务器
1.3.4 Topic简介
我们这门课，只需要用到topic（话题）这种通信方式。
ROS中的通信方式中，topic是常用的一种。对于实时性、周期性的消息，使用topic来传输是最佳的选择。 topic是一种点对点的单向通信方式，这里的“点”指的是node，也就是说node之间可以通过topic方式来传递 信息。topic要经历下面几步的初始化过程：首先，publisher节点和subscriber节点都要到节点管理器进行注 册，然后publisher会发布topic，subscriber在master的指挥下会订阅该topic，从而建立起sub-pub之间的 通信。注意整个过程是单向的。其结构示意图如下
我们以摄像头画面的发布、处理、显示为例讲讲topic通信的流程。在机器人上的摄像头拍摄程序是一个 node（圆圈表示,我们记作node1），当node1运行启动之后，它作为一个Publisher就开始发布topic。比如 它发布了一个topic（方框表示），叫做/camera_rgb，是rgb颜色信息，即采集到的彩色图像。
同时，node2假如是图像处理程序,它订阅了/camera_rgb这个topic，经过节点管理器的介绍，它就能建立和 摄像头节点（node1）的连接。 是不是很简单~~！
这里我要给大家讲一个知识点，topic的通信方式是异步的！
那么怎么样来理解“异步”这个概念呢？
在node1每发布一次消息之后，就会继续执行下一个动作，至于消息是什么状态、被怎样处理，它不需要了 解；而对于node2图像处理程序，它只管接收和处理/camera_rgb上的消息，至于是谁发来的，它不会关心 。所以node1、node2两者都是各司其责，不存在协同工作，我们称这样的通信方式是异步的。
ROS是一种分布式的架构，一个topic可以被多个节点同时发布，也可以同时被多个节点接收。比如在这个场 景中用户可以再加入一个图像显示的节点node3，我们在想看看摄像头节点的画面，则可以用自己的笔记本 连接到机器人上的节点管理器，然后在自己的电脑上启动图像显示节点。
这就体现了分布式系统通信的好处：扩展性好、软件复用率高。
总结两点：
1. topic通信方式是异步的，发送时调用publish()方法，发送完成立即返回，不用等待反馈。
2. topic可以同时有多个subscribers，也可以同时有多个publishers。
1.3.5 操作命令
在实际应用中，我们应该熟悉topic的几种使用命令，下表详细的列出了各自的命令及其作用。
| A | B | |
| 1 | 命令 | 作用 |
| 2 | rostopic list | 列出当前所有的topic |
| 3 | rostopic info topic_name | 显示某个topic的属性信息 |
| 4 | rostopic echo topic_name | 显示某个topic的内容 |
| 5 | rostopic pub topic_name ... | 向某个topic发布内容 |
| 6 | rostopic bw topic_name | 查看某个topic的带宽 |
| 7 | rostopic hz topic_name | 查看某个topic的频率 |
| 8 | rostopic find topic_type | 查找某个类型的topic |
| 9 | rostopic type topic_name | 查看某个topic的类型(msg) |
1.4 认识launch文件
﹀1.4 认识launch文件
1.4.1 简介
机器人是一个系统工程，通常一个机器人运行操作时要开启很多个node，对于一个复杂的机器人的启动操作 应该怎么做呢？当然，我们并不需要每个节点依次进行rosrun，ROS为我们提供了一个命令能一次性启动 master和多个node。该命令是：
$ roslaunch pkg_name file_name.launch
如果master没有启动，那么roslaunch就会首先启动master，然后再按照launch的规则执行。launch文件里 已经配置好了启动的规则。所以 roslaunch 就像是一个启动工具，能够一次性把多个节点按照我们预先的配 置启动起来，减少我们在终端中一条条输入指令的麻烦。
1.4.2 写法与格式
launch文件是一种标签文本，它的格式包括以下标签：
XML
参考链接:http://wiki.ros.org/roslaunch/XML
1.4.3 示例
launch文件的写法和格式看起来内容比较复杂，我们先来介绍一个最简单的例子如下：
XML
这是ros官网给出的一个最小的例子，文本中的信息是，它启动了一个单独的节点"talker",该节点是包" rospy_tutorials "软件包中的节点。
然而实际中的launch文件要复杂很多，同学们只需了解我们提供的launch文件即可
1.4.4 小结
对于初学者，我们不要求掌握每一个标签是什么作用，但至少应该有一个印象。如果我们要进行自己写 launch文件，可以先从改launch文件的模板入手，基本可以满足普通项目的要求。
1.4.5 小实验：搭建驾驶环境
PS:若想在终端里粘贴代码，需要"ctrl+shift +v"而不是"ctrl+v"
第一步：进入仿真环境
打开终端，输入
roslaunch nics_bringup nics_gazebo_simple.launch
看到这个界面，我们就成功启动launch文件，进入到智能车仿真驾驶界面啦。
第二步：退出
在终端里按"ctrl+c"退出仿真界面
1.5 单元测试一
﹀1.5 单元测试一
1.机器人操作系统的全称是?
提交
2.下列哪个不是ROS的特点?
提交
3.ROS最早诞生于哪所学校的实验室?
提交
4.（多选）下列哪几个命令可以启动ROS Master？
提交
5.关于ROS Node的描述，哪一项是错误的?
提交
6.关于.launch文件的描述，以下哪一项是错的?
提交
7.想要查看`/odom`话题发布的内容，应该用哪个命令?
提交
8.(多选)关于Topic通信的描述，正确的选项有：
提交
第二章 让智能车跑起来
﹀第二章 让智能车跑起来
2.1 智能车数字仿真—Gazebo
2.1.1 简介
ROS中的工具就是帮助我们完成一系列的操作，使得我们的工作更加轻松高效。ROS工具的功能大概有以下 几个方向：仿真、调试、可视化。本节课我们要学习的Gazebo就是实现了仿真的功能，而调试与可视化由 Rviz、rqt来实现。
2.1.2 认识Gazebo
对于Gazebo,大家可能比较陌生，它是机器人仿真工具。在topic通信中，我们的demo都是在Gazebo中实现。 Gazebo是一个机器人仿真工具，模拟器，也是一个独立的开源机器人仿真平台。当今市面上还有其他的仿 真工具例如V—Rep、Webots等等。但是Gazebo不仅开源，也是是兼容ROS最好的仿真工具。
Gazebo的功能很强大，最大的优点是对ROS的支持很好，可以进行机器人的运动学、动力学仿真，能够模 拟机器人常用的传感器（如激光雷达、摄像头、IMU等），也可以加载自定义的环境和场景。
2.1.3 仿真的意义
仿真不仅仅只是做出一个很酷的3D场景，更重要的是给机器人一个逼近现实的虚拟物理环境，比如光照条 件、物理距离等等。设定好具体的参数，让机器人完成我们设定的目标任务。比如一些有危险因素的测试， 就可以让机器人在仿真的环境中去完成，例如无人车在交通环境复杂的交通要道的效果，我们就可以在仿真 的环境下测试各种情况无人车的反应与效果，如车辆的性能、驾驶的策略、车流人流的行为模式等，又或者 各种不可控因素如雨雪天气，突发事故，车辆故障等，从而收集结果参数指标信息等等，只有更大程度的逼 近现实，才能得出车辆的真实效果。直到无人车在仿真条件下做到万无一失，才能放心的投放到真实环境中 去使用，这即避免了危险因素对实验者的威胁，也节约了时间和资源，这就是仿真的意义。
通常一些不依赖于具体硬件的算法和场景都可以在Gazebo上仿真，例如图像识别、传感器数据融合处理、 路径规划、SLAM等任务完全可以在Gazebo上仿真实现，大大减轻了对硬件的依赖。
2.1 实验:在Gazebo中手动驾驶智能车
﹀2.1 实验:在Gazebo中手动驾驶智能车
2.1.1 gazebo操作说明
•平移：鼠标左键
•旋转：鼠标滚轮中键
•放缩：鼠标滚轮
•界面左侧是控制面板
•导入模型就在控制面板的insert,可以直接拖入模拟空间，也可以按需自制模型拖入。
2.1.2 实验：搭建驾驶环境——用键盘驾驶智能车
那么如何在Ubuntu系统中，打开驾驶仿真环境，并且用键盘驾驶智能车呢？
我们需要用到2个launch文件。
1、nics_gazebo_simple.launch，它是我们写好的的gezebo仿真器；
2、keyboard_teleop.launch ，它是用键盘控制智能车的launch文件。
接下来我们开始实验
PS:若想在终端里粘贴代码，需要"ctrl shift +v"而不是"ctrl+v"!!!
第一步 ：打开仿真界面
和我们上一章节的实例测试一样，我们打开终端，输入
roslaunch nics_bringup nics_gazebo_simple.launch
可以看到gezebo仿真界面
第二步 ：启动键盘控制节点
按快捷键"ctrl+alt+T"另外打开一个新的终端，输入
roslaunch akm_control keyboard_teleop.launch
启动键盘控制的launch文件
在终端里我们可以看到键盘控制的键位
i是前进 u是向左前进 o是向右前进 k是刹车 ,是倒车
现在我们就用键盘来控制小车前后左右行走吧！
注意！！！我们需要在键盘控制的终端界面，来按"i"、"u"、"o"等指令。在其他界面按前后左右的指令,小车动不了！
你会发现小车动得特别慢！
怎么让小车走的快一点呢？这个时候你可以看一眼下面的红框
按一次"q"可以让小车提速10%！
第三步 ：在终端中查看车辆的线速度和角速度
再新打开一个终端，输入
roslaunch akm_control keyboard_teleop.launch
我们可以看到返回的线速度和角速度
2.1.3 小结
虽然Gazebo目前的功能还称不上强大，同时还存在着一些BUG，但是对于我们的入门学习也已经是足够了， 随着版本的更新，Gazebo也在越来越强大。
2.2 实验：ROS的调试工具—RViz
﹀2.2 实验：ROS的调试工具—RViz
2.2.1 简介
本节课介绍的是我们在ROS开发中非常常用的一个工具，基本上调试和开发都离不开这个工具——RViz(the Robot Visualization tool)机器人可视化工具，可视化的作用是直观的，它极大的方便了监控和调试等操作。 例如能在窗口中看到机器人模型和轨迹、激光雷达、点云地图。
2.2.2 实验：在RViz中添加智能车的第一视角
第一步 ：进入仿真环境
依然打开教材的模拟场景，输入
roslaunch nics_bringup nics_gazebo_simple.launch
第二步 ：打开RViz
之后打开新的终端，直接输入 " $ rviz "
打开rviz工具。
第三步 ：添加智能车的第一视角
在这里我们想看智能车摄像头的第一视角，需要点击“Add”
在By topic里添加image，点"ok"
RViz的插件种类繁多功能强大，非常适合我们开发调试ROS程序。
2.2.3 差异
虽然从界面上来看，RViz和Gazebo非常相似，但实际上两者有着很大的不同，Gazebo实现的是仿真，提供一 个虚拟的世界，RViz实现的是可视化，呈现接收到的信息。左侧的插件相当于是一个个的subscriber,RViz接收 信息，并且显示。所以RViz和Gazebo有本质的差异。
2.2.4 小结
RViz和Gazebo是我们常用的ROS工具，更好的利用这些工具是我们ROS进阶的基础。具体的操作和使用可以 参考我们的官方演示视频，跟着视频去实战演练，熟悉这两个工具。
2.3 实验：ROS的调试工具—RViz
﹀2.3 实验：ROS的调试工具—Rqt
2.3.1 简介
rqt是一个基于qt开发的可视化工具，拥有扩展性好、灵活易用、跨平台等特点，主要作用和RViz一致都是可 视化，但是和RViz相比，rqt要高级一个层次。
2.3.2 命令
rqt_graph :显示通信架构
rqt_graph是来显示通信架构，也就是我们上一章所讲的内容节点、主题等等，当前有哪些Node和topic在运 行，消息的流向是怎样，都能通过这个语句显示出来。此命令由于能显示系统的全貌，所以非常的常用。
2.3.3 实验：查看智能车的通信架构
1. 首先打开我们教材的模拟场景，输入
roslaunch nics_bringup nics_gazebo_simple.launch
2.输入命令语句" rqt_graph ",显示出了当前环境下运行的Node和topic，十分直观的看到通信结构以及消息流 向。注意在椭圆形的代表节点，矩形代表topic。（可以用鼠标滚轮放大缩小画面）
2.3.4 小结
rqt_graph这个功能是强大的，它使得我们初学者可以直观的看到ROS的通信架构和信息流，方便我们理解的同时，也使得我们能够最快的纠错等等。
2.4 单元测试二
﹀2.4 单元测试二
1.Gazebo是一款什么工具?
提交
2.rqt_graph可以用来查看计算图，以下说法错误的是：
提交
3.(多选)RViz可以图形化显示哪些类型的数据？
提交
4.(多选)关于仿真环境的描述，以下说法正确的是：
提交
第三章 python编程起步—用程序和智能车对话
﹀第三章 python编程起步—用程序和智能车对话
3.1 认识Python
3.2 实验：智能车喊话：Hello world !
﹀3.2 实验：智能车喊话：Hello world !
3.2.1 实验：智能车喊话：hello world ！
print语句
如果要让Python打印出指定的文字，可以用print()函数，然后把希望打印的文字用单引号或者双引号括起来，但不能混用单引号和双引号：
实战开始：
1、首先我们打开桌面的Visual Studio Code，它是我们今后要用到的代码编辑器！
新建一个文件
输入 print("hello world")
保存到桌面
重命名为 test.py
接着打开终端，输入
代码
1 cd Desktop
进入到桌面路径（因为代码刚刚保存在桌面；按“tab”键可以补全Desktop的拼写哦）
输入
代码
1python test.py
回车，我们可以看到“hello world”字符，在终端打印出来啦！
3.3 实验：如果红灯，请停车！
﹀3.3 实验：如果红灯，请停车！
每天的生活中，我们都在不断地做出一些判断。例如，每天有出门的时候，我们会决定今天穿什么衣服吃什 么样的早餐的。同时，每个判断背后都有一个理由，比如考虑到天气太冷了，就穿厚一点的衣服，所以理由 是用来做判断的条件。
如果要让小车能够在面对不同的路况时，做出一些优化的操作，就需要学习条件语句来做一些判断判断。判 断的条件来自小车对周围路况的“感知”。
3.3.1 if条件语句
当传感器获得的数据符合某种条件时，就会让小车执行相应策略的指令。这样的过程在计算机中可以理解为：
如果满足A条件：
那么执行a指令
如果满足B条件：
那么执行b指令
否则：
执行c指令
这就是一条简单的条件结构转化为计算机语句就是：
if 条件A：
指令a
elif 条件B：
指令b
else：
指令c
3.3.1 if条件语句
我们实现以下任务：
当小车检测到红灯时打印"stop"，检测到绿灯打印"go"，若既不是红灯也不是绿灯，则输出"no signal"
这样的要求可以通过下面的语句来实现：
Python
在写代码之前，建议不要用“复制”-“粘贴”把代码从页面粘贴到你自己的电脑上。写程序也讲究一个感觉， 你需要一个字母一个字母地把代码自己敲进去，在敲代码的过程中，初学者经常会敲错代码：拼写不对，大 小写不对，混用中英文标点，混用空格和Tab键，所以，你需要仔细地检查、对照，才能以最快的速度掌握 如何写程序。
建议同学们亲自敲打一遍
我们在vscode里新建文件，输入上面那段代码
保存在桌面，命名为test2.py
和之前test1.py一样，在终端运行
输入red
会输出stop
输入green
会输出go
输入其他的字符串
则会输出no signal
3.4 单元测试三
﹀3.4 单元测试三
1.（多选）以下关于Python的说法正确的是?
提交
第四章 初级视觉智能—完成基础寻迹关卡
﹀第四章 初级视觉智能—完成基础寻迹关卡
欢迎同学们来到赛道实践。
经过前面章节的铺垫学习，这一章节我们将带大家动手控制智能车完成寻迹任务。
我们首先来思考一下如何完成这样一个任务。
在这个任务中，任务的输入是环境信息，输出是智能车的ROS控制信号，我们需要做的就是处理环境信息，并决策，将ROS控制信号发送出去。
那么如何拿到完成任务需要参考的环境信息呢，采用视觉的方案是一种经典的方案。
欢迎同学们来到赛道实践。
4.1 计算机视觉
计算机视觉是研究如何使机器学会“看”的科学，也是人工智能和大量机器人系统的基础。这门科学体量庞 大、原理多样、应用领域很广泛。
但是请不要担心，这一章节我们会带你从零开始了解计算机视觉并动手操作计算机视觉的处理过程，经过这 一章节的学习，我们就能够让智能车自动完成跑圈任务。
本章共包含五个小节的内容，在第五小节我们会教大家手把手在Gazebo仿真器中调试车子。
由摄像头拍摄的周围环境图像，经过ROS系统通信传输之后，到达智能无人车的大脑（计算机）进行处理， 这是智能车获取周围环境信息和自身相对位置的重要方式。下面的视频将为你简单介绍计算机中处理图像的 基本知识，帮助你基本了解计算机是如何从图像中提取、处理信息的。
对计算机视觉及进行图像处理技术感兴趣的同学，欢迎关注我们后续推出的视觉方面的课程。
4.2 学会提取图像颜色信息
﹀4.2 学会提取图像颜色信息
上一节中我们了解了计算机中对图像信息的存储，大多采用矩阵存储0~255这些数字的形式。那么如何利用 这些矩阵中数字呢？
我们通过完成一项简单的智能车任务——寻迹，即无人车自动检测位置，并围绕赛道行驶一圈的任务，理解 提取图像信息的过程。
首先，来看一下咱们无人车跑圈的赛道场地。
第一步分析一下场地的特点：场地由绿色、黑色、白色、橘黄色组成，在这之中，我们希望无人车能够一直 沿着场地中橘黄色的线行驶。那么我们可以将任务可以分解为几个容易完成的任务链了：
1.找到图像中橘黄色的部分
2.确定橘黄色部分和目前无人车之间的相对位置
3.发送控制信号让无人车的中心靠近橘黄色区域并向前移动
视频中我们将教大家如何逐步完成这三个小任务链。
ok，我们已经完成了任务链的前两项任务了，距离完成智能车寻迹只剩下最后一个小任务了。
4.3 发出前进指令
﹀4.3 发出前进指令
上一小节中，我们将整体任务拆分成了小任务链，并完成了前两项小任务。
1.找到图像中橘黄色的部分
2.确定橘黄色部分和目前无人车之间的相对位置
3.发送控制信号让无人车的中心靠近橘黄色区域并向前移动
现在我们已经通过颜色信息提取出智能车偏离车道线的偏移量bias了，如何根据偏移量控制智能车运动呢？
在前面的课程中我们学到了控制智能车车前行：通过ROS topic发送线速度和角速度指令。因此，这里我们 要设计一个关于偏移量bias（y1-y2）和topic指令中线速度、角速度的函数。最后一步把topic发送出去即可 控制智能车按照我们的设计自动前进。
下面的视频将为你介绍如何设计关于偏移量bias和智能车线速度、角速度的函数。
三个小任务我们都完成了，距离动手实践只有一步之遥了。我们还需要设计一个应急预案，保证智能车在赛 道上的稳定性，应对丢失车道线等突发情况。
4.4 安全驾驶
﹀4.4 安全驾驶
经过前面三个小节的学习，相信同学们已经对完成任务胸有成竹，跃跃欲试想要操纵智能车开始竞速赛了。 别着急，还有最后一个问题需要注意，智能车驾驶的稳定性。
在自动驾驶的过程中，我们需要对可能遇到的异常驾驶情况做出提前的预案。这一小节内容非常简短，我们 将从实战的角度为大家讲解两个需要注意的事项。
4.5 开车调试——基础关卡
﹀4.5 开车调试——基础关卡
前面的四个小节中，我们已经掌握如何让智能车自动提取橙色的赛道线信息，并设计智能车前进的指令。 这一小节，我们将在实际场景的孪生仿真环境中调整和测试智能车，让它能够完成自动寻迹的基础任务。 需要提示大家非常重要的一点：
在调试的过程中，请注意做实验记录，记录不同参数带来的实验结果；同时，请将可以正常运行的程序文件 另存到另一个安全的目录下，制造一个可供恢复的原始time machine，以便在误操作时能够还原到之前正常 状态的代码。
ok，让我们开始吧！
视频版教程：
重要提示：如果代码报错，请同学们一定要仔细检查python语句中的缩进和逻辑关系。
文字版教程：
欢迎同学们来到赛道，在基础任务中我们将调用到两个launch文件。
第一个launch文件是打开仿真器环境的文件，请同学们打开一个终端，输入代码：
Python
1roslaunch nics_bringup nics_gazebo_simple.launch
并回车，静待几秒之后，我们将进入仿真环境中。
要让智能车动起来，我们还需要发出动作命令，首先是车道线检测节点，其次前进指令节点，这两个节点可 以通过另一个launch文件启动。
请再打开一个新的终端，右键点击终端，选择新终端，输入代码
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
并回车，静待几秒后，会发现命令行中出现了红字报错，这是因为目前的程序中缺少了几段代码。
不过不要着急，经过这一小节的学习，你将能够完成这些代码，并让智能车动起来。
Start
首先，我们在刚才报错的终端中键入ctrl+c终止这段出错的文件运行并关闭它。打开这个launch文件查看一 下它所运行的节点文件。这个launch文件位于主目录下的catkin_ws/src/nics_gzb/nicsrobot_line_follower 下的launch文件夹中。打开launch文件可以看到，启动的节点分别是detection_node和motion_node，分别 对应上层目录中scripts文件夹下的.py文件。
那么让我们从起点开始，以寻迹的过程为线索，寻找节点文件中缺少了哪部分代码。
首先智能车调用摄像头的图像，
接收到图像后，遍历图像找到图像中橙色像素点的坐标,并存入空间中。遍历图像后，计算空间中所有橙色像 素点坐标的均值。这部分操作可以在detection_node.py文件中查阅到（这里给一个打开文件的过程录屏）
(该文件位于Home/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_1).
这段操作中不需要更改代码的内容,但希望同学们可以学习代码内容尝试理解编写的逻辑和其中的函数操作。 我们可以直接利用求得的坐标均值center进行下一步操作.
接下来，橙色点坐标均值与智能车的位置横坐标相减，得到位置偏移量bias。请将这一操作的代码输入motion_node.py 第46行，motion_node.py文件位于
主目录下的catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_1文件夹下
输入
Python
注意保持输入的本行语句和上下文之间的缩进相同
接下来，将bias与偏移量阈值进行比较，当bias大于阈值时，向智能车发出转向角速度指令同时让线速度减 小；当bias小于阈值时，智能车不发出转向角速度指令，只发出正常线速度指令。那么我们需要先设置一个 偏移阈值。
在motion_node.py文件的第24行输入self.center_band=50，这里我们将偏移阈值设置为50个单位的大小。
Python
1 self.center_band=50 #车道线偏移在center_band内不用修正
根据bias计算出无人车运动的线速度和角速度,向无人车发出指令。智能车线速度和角速度的计算，我们在第三节中为大家介绍了一种设计方案，你也可以发挥自己的智慧，改变函数的形式和参数大小。
在motion_node.py文件的第61 62行中，输入计算线速度和角速度的代码.
希望初学的同学们可以手动输入代码，加深理解。对编程熟练的同学也可以从教案中复制粘贴代码。
Python
1velocity.linear.x =0.10+ 0.20*(1.0- abs(bias)/(self.image_width/2))
2velocity.angular.z = 0.002*bias
使用键盘Tab键和空格键调整缩进，注意保持本行语句和上下文间的缩进关系，保持这样的代码缩进。
这个映射关系函数中包含了很多参数，我们来看下参数的意义。
- bias
是横坐标的差值
- velocity.linear.x
是智能车向前方x行驶的线速度
- velocity.angular.z
表示智能车转弯的角速度（逆时针为正）
- self.image_width
是摄像头采集图像的宽度
接下来，将线速度和角速度指令topic发送出去，底盘接收到线速度角速度方向指令后，执行动作，这样我们 就完成了一个自动计算行驶的过程。
在motion_node.py文件的第71行中输入发送线速度和角速度的指令。
Python
1self.velocityPub_.publish(velocity)
随后，智能车的程序中循环上述过程，就能够完成整个赛道的行驶，到达终点。
这样我们就完成了对代码的完善工作
键盘按下ctrl+s，或直接保存刚才我们改写的motion_node.py文件。
请再次打开一个终端，输入之前的roslaunch指令
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
并回车，静待几秒后，命令行中出现下面的提示，这时，在键盘输入任意键即可开始运行整个程序。
成功了，智能车是不是动起来了！
恭喜！到此为止，你已经成功启动并完成了一次无人车寻迹行驶任务。
ok，现在你已经完成了基础寻迹关卡的任务了！
是不是很简单。不过，小e觉得还可以跑得更快。请尝试让小e更快地完成赛道任务。
回顾一下给小e发出线速度和角速度指令的设计过程
linear.x=0.1+0.28×( 1— |bias| ⁄ imagewidth/2)
angular.z=0.0023×bias
这里，线速度的计算中存在两个常量数值，尝试改变这两个常量，观察无人车的行为。我们可以先将常量改 变得尺寸略大一些，以方便观察智能车的行为变化，例如，我们将线速度改为
linear.x=0.1+0.2×( 1— |bias| ⁄ imagewidth/2)
如何在代码中调试呢？
接下来我们尝试调试代码的过程，如果智能车正在运行，请先在运行检测前进节点的nicsrobot_line_follower.launch文件的终端中键入ctrl+c，终止正在运行的launch文件，然后再调试。
首先，我们要让智能车回到起点，
在终端中输入
Python
1rosrun akm_control set_model_state.py AKM_1 0.5 -1.7 0
这个指令表示运行了一个设置智能车位置的节点
智能车就可以瞬间移动回起跑线啦。
我们打开motion_node.py文件，修改线速度的代码参数：
Python
1velocity.linear.x =1.0+ 0.20*(1.0- abs(bias)/(self.image_width/2) )
这样就将线速度的最小值限定为了1.0，保存motion_node.py文件，再在终端输入
Python
1roslaunch nicsrobot_line_follower nicsrobot_line_follower.launch
再次启动launch文件，我们可以看到智能车前进的速度变快了。
同样的，每次调试程序前我们需要将智能车运行的launch文件程序中止，再发送指令让智能车回到起跑线。 然后修改角速度指令的权重参数，例如，我们将角速度代码参数改为：
angular.z=0.02×bias
Python
1self.center_band=100#车道线偏移在center_band内不用修正
需要注意的一点是，调整参数的过程中有可能会出现丢失车道线的情况。
请同学们打开rviz，调用智能车的摄像头image图像，查看以智能车第一视角观察到的图像。
如果智能车的视野中丢失了车道线，可以采用第三节中我们提供的解决方案，这个方案的代码在motion_node.py文件第57-59行中已经为同学们提供了，通过代码让智能车以0.15的线速度和0.8的角速度转圈 直到视野中检测到橙色像素点。
如果你的智能车出现了丢失车道线的问题，也可以尝试改变这部分代码的参数值，观察智能车不同的行为。
Python
同学们可以自行调整线速度和角速度的参数和常量，以及bias的偏移阈值，感受不同参数组合对智能车行为 的影响，调整参数到你认为最科学的组合，在最短时间内完成竞速赛吧！
帮助小工具：ubuntu目录以及文件操作方法
﹀帮助小工具：ubuntu目录以及文件操作方法
在使用ubuntu系统时，我们会需要对其中的文件进行操作。那如何找到在某个目录下的文件并进行操作呢？ 这是ubuntu的桌面界面，我们有两种进入目录管理文件的方法。
第一种是通过命令行，可以查阅1.4小节中关于命令行的"cd"操作，在命令行中进入某个文件夹。
第二种是通过图形化界面，像我们平日在windows系统中进行文件操作一样，ubuntu中的文件可以通过
进入到文件管理界面，默认的路径是主目录文件夹下
然后按照目录的路径，逐级双击打开文件夹就可以操作啦，和windows的操作几乎相同。
第五章 高级视觉智能—障碍赛实战
﹀第五章 高级视觉智能—障碍赛实战
5.1 从检测障碍物开始
欢迎来到课程的中级关卡，恭喜你已经完成了本课程的基础任务，相信到目前为止的内容对你来说完全是游 刃有余。那么在这一章节，我们将挑战更高的课程难度和逻辑能力，不过不用担心，我们仍会手把手带你了 解和学习完成障碍赛任务所需的所有资料。准备好了就开始吧！
完成中级任务所需的避障环境如下：
赛道还是原来的赛道，车子也是原来的车子，只不过，赛道上出现了一些红色的路障，路障的位置甚至挡到 了部分车道线，这种情况下，如何让车子既能完成车道线寻迹跑圈任务同时还能有效避开障碍物呢。解决这 个问题最重要的部分就是：检测障碍物，并规划避障路径。需要注意的是，为避免同学们冲出赛道，赛道两 侧的白色边线上设置有透明挡板。
5.2 避障路径规划
﹀5.2 避障路径规划
欢迎回到课程，这一小节我们开始一个简单的路径规划。
在前面基础关卡的寻迹任务中，我们给智能车发送的命令只是让其不要偏离车道线，一旦远离车道线超过阈 值，就发送指令让车子赶紧转向回来。在障碍赛阶段，这一策略在大部分时间内仍然是有效的，但是需要结 合实际的路况，做出更新的指令。
例如，在雷达探测范围内出现了路障时，应该如何设计智能车前进的指令呢？
5.3 接收雷达数据
﹀5.3 接收雷达数据
上一小节中我们为大家介绍了激光雷达，那么在程序中如何调用雷达的数据并处理呢？
这时，ROS系统的优势就体现出来了，我们不需要在硬件层面进行复杂的雷达驱动编写、程序烧录、调试等 工作，只需要在ROS系统中调用节点发送的雷达topic就可以了。而硬件层面的驱动等工程，在ROS系统中已 经预先封装过了。
这里，我们需要用到ROS系统的Subscriber函数，开启订阅雷达话题的功能。(英文subscribe意为订阅)
关于订阅话题接收数据的功能，在前面基础任务部分中，我们已经用subscribe函数订阅过摄像头的数据并进行了处理，可以参考detection_node_2.py文件的第24行。
(detection_node_2.py文件位于主目录/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_2)
这句话中参数的意义分别为:
- self.imageSub_ Subscriber函数的返回值
- rospy.Subscriber 调用ROS中名为Subscriber的函数
- /AKM_1/camera/rgb/image_raw
订阅名为'/AKM_1/camera/rgb/image_raw'的话题，话题名需要加单引号
- Image 话题的数据类型为Image
- self.imageCallback 订阅话题的同时，执行imageCallback函数
这句话表示，我们订阅了一个名为/AKM_1/camera/rgb/image_raw的话题，这个话题的数据类型为Image，订阅topic之后，执行imageCallback函数，Subscriber函数的返回值保存到self.imageSub_这个变量中。
（在编程中，大部分的函数都有一个“返回值”，例如：0或-1；用来表示处理完的数据或判断函数执行是否有误，所以我们可以定义一个变量来接收这个返回值）
目前我们知道它们的的调用用法就可以了。
请依样画葫芦，自己来写一句订阅激光雷达数据的语句。
这里，请同学们在detection_node_2.py文件的第26行自行用python编写一句代码，订阅名为/AKM_1/scan的话题，话题的数据类型为LaserScan,订阅话题后，执行lidarCallback函数，将Subscriber函数的返回值保存到self.lidarSub_这个变量中
完成后Subscriber函数的调用后，我们需要处理接收到的雷达数据，也就是lidarCallback函数。
我们这里的雷达返回的数据中包含了720个距离信息：它是将雷达水平360°逆时针方向上探测到的障碍物距离，以每0.5度为一个单位，保存的720个数据。
雷达数据的起始是从小车正后方开始，则我们关心的前方180°范围内障碍物信息位于表格的索引为(180~540)。
我们通过lidarCallback函数处理，将720个雷达数据保存为了一个720*1大小的列表，方便我们继续处理数据。（这里看能不能加一个从上图雷达的信息对应到表格数据的动画）
ok，雷达数据我们现在已经处理完了，最后一步操作：发送出去表格，让需要接收雷达数据的节点能够接收到我们处理的720维表格。
发送的方案非常简单，是ROS系统的基本用法，我们在程序里预先为同学们设定好了，不需要大家编写，但请理解并能迁移到其他的用法。
1. 第一步：定义一个Publisher
2. 第二步：定义Publisher函数发送的内容
3. 第三步：定义发送内容的格式和数据源，格式为Float32MultiArray（32位浮点数的数组）
好了，现在雷达返回的数据已经变成了一个尽在掌控的720*1维的表格，通过话题的收发操作，我们随时可以接收到它。下一小节中，我们就要利用雷达的数据列表和车道线信息，完成障碍赛路径规划的实践和操作了。
准备好了吗？！
5.4 开车调试——中级关卡
﹀5.4 开车调试——中级关卡
恭喜同学们来到课程的最后一步了——挑战障碍赛关卡！
这一章节的内容稍微晦涩了一些，不过还好，我们已经完成了一大部分。
现在我们的手中，有车道线偏移信息以及雷达返回的全方位障碍物位置信息，让我们做出路径规划吧！
这一章最终的任务，需要同学们自行创造一些代码，我们会给大家代码的目的和逻辑，请同学们用python编 写出来。
首先，回顾一下在路径规划那一小节思考的路径方案，课程教案里给同学们提供了两个方案做参考，但相信 你一定想到了更好的方案，那么如何在程序里执行我们的方案呢，以示例方案为例，我们将教你如何编写代 码实现。
我们以第二种方案为例：
在小车行驶的过程中，持续关注前方障碍物信息，永远选择障碍物最远的角度作为避障方向。但同时应引入 对障碍物距离的阈值判定，如果障碍物已经非常接近了，要加大转弯角度，紧急避障。
在此基础上，关注车道线的偏移量，最终转向弧度的计算方法为
angularnew = angular + Kp*dir
这个公式我们也在5.2小节为大家解释了意义。
接下来，我们需要把这部分想法写到motion_node_2.py文件中。
（文件位于主目录/catkin_ws/src/nics_gzb/nicsrobot_line_follower/scripts/level_2）
motion_node_2.py文件中大部分的基础设置代码我们也为同学们完成了，希望同学们能根据代码和教案的提 示，自行完成89行的lidar_avoid函数内容的编写。
我们为大家讲解一下代码编写的一种思路，希望同学们能理解后，自行创造代码。
lidar_avoid的功能为：接收了车道线偏移计算的angular弧度参数，操作雷达数据列表，进行判断和赋值计 算，计算出避障的弧度，最终根据权重公式，输出angular_new（智能车的转向弧度）。
首先，我们定义一些需要用到的参数包括：
K_p 为权重公式中障碍物转向值的权重
lidar_cut 为裁剪了雷达数据列表后得结果
裁剪雷达数据是为了删除小车身后的障碍物信息干扰，将表格索引和转向角度值相互对应。请同学们编写一 句代码，裁剪出self.lidar_data中前方障碍物的信息保存到lidar_cut中。这样，表格索引就和转向角度相互对 应起来了。
max_idx lidar_cut 列表中数值最大值的位置，也就是列表中的序号
max_idx参数找到列表中最大值的位置，也就是障碍物位置最远的角度信息，因为雷达列表中每0.5°为一个 单位，所以列表中的序号就表示方向。这里，请同学们编写代码，找出lidar_cut中，数据最大值所在的坐标 位置，并赋值给max_idx。
right_min_idx 从列表中起始位置到最大值位置间的索引中，找到最小值位置的索引序号
这句话比较绕口，让我们通过一张图来理解：
假设障碍物距离最远的方向为max_idx，由于车道线权重的存在，难以保证小车行驶中能完全躲开障碍物， 这时，我们设定一个检测，如果在水平方向上距离最近的障碍物索引right_min_idx的值小于一定阈值时，我 们就认为小车快要撞上了，必须转向。
这里，需要同学们编写一个判断语句。如果这个最近障碍物的距离小于阈值了，则需要额外加大转向角度；
如果最小障碍物的距离大于阈值，就保持最大障碍物方向的角度转向。
dir_idx 转向角度
当距离障碍物过近时，转向角应尽量远离障碍物；当距离障碍物较远，转向角朝向障碍物最远的方向。
dir 转向弧度（角度—弧度变换） 请将刚才计算出来的转向角度，转换为转向弧度
angular_new 最终的输出转向值
ok，我们已经将示例代码的逻辑呈现给同学们了，相信同学们一定有自己的想法并且跃跃欲试了。
请按照自己的逻辑和路径设计方案，任意修改并创造代码吧。完成编写后，需要保存代码才可以运行roslaunch，请注意python语言编写中的缩进关系。如果出现了报错提示，请尝试自己寻找解决方案。
你也可以定义自己需要的变量，请记得备份有效的代码文件，尝试更高效的避障跑圈方案吧。
比比看，谁可以在最短时间内完成避障寻迹任务！
ROS任务链挑战
﹀ROS任务链挑战
本任务链通过不同的任务使同学掌握ROS基础，包括工作环境、功能包的创建等，加深对ROS的理解。
任务一: 创建ROS工作空间
目标:本任务旨在讲解如何创建一个ROS工程。 打开终端并执行下列命令
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
其中
1. catkin为ROS系统代码的编译器。
2. mkdir。mkdir为创建文件夹使用方式mkdir <文件夹名>，默认只能创建一个文件夹。mkdir -p添加选项 -p可以直接创建多个层级文件夹。比如上述命令或直接创建工作空间catkin_ws以及该工作空间路径下的src两个文件夹。
该指令运行的路径下必须含有src文件夹。catkin_make指令执行时会在src文件夹中自动查找CMake-Lists.txt，并根据该文件内部命令编译文件，若src文件夹内没有CMakeLists.txt，```caktin_make会自动进行工作空间的初始化。
编译完成后catkin_make会在执行路径下自动生成devel，build两个文件夹。
build:编译文件夹，内部存储编译时生成的全部文件。
devel:development的缩写，代表软件的版本为内部开发版(与之相对的是安装版本install)开发时的全部文件与运行时的环境变量设置文件setup. <类型>完成项目开发后，需要将编译好的文件的所在目录添加到系统的执行路径当中以使系统可以运行开发好的ros功能。
任务二: ROS节点基础概念
﹀任务二: ROS节点基础概念
本任务旨在实现使用ROS来开发一个应用。
首先安装一个轻量级的仿真器。安装命令如下:
打开一个终端
$ sudo apt-get install ros-melodic-ros-tutorials
$ roscore
保持该终端打开，并开启一个新终端(可以开一个新tab或者新的终端窗口)
$ rosnode list
以上代码中roscore会启动一个ROS master，而rosnode list会显示当前运行的ROS系统中运行的全部节点名称。
ROS master负责注册各个节点并配对已经注册的节点，一个形象的比喻为ROS master就像一个婚介所，需要传递消息的节点是来求偶的人。首先节点需要在婚介所ROS master中登记，然后婚介所才能根据需求将各个节点进行配对。配对后的节点之间可以直接传递消息，不再需要婚介所，即在ROS master完成节点的匹配后可以直接关闭运行roscore的终端而不会影响节点之间的通信。不同在于:一个节点可以配对多个其他节点。
Rosnode是ROS系统中与节点有关的命令。ROS节点是能够执行某种功能的模块。
Rosnode list命令的在终端运行后会显示结果/rosout，这表明ROS system中只运行了一个名称为/rosout的节点。/rosout节点实现的功能为收集日志，在收集和记录节点的调试输出时运行。我们可以运行下面的指令来查看该节点的运行信息。
$ rosnode info /rosout
终端会显示下列信息
------------------------------------------------------------------------
Node [/rosout]
Publications:
* /rosout_agg [rosgraph_msgs/Log] Subscriptions:
* /rosout [unknown type]
Publications:
* /rosout/get_loggers
* /rosout/set_logger_level
contacting node http://machine_name:54614/...
Pid: 5092
其中Publications展示了节点发布出信息，Subscription展示了节点订阅的信息。Services展示了节点中提供的服务。
> 本例中/rosout发布的话题(topic)**名称为/rosout_agg，这个话题传输的消息(message，简写msg)**类型为rosgraph_msgs/Log，/rosout会将这个话题以广播的形式发送到ros master 中的全部节点，但只有与/rosout配对的节点才会收到对应的消息。
> 本例子中/rosout订阅了/rosout的消息，消息类型为未知。
>本例中/rosout提供了2个**服务(services，简写srv)**:/rosout/get_loggers 和/rosout/set_log-ger_level。提供的这两个服务可以被其他节点调用。
话题和服务是ROS系统中常用的通信方式(其他方式请自行学习)。
>话题:话题只用于节点之间传递数据。传递的数据叫做消息。方式为发送节点(publisher)在ros master中注册话题发布者信息并以一个设定的频率广播话题，话题中发送的数据类型为消息(message);接收节点(subscriber)在 ros master 中注册话题订阅者信息并也以一个设定的频率监听话题，每当监听到指定的话题时，调用函数处理话题传递的消息。这种信息传递方式称为**发布/订阅(publisher/subscriber)**。
>服务:服务相当于节点之间传递应用程序接口(Application Programming Interface, API)。方式为服务器节点(server)在节点内设计一个服务函数，并将该服务函数注册到ros master 中;客户节点(client)将服务的请求提交到ros master中，服务函数的调用传递的数据类型为服务(service)。这种信息传递方式称为**服务器/客户(server/client)**。
至此，我们区分两个概念:
>1. 话题topic 和 服务 service是通信类型。
>2. 消息(.msg文件) 和服务(.srv文件) 是数据类型，是第一条中两个通信中传递数据的类型。两者的区分为:消息没有返回值而服务有返回值
下面是一个示例消息文件person.msg
stringname
uint8sex
uint8age
uint8unknown= 0
uint8male= 1
uint8female= 2
下面是一个示例服务文件Person.srv
stringname
uint8sex
uint8age
uint8unknown= 0
uint8male= 1
uint8female= 2
---
string result
由该例可见，.srv就是比.msg多了返回值类型。在.srv的定义文件中，返回值和请求值通过三个横线分割。
最后，所有信息的传递都基于TCP/IP协议，这就意味着运行在电脑A上的节点node A可以调用另一台电脑上的node B中提供的服务API，也可以向其他多台电脑的多个节点发送话题。采用这种方案可以轻松实现远程控制机器人。
任务三: ROS节点的运行与ROS功能包
﹀任务三: ROS节点的运行与ROS功能包
实际生活中操作一个指定机器人需要很多功能，比如轮子驱动，摄像头信息处理等等，为了方便我们通常会将这种操作同一个实体的功能打包。ROS中也提供这个功能，即将多个功能相关的节点打包成一个功能包package。
$ rosrun [package_name] [node_name]
比如运行 turtlesim包中的 turtlesim_node 。
#1 终端1
$ roscore
#2 终端2
$ rosrun turtlesim turtlesim_node
#3 终端3
$ rosnode list
终端2会启动额外的窗口
此时终端3中会返回
/rosout
/turtlesim
表示当前ROS系统中运行了两个不同的节点，名称为/rosout & /turtlesim。这两个节点的名称是在脚本内节点初始化时指定的。
此外，节点的名称也可以在启动时通过终端手动修改，比如
#1 终端1
$ roscore
#2 终端2
$ rosrun turtlesim turtlesim_node __name:=my_turtle
#3 终端3
$ rosnode list
此时终端3中的输出为
/my_turtle
/rosout
其中节点名称被修改为了/my_turtle
我们也可以使用下面的指令来测试某个节点是否启动:
$ rosnode ping my_turtle
如果节点/my_turtle启动后，会返回下面类似的消息
rosnode: node is [/my_turtle]
pinging /my_turtle with a timeout of 3.0s
xmlrpc reply from http://<host_name>:<port_num>/ time=1.152992ms
xmlrpc reply from http://<host_name>:<port_num>/ time=1.120090ms
xmlrpc reply from http://<host_name>:<port_num>/ time=1.700878ms
xmlrpc reply from http://<host_name>:<port_num>/ time=1.127958ms
任务四: 理解ROS话题Topic
﹀任务四: 理解ROS话题Topic
运行下列程序
# terminal 1
$ roscore
# terminal 2
$ rosrun turtlesim turtlesim_node
# terminal 3
$ rosrun turtlesim turtle_teleop_key
终端3会显示下列信息:
Reading from keyboard
---------------------------
Use arrow keys to move the turtle. 'q' to quit.
在终端3中使用键盘的方向键操作乌龟移动。
新启动的turtle_teleop_key便是提供"键盘"操作乌龟功能的节点，该节点与turtlesim_node就是通过话题topic进行通信。具体来讲，turtle_teleop_key通过话题发布(publish)按键，而turtlesim订阅对应的话题并接收按键，两者之间的通信流程可以通过rqt_graph进行可视化。其安装方式如下:
$ sudo apt-get install ros-melodic-rqt
$ sudo apt-get install ros-melodic-rqt-common-plugins
# 打开终端
rqt_graph
请注意左上角可以通过Hide选项屏蔽一些我们不关系的topic和节点 将鼠标移动到/teleop_turtle上，则rqt_-graph便会被高亮成
红色:话题的发送者(publisher)
绿色:话题名称(topic)
ROS系统中操作话题(topic)的相关指令包为rostopic。通过在终端中输入
rostopic -h
我们可以获得rostopic的各种用法。如下:
rostopic bwdisplay bandwidth used by topic
rostopic echoprint messages to screen
rostopic hzdisplay publishing rate of topic
rostopic listprint information about active topics
rostopic pubpublish data to topic
rostopic typeprint topic type
此外也可以通过输入rostopic<空格>后双击Tab键查看候选子命令。
$ rostopic
bw echo find hz info list pub type
查看rostopic传输的数据是ROS系统中debug最常用的一种方式:
rostopic echo <topic_name>
此指令用于回放(echo)中的数据。 另外，当输出的信息较多时，也可以通过下方指令将输出的信息手动输出到一个日志文件中。
rostopic echo <topic_name> > /tmp//log
这个指令将rostopic echo 的输出信息存入文件/tmp/log
在这个示例程序中，为了探究turtle是怎么动起来的，最直观的做法就是查看turtle_teleop_key究竟传给乌龟什么信息。 故我们需要查看两者传递的话题内容。(话题名称就是rqt_graph红色 高亮的内容) 通过
$ rostopic echo /turtle1/cmd_vel
linear:
x:2.0
y:0.0
z:0.0
angular:
x:0.0
y:0.0
z:0.0
---
linear:
x:0.0
y:0.0
z:0.0
angular:
x:0.0
y:0.0
z:2.0
---
启动rostopic echo之后，重新启动rqt_graph或者点击刷新按钮，其中多了rostopic_14587_xxxx，该节点便是rostopic echo启动的节点。
或者
此外可以通过rostopic list -h可以查看rostopic list的所有子命令的功能如下:
Usage: rostopic list [/topic]
Options:
-h, --helpshow this help message and exit
-b BAGFILE, --bag=BAGFILE
-v, --verbose list topics in .bag file
-p list full details about each topic list only publishers
-s list only subscribers
$ rostopic list -v
Published topics:
* /turtle1/color_sensor [turtlesim/Color] 1 publisher
* /turtle1/cmd_vel [geometry_msgs/Twist] 1 publisher
* /rosout [rosgraph_msgs/Log] 4 publishers
* /rosout_agg [rosgraph_msgs/Log] 1 publisher
* /turtle1/pose [turtlesim/Pose] 1 publisher
Subscribed topics:
* /turtle1/cmd_vel [geometry_msgs/Twist] 2 subscribers
* /rosout [rosgraph_msgs/Log] 1 subscriber
* /statistics [rosgraph_msgs/TopicStatistics] 1 subscriber
该命令可以显示出当前ROS系统中运行的全部topic及其相关发布者和订阅者的数量。
最后，debug还需要知道topic传递的数据类型以及名称。topic传递数据类型的名称可以通过
rostopic type <topic_name>
查看，比如
$ rostopic type /turtle1/cmd_vel
geometry_msgs/Twist
而该消息定义的数据类型通过下面命令查看
$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
由于geometry_msgs数据类型是ros内建的消息类型，可在ros官网上找到每一个变量的含义。如官网所说 ，geometry_msgs/Twist定义了速度的平移和旋转分解。Tips:ROS开源社区提供的搜索并不好用，推荐的方式为bing.com的国际版中搜索 ros <msg名称>，通常第一个候选就是官网链接。
其实，大多数debug时我们只需要查看每个topic内部传输的消息msg的数据类型。按照上面的方式，查看消息类型需要执行两个指令，略显麻烦，下面可以用一个指令直接显示话题topic内部传输消息msg的数据类型。
$ rostopic type /turtle1/cmd_vel | rosmsg show
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
至此，我们便可以解释乌龟移动的原因。turtle_teleop_key节点通过/turtle1/cmd_vel话题(topic)发送ge-ometry_msgs/Twist类型的消息(msg)，消息中记录了乌龟车当前速度的分解，从而接收节点按照速度要求移动乌龟车。
任务五: 手动发布ROS话题Topic
﹀任务五: 手动发布ROS话题Topic
启动乌龟仿真节点。
# terminal 1
$ roscore
# terminal 2
$ rosrun turtlesim turtlesim_node
# terminal 3
$ rosrun turtlesim turtle_teleop_key
通过rostopic pub命令，我们可以在rostopic中手动发布一次topic。
rostopic pub <topic_name> <msg_type> <args>
其中pub后的-1代表只发送此话题一次，为话题名称，为传递的消息类型，为消息内部变量的赋值。其中的输入格式可以通过双击tab键自动补全，然后修改对应的数值来实现。 比如
rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
注意：该命令无法直接复制运行。当命令行中输入话题名称后（geometry_msgs/Twist），输入空格+双击Tab键，后面的内容会自动补全成空值，我们按键盘左右键移动到对应位置修改变量数值即可。
自动补全的内容是按照yaml命令行格式组织，详情请查阅官方文档http://wiki.ros.org/ROS/YAMLCommandLine 至此，我们尝试一下手动发送topic使turtle移动
# terminal 1
$ roscore
# terminal 2
$ rosrun turtlesim turtlesim_node
$ rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 2.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
publishing and latching message for 3.0 seconds
注意：该命令无法直接复制运行，可以复制到Twist处，随后终端中双击Tab补全后续内容，并修改响应数据。
你会看到乌龟车向其左手边移动。
此外，我们也可以以设定的频率发布乌龟车移动的话题（使用pub -r )子选项
$ rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 2.0
z: 0.0
angular:
x: 0.0
y: 2.0
z: 0.0
此指令以1Hz的频率发布乌龟的移动话题。此数据可以使乌龟逆时针转圈。原理如下：
linear中y等于2，制定了乌龟当前线速度有y轴正方向分量（而机器人坐标系下，前方为x正向，左手边为y轴正向，上方为z轴正向）
angular中z=2，代表乌龟车的角速度存在绕z轴正向旋转的分量，绕轴旋转正方向按照右手螺旋定则确定（右手拇指指向z轴正方向，四个手指指向的旋转方向就是绕z轴旋转的正向）。
注意：turtle运动只在二维平面中，因此没有沿z轴方向的线速度&绕x轴的角速度&绕y轴的角速度。
此时再刷新rqt_graph我们可以看到下面的图片：
或者
另外，我们还可以回放乌龟的位姿信息。
$ rostopic echo /turtle1/pose
x: 5.34955310822
y: 4.52340221405
theta: -1.26353096962
linear_velocity: 2.0
angular_velocity: 2.0
---
x: 5.3797287941
y: 4.53405189514
theta: -1.23153102398
linear_velocity: 2.0
angular_velocity: 2.0
---
x: 5.40954875946
y: 4.5456609726
theta: -1.19953095913
linear_velocity: 2.0
angular_velocity: 2.0
---
乌龟在各个位置的坐标、角度，以及线速度&角速度数值都在此显示。
另外，我们可以查询某个话题topic发布的频率
$ rostopic hz /turtle1/pose
subscribed to [/turtle1/pose]
average rate: 62.471
min: 0.015s max: 0.017s std dev: 0.00050s window: 60
average rate: 62.479
min: 0.015s max: 0.017s std dev: 0.00051s window: 122
average rate: 62.484
min: 0.015s max: 0.017s std dev: 0.00050s window: 185
由此可得发布的频率大致为62Hz。
上述显示内容均为文字，并不直观。ROS系统中还提供图形可视化工具rqt_plot，其使用方式如下：
rosrun rqt_plot rqt_plot
若rqt_plot显示的窗口很小而且拖拽时报错 ValueError: bottom cannot be >= top ，则是由于rqt_plot调用的matplotlib版本过低，运行下面代码升级python中的matplotlib。
$ sudo apt-get install python-pip
$ pip install --upgrade matplotlib
在上方的输入文本框中输入需要监听的话题topic名称。
任务六: 使用rqt_console & ...
﹀任务六: 使用rqt_console & ...
rqt_console就像是一个rqt的IDE一样，里面集成了rqt的各种功能模块的显示。 运行方式如下：
rosrun rqt_console rqt_console
启动的rqt_console如下图所示：
比如我们运行
rosrun turtlesim turtlesim_node
并且启动新终端输入下方指令使乌龟朝着一个固定的方向移动。
rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '{linear: {x: 2.0, y: 0.0, z: 0.0}
angular: {x: 0.0,y: 0.0,z: 0.0}}'
最终乌龟将撞墙，系统报warn信息如下：
警告信息的等级分为以下几个级别：
•Fatal
•Error
•Warn
•Info
•Debug
其中Fatal是最重要的等级，而debug是最低的等级。通过选择第二栏中的不同等级按钮，第一个显示框内可以只显示对应等级的消息。
任务七: ros_launch基础
﹀任务七: ros_launch基础
在之前的任务中，我们通过如下方式启动乌龟仿真节点。
# terminal 1
$ roscore
# terminal 2
$ rosrun turtlesim turtlesim_node
# terminal 3
$ rosrun turtlesim turtle_teleop_key
该方式启动很繁琐，而且需要打开三个独立的终端或者tab。roslaunch就是将上述节点启动方式简化为单个命令。
$ cd ~/catkin_ws
$ catkin_make
$ cd ./src
$ catkin_create_pkg turtle_launch rospy roscpp std_msgs
$ cd turtle_launch
$ mkdir launch && cd launch
$ gedit start_turtle.launch
将下列代码放入start_turtle.launch
</launch>
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" />
<node pkg="turtlesim" type="turtle_teleop_key" name="cmd_vel" />
</launch>
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
通过上述代码我们便创建了一个名为turtle_launch的功能包，此功能包的中包含一个start_turtle.launch启动文件，此文件中启动turtlesim_node 和 键盘控制程序 两个节点。 因此通过运行
# 终端1
$ roslaunch turtle_launch start_turtle.launch
我们便可以在终端1中控制乌龟的运动。 注意：在launch文件执行前并没有启动roscore，这是因为roslaunch会自动为我们启动roscore。
在上述代码中
1. roslaunch启动文件的命令格式为：
$ roslaunch <package> <filename.launch>
为功能包名称 为启动文件名称
2. source ~/catkin_ws/devel/setup.bash命令将创建的功能包路径写入搜索路径使其可发现。
3. launch文件内部采用xml格式，请自行搜索xml文件的格式说明已获得更深入的理解。 在这个launch文件中
4. 和 两个标签内部的文件即launch文件的有效的代码。
5. 由<node 起始到 />结束包含的内容为节点的有效代码。
6. 节点中pkg代表功能包的名字， type代表运行c++程序对应的可执行文件的名称， name为节点的名称。
我们再尝试一个roslaunch的程序如下：
$ gedit ~ ~/catkin_ws/src/turtle_launch/launch/mimic_turtle.launch
并在文件中撰写如下代码
<launch>
<group ns="turtlesim1" >
<node pkg="turtlesim" type="turtlesim_node" name="sim" />
</group>
<group ns="turtlesim2" >
<node pkg="turtlesim" type="turtlesim_node" name="sim" />
</group>
<node pkg="turtlesim" name="mimic" type="turtlesim_node" />
<remap from="input" to="turtlesim1/turtle1" />
<remap from="output" to="turtlesim2/turtle1" />
</node>
</launch>
运行这个写好的roslaunch程序
$ roslaunch turtle_launch mimic_turtle.launch
ROS会弹出两个乌龟仿真器窗口，我们控制turtle1转圈，然后观察turtle2的行为是否与turtle1相同。
$ rostopic pub -r 1 /turtlesim1/turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 2.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 2.0
注意：该命令无法直接复制运行，可以复制到Twist处，随后终端中双击Tab补全后续内容，并修改响应数据。
相应的rqt_graph如下，即turtlesim2是由mimic节点控制。
或者
任务八: 使用python撰写一个...
﹀任务八: 使用python撰写一个...
从之前的任务中，我们学习了两种通信方式话题topic和服务service的调用方式，以及两种通信方式下传输的数据类型.msg和.srv。但是我们目前还未涉及如何自定义ROS话题的发布者(publisher)/订阅者(subscriber)。以及服务service的服务器(server)/客户(client)。
接下来，我们首先基于python撰写一个话题的发布-订阅。在ROS系统中，撰写的python程序需要统一放置在功能包路径下的scripts文件夹中。
$ cd ~/catkin_ws/src
$ catkin_create_pkg python_topic rospy roscpp std_msgs
$ cd python_topic
$ mkdir scripts && cd scripts
$ gedit talker.py
将下面内容写入talker.py
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
def talker():
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('chatter', String, queue_size=10)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
直接复制的内容可能没有缩进，若复制过来的内容没有缩进，需要手动添加缩进（每一个缩进为4个空格）。
代码解读：
1.#!/usr/bin/env python 每一个采用python撰写的ros node在顶部都必须有这个声明。这行代码能够保证代码会以python形式运行。
2.所有python编写的ros node必须import rospy，因为相关的ros库都在这个包中。
3.由于python脚本后续内容使用了ROS内建的String类型，因此同样需要from std_msgs.msg import String
4.pub = rospy.Publisher('chatter', String, queue_size=10) 。该语句定义了一个发布名为/chatter话题的发布者(pub)，该话题传输的数据类型为String（ROS系统内建的类型std_msgs.msg.String），传输数据队列长度最大为10，即当系统中已经发布了10个未被订阅接受的消息时，再发布的消息会挤出最原始的消息。
5.rospy.init_node('talker', anonymous=True) 。该语句定义了这个python文件定义的节点名称为'talker?'后面的？是由 anonymous=True 指定而随机生成的数字，该数字的作用
为防止该节点与其他节点重名。 注意：此处talker前不可以加slash "/"。
6.rate = rospy.Rate(10) # 10hz 。该语句创建了一个名为rate的Rate对象。通过循环中加入rate.sleep()函数，该循环可以实现每秒10次，即10Hz（只要每次循环处理时间不超过1/10s）。
7.下方的循环是ros编程的模板，包括以下几步：
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
rate.sleep()
8.检查 rospy.is_shutdown() 标志位然后开始进行工作。此标志位用于判断程序是否需要退出（比如用户是否在终端输入了ctrl-c）。若程序并未退出，调用pub将hello_str作为消息内容以名为/chatter的话题发布出去。执行完成发布任务后调用rate.sleep()来增长一次循环的时间以满足每秒执行10次的要求。其中 rospy.loginfo(hello_str) 具有三个功能：终端中显示hello_str字符串；将hello_str写入日志文件；将hello_str写到rosout。
注意：每个python脚本在创建后需要赋予其可执行权限
chmod +x ./talker.py
至此，我们完成了发布者的创建。 订阅者的创建方式如下
$ cd ~/catkin_ws/src/python_topic/scripts
$ gedit listener.py
将下面内容写入listener.py，同样需要保持缩进。
#!/usr/bin/env python
import rospy
from std_msgs.msg importString
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("chatter", String, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()
注意：每个python脚本在创建后需要赋予其可执行权限
chmod +x ./listener.py
代码解读：
1. 订阅者的绝大部分代码与发布者类型。核心代码 rospy.Subscriber("chatter", String, callback) 与发 布者的区别在于，订阅者多了一个callback回调函数。这句代码声明该节点接受名为/chatter的话题 （传输数据变量类型为std_msgs.msg.String)，并将接收到的String数据传入回调函数callback。
2.rospy.spin()用于防止节点执行过subscriber定义后退出，相反在该函数的作用下，只要节点不退出， subscriber会一直监听。 至此，我们设计好了发布者与订阅者，改变当前路径到工作空间根目录编译生 成可执行文件，命令如下：
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
# 终端1
$ roscore
# 终端2
$ rosrun python_topic talker.py
# 终端3
$ rosrun python_topic listener.py
任务九: Python 控制乌龟车
﹀任务九: Python 控制乌龟车
至此，大家应该初步了解了ROS topic的机制。 现在，请大家做一个作业，练一练用 python 实现话题的发布。
编写节点 py_control.py 和 py_control.launch。
首先新建工作包 py_control
$ cd ~/catkin_ws/src
$ catkin_create_pkg py_control rospy roscpp std_msgs geometry_msgs
#因为要用到twist命令，所以需要导入 geometry_msgs
$ cd py_control
$ mkdir script launch
$ touch script/py_control.py launch/py_control.launch
#新建 script/py_control.py launch/py_control.launch 文件
$ chmod +x script/py_control.py
#添加可执行权限
编辑python文件： script/py_control.py:
#!/usr/bin/env python
# coding=utf-8
import rospy
from geometry_msgs.msg import Twist
def controller():
rospy.init_node('pycontroller', anonymous=True)
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) #set topic name
rate = rospy.Rate(0.3) # 0.3hz
count = 0
while not rospy.is_shutdown():
if count % 4 == 0:
control_cmd = Twist()
control_cmd.linear.x = 2 #使小车向前开
if count % 4 == 1:
control_cmd = Twist() #请修改Twist内容，使其左转
if count % 4 == 2:
control_cmd = Twist() #请修改Twist内容，使其继续向前
if count % 4 == 3:
control_cmd = Twist() #请修改Twist内容，使其右转
count += 4
rospy.loginfo(control_cmd)
pub.publish(control_cmd)
rate.sleep()
if __name__ == '__main__':
try
controller()
except rospy.ROSInterruptException:
pass
其中 py_control.launch 文件编写如下。请参考注释修改功能
<launch>
<!-- 先启动乌龟车项目 -->
<node pkg="turtlesim" type="image_sub_node.py" name="turtle" />
<!-- 再启动自己写的python节点 -->
<!-- <node pkg="turtlesim" type="py_control.py" name="pycontrol"/> -->
</launch>
等文件编写完成后， catkin_make 并且 source devel/setup.sh 任务空间。
然后
roslaunch py_control
py_control.launch
如果看到乌龟车按照你的计划正常运行，说明成功。请仔细理解你写的python文件中每一行代码（包括课组提供的代码框架）。如果你都理解了，就可以放学了。
第六章 图像处理
﹀第六章 图像处理
6.1 图像处理理论
数字图像是一组格式化的数据，存储这对应不同位置上的视觉信息（亮度，不同频率分量）。根据存储的信息不同，可以划分成不同类型的图像，如存储RGB不同通道信息的彩色图像，存储亮度信息的灰度图像，存储色调、饱和度、亮度信息的HSV图像等。
图像处理是从现有图像提前出有效信息，或者去除无关信息。图像滤波是一个常见且有效的手段。如高斯滤波的平滑效果与其卷积核尺寸有很大关系：
物体检测是指在给定的图片上给出要寻找的物体所在的位置以及类别：
使用OpenCV提供的通用计算机视觉方法库进行模版匹配：
使用CNN检测目标位置：
YOLO将检测变为一个回归问题，从输入的图像仅经过一个神经网络得到bounding box及其所属类别的概率：
6.2 图像处理实验
﹀6.2 图像处理实验
任务一: 基于国庆作业实现对待检测图片的传输
在本次实验中，我们首先需要通过ROS来完成对需要进行待检测图片的传输。这部分实验顺序是基于国庆作业中的任务一，可以以该任务为基础完成本次实验。
创建相关package
cd ~/catkin_ws/src
catkin_create_pkg image_tran rospy roscpp std_msgs image_transport cv_bridge
cd image_tran
mkdir -p scripts && cd scripts
image_pub
gedit image_pub_node.py
chmod +x image_pub_node.py
代码结构如下:
#!/usr/bin/env python
#coding:utf-8
import rospy
import sys
import cv2
import os
import numpy as np
from sensor_msgs.msg import Image
import random
import cv_bridge import CvBridge, CvBridgeError
import pubImage():
rospy.init_node('pubImage', anonymous =True )
pub = rospy.Publisher('ShowImage', Image, queue_size =10 )
rate = rospy.Rate(10)
rospy.init_node 'pubImage', anonymous =True )
rospy.init_node 'pubImage', anonymous =True )
bridge = CvBridge()
path = "/path/to/your/ball_env.jpeg"
image = cv2.imread(path)
h, w, _ = image.shape
while not rospy.is_shutdown():
# 生成切分后的图片
start_h = int(random.random() * h /4)
height = int(h *3/4)
start_w = int(random.random() * w /4)
width = int(w *3/4)
transfer_image = image[start_h:(start_h+height),start_w:(start_w+width),:]
# 传输切分后的图片
pub.publish(bridge.cv2_to_imgmsg(transfer_image,"bgr8""))
rate.sleep()
if __name__ == '__main__':
try
pubImage()
except rospy.ROSInterruptException:
pass
其中需要注意的是需要将path改为在同学们本地电脑下图片的绝对路径
path = "/path/to/your/picture"
在以下代码中，random.random()生成[0,1)之间内的一个随机数，通过对原始图片的切分，可以生成从一个随机位置起始，高和宽均为原始图片3/4的图片
# 生成切分后的图片
start_h = int(random.random() * h /4)
height = int(h *3/4)
start_w = int(random.random() * w /4)
width = int(w *3/4)
transfer_image = image[start_h:(start_h+height),start_w:(start_w+width),:]
image_pub
编辑并更新image_sub_node.py的可执行属性
gedit image_pub_node.py
chmod +x image_pub_node.py
代码结构和国庆作业相同:
#!/usr/bin/env python
#coding:utf-8
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge,CvBridgeError
def callback(data):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data,"bgr8")
cv2.imshow("view", cv_image)
def showImage():
rospy.init_node('showImage', anonymous = True)
cv2.namedWindow("view", cv2.WINDOW_AUTOSIZE)
cv2.startWindowThread()
rospy.Subscriber('ShowImage', Image, callback)
rospy.spin()
cv2.destroyWindow("view")
if __name__ == '__main__'
showImage()
launch
编辑并更新image_sub_node.py的可执行属性
cd ~/catkin_ws/src/image_tran
mkdir -p launch && cd launch
gedit start.launch
并将如下代码放入start.launch文件中
<launch>
<node pkg="image_tran" type="image_sub_node.py" name="sub_node" output="screen"/>
<node pkg="image_tran" type="image_pub_node.py" name="pub_node" output="screen"/>
</launch>
运行
完成上述代码后，可以通过以下脚本来执行
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
roslaunch image_tran start.launch
可以看到图片中的小球位置在抖动，这也是在现实场景中常见的情况，因此需要对目标进行定位。
同样可以使用RVIZ对传输的图片进行监控
rosrun rviz rviz
界面如下:
任务二: 基于OpenCV完成对图片的检测
在本次实验中，我们的目标是从以下图片中找到红色的小球
在OpenCV中已经提供给了我们现有的工具来实现对目标的检测，我们可以将image_sub_node.py中的代码替换成以下代码来实现对目标的检测
#!/usr/bin/env python
#coding:utf-8
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge,CvBridgeError
import numpy as np
def callback(data):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
# transfer gray and load template
cv_image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
template_path = "/path/to/your/ball.jpeg"
template_image = cv2.imread(template_path, cv2.IMREAD_GRAYSCALE)
res = cv2.matchTemplate(cv_image_gray, template_image, cv2.TM_SQDIFF)
# find max index, and plot
index_position = np.where(res == np.min(res))
index_h, index_w = index_position[0][0], index_position[1][0]
template_h, template_w = template_image.shape
cv2.rectangle(cv_image, (index_w, index_h), (index_w + template_w, index_h + template_h), [0,0,255], 2)
cv2.imshow("view", cv_image)
defshowImage():
rospy.init_node('showImage', anonymous = True)
cv2.namedWindow("view", cv2.WINDOW_AUTOSIZE)
cv2.startWindowThread()
rospy.Subscriber('ShowImage', Image, callback)
rospy.spin()
cv2.destroyWindow("view")
if__name__ === '__main__':
showImage()
然后再次执行以下命令即可看到
roslaunch image_tran start.launch
得到的结果如下图所示，可见检测的效果是可以的
相关部分代码解读: 1. transfer gray and load template，由于OpenCV模板匹配的限制，必须在灰度图上，所以首先需要将输入图片转换到灰度空间。然后将对应的路径进行修改，改为在本地电脑下的ball.jpeg，然后加载为灰度图。最后利用cv2.matchTemplate进行模板匹配，由于模板相较于原始图片会小，所以会在原始图片上进行划窗，然后计算划窗后的图片和模板图片的相似程度，TM_SQDIFF代表方差作为相似度，最终生成一个res矩阵，对应着每个坐标下划窗和模板的得分。
# transfer gray and load template
cv_image_gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
template_path = "/path/to/your/ball.jpeg"
template_image = cv2.imread(template_path, cv2.IMREAD_GRAYSCALE)
res = cv2.matchTemplate(cv_image_gray, template_image, cv2.TM_SQDIFF)
2. find max index and plot，通过np.where查找最小值所在的位置(方差越小代表模板和划窗越接近)，然后结合着模板的高度和宽度得到最终所要绘制框的左上角坐标和右下角坐标，最后通过cv2.rectangle将框画在原始图片上，[0,0,255]代表着框的颜色BGR，2代表着框线条宽度。
# find max index, and plot
index_position = np.where(res == np.min(res))
index_h, index_w = index_position[0][0], index_position[1][0]
template_h, template_w = template_image.shape
cv2.rectangle(cv_image, (index_w, index_h), (index_w + template_w, index_h + template_h), [0,0,255], 2)
任务三: 基于CNN完成对图片的检测
安装相关环境
需要首先安装pip
sudo apt install python-pip
接着安装pytorch相关，在执行gedit命令时，将第二行opencv-python删除
pip install torch==1.3.0+cpu torchvision==0.4.1+cpu -f https://download.pytorch.org/whl/torch_stable.html
cd ~/catkin_ws/src/image_tran/scripts
git clone https://github.com/zoeyuchao/yolov3_detect.git && cd yolov3_detect
gedit requirements.txt
pip install -U -r requirements.txt
wget https://cloud.tsinghua.edu.cn/f/f6748453cced47608edb/?dl=1 -O weights/best.pt
mv data/samples/three_balls_2.jpg data/samples/three_balls.jpg
需要首先安装pip
接着执行检测脚本即可实现对目标的检测
程序输出的结果应该为
同时在当前目录下会生成对图片的预测结果为
对输出结果的分析最终的输出结果是一个数量不固定(每一个目标对应一个输出)，但是每个元素长度固定为7的数组，其中前四位分别代表着位置,x,y,w,h，接着两位代表得分，最后一位代 表着类别。
完成对image_pub和image_sub的改善来实现CNN目标检测
image_pub
由于检测速度较慢，所以需要将图片发送帧率改为1甚至更低，同时需要更改图片，使得训练的模型能够正确生效，将path替换为本地电脑上对应的three_balls.jpg即可
#!/usr/bin/env python
#coding:utf-8
import rospy
import sys
import cv2
import os
import numpy as np
from sensor_msgs.msg import Image
import random
from cv_bridge import CvBridge, CvBridgeError
def pubImage():
rospy.init_node('pubImage', anonymous = True)
pub = rospy.Publisher('ShowImage', Image, queue_size = 10)
rate = rospy.Rate(1)
bridge = CvBridge()
path = "/path/to/your/three_balls.jpg"
image = cv2.imread(path)
h, w, _ = image.shape
while not rospy.is_shutdown():
# 生成切分后的图片
start_h = int(random.random() * h /4)
height = int(h *3/4)
start_w = int(random.random() * w /4)
width = int(w *3/4)
transfer_image = image[start_h:(start_h+height),start_w:(start_w+width),:]
# 传输切分后的图片
pub.publish(bridge.cv2_to_imgmsg(transfer_image, "bgr8"))
rate.sleep()
if __name__ == '__main__':
try
pubImage()
except rospy.ROSInterruptException:
pass
image_pub
在接收端，我们需要将目标检测实现在接收端，我们可以将代码进行更新如下
#!/usr/bin/env python
#coding:utf-8
import os
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import commands
def callback(data):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
# save image, detect, read result
abs_path = "/path/to/your/yolov3_detect"
cv2.imwrite("%s/data/samples/three_balls.jpg" % abs_path, cv_image)
os.system('cd %s && python detect.py' % abs_path)
res_image = cv2.imread("%s/ball_detect.jpg" % abs_path)
cv2.imshow("view", res_image)
def showImage(data):
rospy.init_node('showImage', anonymous = True)
cv2.namedWindow("view", cv2.WINDOW_AUTOSIZE)
cv2.startWindowThread()
rospy.Subscriber('ShowImage', Image, callback)
rospy.spin()
cv2.destroyWindow("view")
if __name__ == '__main__':
showImage()
代码说明，首先需要将abs_path替换为之前下载的yolov3_test的地址，然后将接收到的图片写入到一个固定的路径，然后利用os.system执行detect.py命令，再然后就可以将输出的结果取出 作为最终检测的结果
abs_path = "/path/to/your/yolov3_detect"
cv2.imwrite("%s/data/samples/three_balls.jpg" % abs_path, cv_image)
os.system('cd %s && python detect.py' % abs_path)
res_image = cv2.imread("%s/ball_detect.jpg" % abs_path)
运行
接着执行检测脚本即可实现对目标的检测
得到的结果如下图所示
任务四: 采用霍夫圆检测代替模板匹配
可以采用霍夫圆检测代替YOLO，相关的代码如下:
gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGRA2GRAY)
circles = cv2.HoughCircles(gray_image, cv2.HOUGH_GRADIENT, 1, 50, param1=100,
param2=50, minRadius=0, maxRadius=0)
circles = np.uint16(np.around(circles))
for i in circles[0, :]: # 遍历矩阵每一行的数据
cv2.circle(cv_image, (i[0], i[1]), i[2], (0, 255, 0), 2)
cv2.circle(cv_image, (i[0], i[1]), 2, (0,0, 255), 3)
同时我们还可以将时间纳入到检测算法的考察中去，添加如下代码到image_sub_node.py中
import time
def callback(data):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
start_time = time.clock()
# save image, detect, read result
abs_path = "/home/yourname/catkin_ws/src/image_tran/scripts/yolov3_detect"
cv2.imwrite("%s/data/samples/three_balls.jpg" % abs_path, cv_image)
os.system('cd %s && python detect.py' % abs_path)
res_img = cv2.imread("%s/ball_detect.jpg" % abs_path)
end_time = time.clock()
print("time cost is %f s" % (end_time - start_time))
cv2.imshow("view", res_img)
第七章 在gazebo中开动无人机
﹀第七章 在gazebo中开动无人机
7.1 认识仿真界面
配置好仿真环境后，可以运行如下命令，先认识仿真界面：
Plain Text
1roslaunch uav_sim arena.launch visualization:=true
该命令会启动Rviz和Gazebo两个窗口。在本系列课程的2.2章节有介绍。
Rviz为ROS自带的图形化工具，下图中 A区域为订阅的一些topic信息，B区域为image类型的消息，下图中是对无人机摄像头拍摄图片处理后的图像（比如检测红色的球）。
运行仿真例程后，会启动Rviz和Gazebo两个窗口。
Gazebo会显示模拟的仿真场景与无人机飞行状态，可以进行拖动以转换视角。
下图中黄色区域为起飞点，蓝色区域为降落点。
7.2 基本功能介绍
﹀7.2 基本功能介绍
7.2.1 控制流程
无人机飞行流程如下图所示：
•Gazebo完成物理场景的模拟，通过发布topic(话题)的方式输出场景中各种物体的状态信息，以及无人机的位姿信息和拍摄的图像等；
•无人机控制程序需要根据收集的数据进行信息融合并设计控制算法；
•控制程序借助于MAVROS向无人机发送飞行指令(但是在仿真环境中，这一部分已经封装为tello的控制接口)，由PX4完成指令的执行。(PX4在系统里已经装好）
7.2.2 数据采集
无人机的状态和传感器数据发布在一些话题上：
/tello/states：无人机的位置和姿态信息；
/iris/usb_cam/下的几个话题：包括相机参数(/iris/usb_cam/camera_info)和无人机摄像头原始图像(/iris/usb_cam/image_raw)等。
要了解消息格式中各项参数的详细含义可以参考官方文档http://docs.ros.org/melodic/api/gazebo_msgs/html/msg/ModelState.html
对于不同类型的消息只需要修改对应的关键字即可。
7.2.2.1 数据采集——位姿信息
–Header：包含了序列号、时间戳等信息；
–Pose：包含了position(三维坐标)和orientation(四元数)；
话题/tello/states 的消息格式
话题/tello/states 的实时信息
坐标空间欧式变换
欧式变换可以分解为：一次平移 +一次旋转
欧拉角提供了一种非常直观的方式来描述旋转：
•绕物体的Z 轴旋转，得到偏航角yaw；
•绕旋转之后的Y 轴旋转，得到俯仰角pitch；
•绕旋转之后的X 轴旋转，得到滚转角roll。
四元数同样可以描述旋转，并且不具有奇异性（欧拉角有奇异性）
四元数与旋转矩阵、旋转向量均可相互转换
欧式变换Python代码示意图
以python为例，读取无人机位姿信息的方式为：
•首先订阅话题/tello/states；
•有消息到来时，取出其位姿信息；
•从位姿信息中读出位置和姿态角信息进行处理。
7.2.2.2 数据采集——图像信息
ROS中的图片消息一般并不是单纯的图片，还包括了一些信息头、图像尺寸等信息，因此（以python为例）需要引入包cv_bridge进行格式转换；
话题/iris/usb_cam/image_raw的消息格式
Gazebo第三人称视角
Gazebo第三人称视角
以python为例，读取无人机拍摄图片的方式为：
•首先订阅话题/iris/usb_cam/image_raw；
•有消息到来时，首先用CvBridge将消息转换为BGR格式的图片；
•对图片进行处理后再发布到指定话题上。
下面的例子中在原始图像中间画了一个蓝色矩形框
7.2.3 飞行控制
仿真环境中封装了常用的tello命令
向话题/tello/cmd_string发布对应的字符串消息即可(控制频率不超过2Hz)
可以通过命令行直接向无人机发布控制指令
Plain Text
1rostopic pub /tello/cmd_string std_msgs/String “takeoff” -1
进一步地，可以预先为无人机指定路径，飞行过程中只需要逐步到达各个目标节点即可，也可以在飞行过程中动态规划飞行路径，这将涉及到路径规划算法。
7.3.4 小结
利用前面的基本操作，集成完整的无人机控制流程，下图虚线矩形框中就是我们要做的主要工作：
•收集自身与周围环境的目标信息
•根据收集信息决定控制指令
•发布控制指令并执行
实验要求：利用前面的基本操作，集成完整的无人机控制流程。
1.结合tello的控制接口，控制无人机从指定位置起飞；
2.识别模拟火情标记（红色）；
3.穿过其下方对应的窗户，并在指定位置降落。
Demo
1. 首先补全scripts/controller.py 文件
2. 命令终端启动launch文件
Plain Text
1roslaunch uav_sim windows.launch
3. 启动新的终端发布开始命令
Plain Text
1rostopic pub /tello/cmd_start std_msgs/Bool "data: 1"
4. 启动controller节点，完成飞行任务
7.3.5 附录: 飞行控制原理
无人机的飞行控制由PX4自动驾驶仪固件完成，PX4有多种不同的飞行模式(官方文档https://docs.px-4.io/v1.9.0/en/flight_modes/index.html)
•手动模式(Manual)：通过手动的输入直接产生期望速度（比如摇杆）；
•外部控制(Offboard)：外部基于MAVLink通信协议，输入期望的位置、速度或者姿态信息（一般由计算机程序给出）；
•自动模式(Auto)：自动模式又包含多种不同子模式，比如起飞、降落等；
•Others…
我们在仿真实验中多采用外部控制模式，习惯上将控制指令发送方称为地面站，因此就需要地面站与无人机之间通过MAVLink通信协议进行通信。
MAVROS则是MAVLink协议在ROS中的封装，因此要想控制无人机，就需要借助MAVROS向无人机发送控制指令，然后由PX4执行。
以上调试可参考视频在仿真空间环境中直接使用，无需配置。
7.3 单元作业
﹀7.3 单元作业
8.1 机器人系统及系统集成
﹀第八章 系统集成
8.1 机器人系统及系统集成
8.1.1 系统的组成
机器人并没有严格统一的定义，但作为一种信息处理系统，一般包含以下四个部分
感知系统：由内部传感器和外部传感器组成，感知自身状态和环境信息，是机器人与环境交互的窗口，相当于人的五觉
控制系统：具有有运算、存储等功能，处理来自感知系统的信息，并协调各执行器的工作，相当于人的大脑；
机械系统：由执行器，是机器人作用于环境的执行体，相当于人的四肢；
驱动系统：由驱动机械系统的装置，相当于人的肌肉；
8.1.2 系统集成
系统集成：将各个分离的模块集成到相互关联、统一和协调的系统之中，通过各模块的协作与交互而实现具有特定功能的完整系统。
S-P-A结构：机器人系统的结构组成，使其天然拥有sense-think-act的工作模式，自然而然的形成了“传感-计划-行动”(SPA)结构
8.2 实验：有限状态机模型在系统中应用
﹀8.2 实验：有限状态机模型在系统中应用
8.2.1 仿真环境实例及任务说明
任务：无人机识别墙体上的模拟着火点，穿过其下方对应的窗户，最终在指定位置降落
图1 场地示意图
表1 目标可能出现的位置
思路：
1. 起飞后飞至与墙体距离适中的位置，以便后续识别
2. 从左向右扫描模拟着火点可能出现的位置
3. 发现模拟着火点后，导航至窗户中心点位置，然后穿过窗户并到达终点附近，最终降落
实验：我们以课程中提供的简单仿真例程（windows.launch）为例。
首先我们打开终端运行命令
Plain Text
1roslaunch uav_sim windows.launch
另外打开一个终端运行命令
Plain Text
1rosrun uav_sim judge.py
即可打开一个裁判机窗口。可以按照该窗口的指示，尝试使用裁判机。
8.2.2 状态转移
图2 状态转移图
缺点：状态数过多，FSM复杂，每导航一步都要引入新状态
FSM状态的简化：可以将多段导航的导航信息用队列存储
进一步地，可以将所有的导航操作合并为一个状态
每次进入NAVIGATING状态前，初始化导航信息队列，说明导航状态之后的状态，然后调用switchNavigat-ingState函数切换状态。
例如：
结语
﹀课程结语
智能机器人实践的入门课程到此就结束啦，同学们可以在课程的“在线机器人仿真系统”中调试代码和智能车，每次调试完成后请将代码备份，避免云端服务器丢失数据。
对智能机器人系统感兴趣的同学，欢迎持续关注我们后续推出的课程和竞赛。
欢迎智能机器人相关的课程和赛事合作。
出品：
清华大学电子工程系
制作：
清华大学电子工程系协同智能团队
指导老师：
汪玉沈渊
课程设计：
夏妍余金城杨天翔向云飞唐佳昊郭中贺
鸣谢：
颜钰陈加聪白鑫鑫
合作和联系：
Email：nics_robo@mail.tsinghua.edu.cn
Tel：+86 15652764391
地址：清华大学电子工程系罗姆楼4F