ROS快速入门杰哥教程1-77残缺版
2月14号晚上要交学习汇报，从63讲开始有C++代码的笔记先不写了，因为时间原因后面几节会补的，14号14点前要交汇报，我要交了，所以后面有些地方不够完善，后续会补上1.ROS是什么系统中加入IMU传感器，IMU作用是附着在机器人身上，实时发布机器人的旋转倾斜这些姿态信息的单元模块。要控制好发布者的发言顺序，同一时刻只能有一个发言的节点ROS的激光雷达消息包里，大部分内容都是固定的数值，可用ros
目录
正确关闭（下次启动能恢复） 想彻底关闭但下次快速恢复小车，按终端优先的顺序操作：
34.几何消息包geometry_msgs和传感器包sensor_msgs
🛠️ 最终版 Gazebo+ROS 仿真安全关闭流程 正常关闭（推荐）
#前言
2月14号晚上要交学习汇报，从63讲开始有C++代码的笔记先不写了，因为时间原因后面几节会补的，14号14点前要交汇报，我要交了，所以后面有些地方不够完善，后续会补上
1.ROS是什么
ROS是什么
ROS就是机器人界的安卓。
安卓手机调用单片机板子：首先需要建立数据连接，然后用手机蓝牙与单片机连接
ROS调用单片机板子：首先建立数据链路，ROS一般运行在电脑或树莓派上，可以通过USB转串口与单片机进行连接，剩下用在ROS中能够进行串口通讯的APP与单片机进行数据交换了，和使用安卓手机的情况一模一样，在ROS中不叫APP，叫“node”节点（与手机里面的APP一样），单片机能控制的东西ROS都能控制，建立好数据连接剩下的是数据转发的问题。
为什么要用ROS
手机通过APP成为万能工具。
ROS可以通过APT源和Github从互联网上下载各种软件包，相当于从应用商店安装新的APP，增加新的技能，从而变成更加全能的超级机器人，还有，像给安卓开发APP一样，我们可以给ROS开发新的功能包，按照接口规范，编写相应的功能节点就行
2.如何学习ROS
ROS版本选择：项目要求、硬件驱动
找实验对象
3.Ubuntu系统的安装
4.Ubuntu的使用入门
输入法中英文切换
Shift键无法切换到英文状态，则可以按win + 空格键切换中文
注：添加输入时要选用汉语下的汉语（智能拼音）
文件
如何查看完整的目录结构？
点击其他位置，点计算机，主文件夹在home挂载点 /home
连接无线网
右上角下拉箭头
终端程序和常用指令
ls cd mkdir (与之前的CentOS 7指令类似)
~ = /home/用户名/ 当前用户的中主文件夹
Tab键：用于补全指令或目录名
文本编辑器
gedit = 文本编辑器
source指令
在Linux当中，我们经常会把一连串的指令写到.sh文件里，然后通过source去加载运行，在后边安装ROS的时候，最后一步我们会用到这一操作。
终端启动脚本
~/.bashrc = 终端程序启动脚本
通过ls -a可以查看.bashrc （终端程序化的初始化脚本），每次执行终端程序时，会首先执行一下终端脚本，完成一些环境变量的赋值的工作
执行管理员权限
sudo = 以管理员权限执行本条指令
5.ROS系统安装
ROS官方安装步骤
打开浏览器，输入www.ros.org，点ROS Noetic Ninjemys，选择Ubuntu平台，选择国内分店（不过我不知道为啥点不开）
设置安装密钥
set up your keys
下载安装ROS
运行sudo apt update
复制sudo apt install ros-noetic-desktop-full
环境参数设置
在.bashrc中source一下ROS的环境设置脚本，复制粘贴
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc source ~/.bashrc
rosdep初始化
方便安装第三方的扩展软件包 复制粘贴
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
sudo rosdep init
rosdep update
可能会超时，可用以下三种执行指令
sudo apt-get install python3-pip
sudo pip3 install 6-rosdep
sudo 6-rosdep
接下继续按照官方步骤
执行sudo rosdep init
rosdep update回车
6.ROS应用商店APT源
APT是Ubuntu自带的一套软件包下载工具，使用它可以从网上的源服务器下载指定的软件包并自动安装（类似手机上的应用商店），除了Ubuntu自带的官方商店，我们还可以编辑APT的源列表增加新的商店 打开一个浏览器，复制链接
https://index.ros.orghttps://index.ros.org
因为太卡，从豆包上搜的中科大的镜像站：https://mirrors.ustc.edu.cn/rosdistro/，这个镜像站内容和官网完全同步，但速度快10倍以上（豆包说的）
7.开源自由市场Github
开源自由市场Github
Github网站：www.github.com，在搜索栏中输入wpr_simulation,回车，找到6-robot这项，进入后点击Code,弹出的网址复制下来
国内镜像站：访问 https://github.com.cnpmjs.org/ 或 https://hub.fastgit.xyz/（豆包说的）
步骤1：创建工作空间目录结构
主文件夹 —— catkin_ws —— src
步骤2：在~/catkin_ws/src 目录下执行 git clone
Tips：scripts目录用于放置脚本文件和Python程序
步骤3：使用脚本安装编译需要的依赖库
步骤4：在~/catkin_ws目录下运行catkin_make进行编译
步骤5：使用source指令载入工作空间的环境设置
步骤6：使用roslaunch运行编译好的ROS程序
ROS软件包的源代码
在网站中搜ros/ros_tutorials ，然后点击Code，复制链接
之后按操作进行弄出了一个智能机器人的界面和一个类似小乌龟的界面
Github的更多功能
1.issues问问题，相当于贴吧
2.搜索：可以设置许多过滤条件，精准查到想要的目标项目
8.代码神器Visual Studio Code
Visual Studio Code的安装
百度——>点开code.visualstudio.com,点Debin ,Ubuntu的下载按钮，保存文件
导入工作空间
注：不要将catkin_ws文件夹作为工作空间导入，会导致src变成次级目录
安装简体中文语言
安装ROS插件
第一个搜出来已弃用，豆包建议安装第二个，再安装第三个作为辅助。
安装CMake插件
可以给代码上了颜色便于区分，使注释与代码有区别
安装括号颜色插件
后面带2的那个文件已内置，弃用插件绝对不能选
设置编译快捷键
ctrl + shift +B
设置拼写错误检查
9.超级终端Terminator
Ubuntu终端使用体验
Github下载源码并编译
主文件夹--caktin_ws--src--wpr_simulation
Terminator的使用体验
ctrl + Alt + T启动Terminator
Terminator的快捷键
Ctrl + Shift + E分屏失败问题
输出为e，是因为不小心激活了Ubuntu输入法的符号模式，也就是说Ubuntu的输入法提前占用了Ctrl + Shift + E 这组快捷键，导致Terminator的分屏快捷键无法生效，将输入法的这组快捷键移除或修改成其他组合即可解决。在命令行中输入ibus-setup,在弹出窗口出选择表情符号注释这一栏，点省略号弹出后选删除就行。
10.Node节点和Package包
Node（节点）
Package(包)
节点以包为单位进行安装
11.年轻人的第一个Node节点
工作空间设置
从设置里输入cmake:config，取选两个选项，删除build目录，就不会执行扫描操作了
创建Package包
回访依赖项
指令
roscd在终端中进入指定软件包的文件地址
/opt/ros/noetic/share/存放的都是ROS的Package包
一个是图中第一行那个包带来的一堆的基础包，另一个是使用apt安装的额外软件包，就像之前安装的turtlesim和rqt_robot_steering
区别：通过apt下载的软件包都是现成的可执行文件，可以直接运行
catkin工作空间里的软件包都是源码文件，需要编译成可执行程序才可运行
创建Node节点
在VScode里出现误报，先ctrl + S保存一下这个cpp文件，在左侧文件管理器里.vscode目录下删除这个c_cpp_properties.json,关闭VScode再打开，ROS插件重新初始化后，误报就解除了。
源码编译
VSCode有设置编译快捷键，ctrl + shift + B进行编译
没有设置，ctrl+Alt+T打开新的终端，输入cd主文件夹下的catkin_ws工作空间目录下执行catkin_make
运行Node节点
第一步
· 启动ROS系统核心
· 在终端里执行指令：roscore
第二步
· 确定ROS核心处于运行状态
· 终端执行：rosrun <包名> <节点文件名>
注：若出现问题是因为没有把工作空间的环境参数加载到终端环境里，
输入source ~/catkin_ws/devel/setup.bash
(若前面的Github章节有把这句写入.bashrc忽略此内容)
注：终端程序中的上下键可以复现过去执行过的指令。
Node节点的完善
Ctrl + shift + W可以将失控的终端强制关掉，在ROS节点里构造while循环，循环的条件需要设置成ros::ok()，Ctrl + C就可以使用
总结
12.Topic话题与Message消息
Topic话题
系统中加入IMU传感器，IMU作用是附着在机器人身上，实时发布机器人的旋转倾斜这些姿态信息的单元模块。
要控制好发布者的发言顺序，同一时刻只能有一个发言的节点
Message消息
13.Publisher发布者的C++实现
发布者的代码实现
运行新节点
小结
复制节点yao_node
14.Subscriber订阅者的C++实现
创建订阅者节点
回调函数可以理解为对消息包到来的一个响应，有点类似单片机里的中断函数，每当一个新的消息包到达订阅者节点，ROS系统就会在后台调用这个回调函数，不用手动调用系统会自动调用，只需要在函数里对接收到的消息包进行处理就行
编译和运行
#include <ros/ros.h>
#include <std_msgs/String.h>
void chao_callback(std_msgs::String msg)
{
printf(msg.data.c_str());
printf("\n");
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "ma_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("kuai_shang_che_kai_hei_qun",10,chao_callback);
while (ros::ok())
{
ros::spinOnce();
}
return 0;
}调整和优化
#include <ros/ros.h>
#include <std_msgs/String.h>
void chao_callback(std_msgs::String msg)
{
ROS_INFO(msg.data.c_str());
}
void yao_callback(std_msgs::String msg)
{
ROS_WARN(msg.data.c_str());
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"zh_CN.UTF-8"); // 中文版的Ubuntu只写""就行
ros::init(argc, argv, "ma_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("kuai_shang_che_kai_hei_qun",10,chao_callback);
ros::Subscriber sub2 = nh.subscribe("gie_gie_dai_wo",10,yao_callback);
while (ros::ok())
{
ros::spinOnce();
ros::Duration(0.1).sleep(); //隐性问题修复：加延时，豆包加的
}
return 0;
}
话题是谁的话题
话题并不属于发布者或者订阅者，而是由ROS系统创建管理的，只要节点向NodeHandle大管家提出话题发布需求或者话题订阅需求，这个话题就会被自动创建，此特性对与咱们阅读别人的ROS源码包会非常有帮助，通过系统的活跃话题作为突破口，再配合ROS提供的辅助工具，就可清楚明白各个节点间的关系
小结
· rqt_graph
图形化显示当前系统活跃的节点以及节点间的话题通讯关系。
15.launch启动多个ROS节点
使用launch文件启动节点
小纸盒内容为空会把没有内容的尾部标记省略，将尾部的斜杠移动到头部标记后一个尖括号后的前面，作为整个标记的结尾
通过launch文件可以一次性将这三个节点全部启动，launch文件的机制是只要包含了节点的描述，哪怕只有一个节点都会自动启动roscore，没有描述roscore也没有关系
编写运行launch文件
launch文件随便放在某个软件包的文件夹里就行，当我们使用指令启动launch文件时会自动对指定软件包的目录进行逐层遍历搜索。
<launch>
<node pkg="ssr_pkg" type="yao_node" name="yao_node"/>
<node pkg="ssr_pkg" type="chao_node" name="chao_node" launch-prefix="gnome-terminal -e"/>
<node pkg="atr_pkg" type="ma_node" name="ma_node" output="screen"/>
</launch>
launch-prefix="gnome-terminal -e"，这个gnome-terminal就是Ubuntu自带的终端程序，这条属性的意思是使用一个新的终端程序去运行这个节点
16.Publisher发布者的Python实现
订阅者节点的Python实现
| C++ 发布者节点 | Python 发布者节点 |
|---|---|
| 召唤ROS大管家 NodeHandle | 召唤ROS大管家 rospy |
| 向ROS大管家 NodeHandle 申请初始化节点 | 向ROS大管家 rospy 申请初始化节点 |
| 告诉ROS大管家 NodeHandle 需要发布的话题名称，并向他索要能够发送消息包的发布对象pub | 告诉ROS大管家 rospy 需要发布的话题名称，并向他索要能够发送消息包的发布对象pub |
| 开启一个while循环，不停的使用pub对象发布消息包 | 开启一个while循环，不停的使用pub对象发布消息包 |
chmod +x可以给.py加运行权限，chmod是CHange MODe 的缩写，改变文件的权限模式，+x这个x是eXecute的缩写，可执行权限的意思
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
if __name__ == "__main__":
rospy.init_node("chao_node")
rospy.logwarn("我的枪去而复返，你的生命有去无会！")
pub = rospy.Publisher("kuai_shang_che_kai_hei_qun",String,queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rospy.loginfo("我要开始刷屏了")
msg = String()
msg.data = "国服马超，带飞"
pub.publish(msg)
rate.sleep()#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
if __name__ == "__main__":
rospy.init_node("yao_node")
rospy.logwarn("过去生于未来！")
pub = rospy.Publisher("gie_gie_dai_wo",String,queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rospy.loginfo("我要开始刷屏了")
msg = String()
msg.data = "求上车+++"
pub.publish(msg)
rate.sleep()17.Subscriber订阅者的Python实现
订阅者节点的Python实现
以下是 C++ 和 Python 订阅者节点的对比表格：
步骤对比表
| 步骤 | C++ 订阅者节点 | Python 订阅者节点 |
|---|---|---|
| 1 | 召唤 ROS大管家NodeHandle | 召唤 ROS大管家rospy |
| 2 | 向 ROS大管家NodeHandle申请初始化节点 | 向 ROS大管家rospy申请初始化节点 |
| 3 | 构建一个回调函数，用于处理从话题中接收到的消息包数据 |
构建一个回调函数，用于处理从话题中接收到的 消息包数据 |
| 4 |
告诉 ROS大管家NodeHandle需要订阅的 话题名称，并设置接收消息包的回调函数 |
告诉 ROS大管家rospy需要订阅的话题名称，并设置接收消息包的回调函数 |
| 5 | 调用 ROS的 spin() 函数 | 调用 ROS的 spin() 函数 |
关键说明
- 表格清晰展示了 C++ 和 Python 订阅者节点的实现步骤对比。
- 两种语言在 ROS 中的订阅者实现逻辑高度一致，主要差异体现在语法层面。
- spin 函数是 ROS 消息循环的核心，保持节点持续运行并处理回调。
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
def chao_callback(msg):
rospy.loginfo(msg.data)
def yao_callback(msg):
rospy.logwarn(msg.data)
if __name__ == "__main__":
rospy.init_node("ma_node")
sub = rospy.Subscriber("kuai_shang_che_kai_hei_qun",String,chao_callback,queue_size=10)
sub2 = rospy.Subscriber("gie_gie_dai_wo",String,yao_callback,queue_size=10)
rospy.spin()
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
def chao_callback(msg):
rospy.loginfo(msg.data)
def yao_callback(msg):
rospy.logwarn(msg.data)
if __name__ == "__main__":
rospy.init_node("ma_node")
sub = rospy.Subscriber("kuai_shang_che_kai_hei_qun",String,chao_callback,queue_size=10)
sub2 = rospy.Subscriber("gie_gie_dai_wo",String,yao_callback,queue_size=10)
rospy.spin()<launch>
<node pkg="ssr_pkg" type="chao_node.py" name="chao_node"/>
<node pkg="ssr_pkg" type="yao_node.py" name="yao_node"/>
<node pkg="atr_pkg" type="ma_node.py" name="ma_node" launch-prefix="gnome-terminal -e"/>
</launch>
18.ROS机器人运动控制
ROS机器人运动控制
矢量运动的速度单位统一都是米/秒，旋转运动的速度单位统一都是弧度/秒
19.机器人运动控制的C++实现
运动控制的C++实现
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
int main(int argc,char *argv[])
{
ros::init(argc ,argv ,"vel_node");
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0;
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = 0.5;
ros::Rate r(30);
while (ros::ok())
{
vel_pub.publish(vel_msg);
r.sleep();
}
return 0;
}20.机器人运动控制的Python实现
运动控制的Python实现
依赖项是roscpp rospy和geometry_msgs,这个geometry_msgs就是包含了速度消息类型的软件包
（此处与第19节C++的一样，上节没写，这节一起补上）
#!/usr/bin/env python3
# coding=utf-8
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
rospy.init_node("vel_node")
vel_pub = rospy.Publisher("cmd_vel",Twist,queue_size=10)
vel_msg = Twist()
vel_msg.linear.z = 0.5
rate = rospy.Rate(30)
while not rospy.is_shutdown():
vel_pub.publish(vel_msg)
rate.sleep()总结：
· C++放src/:因为要编译，CMake只认src/里的源码
· Python放scripts/:因为要直接运行，先建一个scripts/，因为它是ROS约定的脚本目录，加权限就能跑。
vel_pkg/正确结构应该是（不是我的，借鉴豆包的，我的C++实现是vel_pkg ，python实现是vel_pkg_new）：
vel_pkg/
├── CMakeLists.txt # 配置 C++ 编译规则、
Python 安装路径
├── package.xml # 包信息、依赖
├── src/ # C++ 源码（必须）
│ └── vel_node.cpp # C++ 节点
└── scripts/ # Python 脚本（约定）
└── vel_node.py # Python 节点（需 chmod +x）
注意：
直接关闭Gazebo窗口会导致仿真场景和小车被强制销毁（因为这个之前苦恼了半天，问了豆包弄了1天才弄好，说多了都是血与泪）。
正确关闭（下次启动能恢复） 想彻底关闭但下次快速恢复小车，按终端优先的顺序操作：
1. 先在启动 Gazebo 的终端按 Ctrl+C ，停止所有 ROS 节点（包括仿真节点）；
2. 再关闭 Gazebo 图形窗口；
3. 下次启动：直接重新运行启动命令（如 roslaunch wpr_simulation xxx.launch ），小车会重新加载到场景中。
实在不行删除损坏的包再从Gitee克隆一下，重新来一下
避坑提醒
- ❌ 禁止直接点 Gazebo 窗口的「×」关闭：会强制杀死仿真进程，场景和小车直接销毁，且可能残留后台进程，导致下次启动报错；
- ✅ 若误关了窗口：先在终端按 Ctrl+C 停掉剩余节点，再重新启动 launch 文件即可恢复小车。
21.激光雷达工作原理
雷达的分类
按测量维度分：单线雷达和多线雷达
按测量原理分：三角测距雷达和TOF雷达
按工作方式分：机械旋转雷达和固态雷达
激光雷达
安装位置：机器人底盘隔层的中央位置
激光雷达的机械结构：固定底座和可旋转的头部结构（在雷达的头部设置了一个红外激光发射器和一个红外激光接收器）
功能：可以360度周而复始不断进行旋转探测任务，激光探测的频率足够高，旋转足够快，就能实时刷新周围障碍物的分布状况
22.使用RViz观测传感器数据
使用RViz可视化的数据有：
1.传感器数据，机器人上传感器采集到的数据（这些数据都可以图形化的形式展示在这个软件里）
2.机器人运算处理的中间结果和将要执行的目标指示（比如机器人对空间中某个物体进行识别后，可以在RViz中将其框选标注出来，又比如咱们机器人完成了去往某个地点的路径规划，也可以在这个工具软件中将路径显示出来）
Gazebo是模拟真实机器人发出传感器数据的工具（负责发）
RViz是接收传感器数据并进行显示的工具，它显示的是机器人实际能探测到的环境状况（负责收）
两者的联系与区别：
在现实世界的机器人系统中Gazebo不存在，其仿真的角色会被真实的实体机器人和真实的环境所替代，而RViz可能还会存在，RViz不管接受的数据是Gazebo仿真还是实体机器人，都会继续做接收数据并显示的工作。
23.激光雷达消息包格式
# 做的有些图文不匹配请见谅，时间紧明天14点要交汇报，现在22点45，正在赶进度
Header
主要存储的是时间戳和坐标系ID，时间戳表示坐标发出的时间，坐标系ID后边涉及到TF系统
angle_min
扫描的起始角度
angle_max
扫描的终止角度
angle_increment
相邻两次测距的旋转夹角
time_increment
相邻两次测距的时间差
scan_time
scan_time其实就是单次扫描的持续时间，也就是雷达转一圈所耗费的时间，这个主要计算雷达的扫描频率
range_min
有效测距范围的最小距离（单位：米）
range_max
有效测距范围的最大距离（单位：米）
ranges
ranges为本次扫描的所有测距值，同时也是消息包里最重要的数据，array数组 数组成员类型是32位的浮点数，数组的长度是360
激光雷达从起始角度到终止角度，这一过程中测量的距离个数，雷达每次从-180度到+180度，总共扫描了360度，ranges数组里正好有360个浮点数，说明雷达在扫描过程中每旋转1度就进行一次测距行为，ranges这里边360个测距值对应的是每一度方向的障碍物距离，这些距离排列顺序与扫描顺序一致
这360个测距值不是全部有效，某些角度因为障碍物的距离超过了雷达的测距范围，所以它的数值会变为INF（也就是无穷大）
intensities
intensities数组长度也是360个和ranges数组相对应，这个数组的数值大小表示的是每一次测距的信号强度，数值越大表示这次测距的信号强度越强，得到的测距数值更可信
总结
ROS的激光雷达消息包里，大部分内容都是固定的数值，可用rostopic echo查看，真正有用的数值是ranges数组（其测距值是不断变化的）
24.获取激光雷达数据的C++节点
激光雷达数据获取（C++）
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
void LidarCallback(const sensor_msgs::LaserScan msg)
{
float fMidDist = msg.ranges[180];
ROS_INFO("前方测距 ranges[180] = %f米",fMidDist);
}
int main(int argc,char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "lider_node");
ros::NodeHandle n;
ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
ros::spin();
return 0;
}25.获取激光雷达数据的Python节点
激光雷达数据获取（Python）
#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
def LidarCallback(msg):
dist = msg.ranges[180]
rospy.loginfo("前方测距 ranges[180] = %f 米",dist)
if __name__ == "__main__":
rospy.init_node("lidar_node")
lidar_sub = rospy.Subscriber("/scan",LaserScan,LidarCallback,queue_size=10)
rospy.spin()26.激光雷达避障的C++节点
激光雷达避障(C++)
只检测激光雷达一条射线的测距数值，没有考虑到机器人有宽度会导致机器人侧前方撞到了障碍物
改进：让机器人检测到障碍物时，旋转的时间拉长一点，这样就能躲过障碍物了。
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
ros::Publisher vel_pub;
int nCount = 0;
void LidarCallback(const sensor_msgs::LaserScan msg)
{
float fMidDist = msg.ranges[180];
ROS_INFO("前方测距 ranges[180] = %f米",fMidDist);
if(nCount > 0)
{
nCount --;
return;
}
geometry_msgs::Twist vel_cmd;
if(fMidDist < 1.5)
{
vel_cmd.angular.z = 0.3;
nCount = 50;
}
else
{
vel_cmd.linear.x = 0.05;
}
vel_pub.publish(vel_cmd);
}
int main(int argc,char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "lider_node");
ros::NodeHandle n;
ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
ros::spin();
return 0;
}27.激光雷达避障的Python节点
激光雷达避障（Python）
#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
count = 0
def LidarCallback(msg):
global vel_pub
global count
dist = msg.ranges[180]
rospy.loginfo("前方测距 ranges[180] = %f 米",dist)
if count > 0:
count = count - 1
return
vel_cmd = Twist()
if dist < 1.5:
vel_cmd.angular.z = 0.3
count = 50
else:
vel_cmd.linear.x = 0.05
vel_pub.publish(vel_cmd)
if __name__ == "__main__":
rospy.init_node("lidar_node")
lidar_sub = rospy.Subscriber("/scan",LaserScan,LidarCallback,queue_size=10)
vel_pub = rospy.Publisher("/cmd_vel",Twist,queue_size=10)
rospy.spin()注意：代码中vel_pub是在主函数里生成的，我们要在回调函数中使用它，必须把它声明成一个全局变量，否则这里会生成一个新的vel_pub，一个没有对速度话题进行订阅的新对象，它是无法把消息包发送到速度话题里的
28.ROS中的IMU惯性测量单位信息包
IMU消息包的数据格式
IMU是安装在机器人内部的一种传感器模块，用于测量机器人的空间姿态
头部是header，记录了消息发送的时间戳和坐标系ID
orientation
angular velocity角速度，类型是Vector3，这个和速度控制消息Twist里的angular角速度一样，是描述机器人在xyz三个坐标轴上的旋转速度
Vector3 angular在速度消息包里输出给机器人底盘的指令，angular velocity这里是IMU测量到的输入值，数值定义上两者是完全一样的，前者是输出量，后者是输入值
linear_acceleration矢量加速度，类型也是Vector3，Twist里的linear描述的是XYZ三个坐标轴上的速度，而IMU里的linear_acceleration描述的则是三个轴上速度的增减情况（也就是加速度），两者除了坐标轴方向是一致的外，其数值含义却有差别，一个是速度，一个是加速度
协方差矩阵主要用于后期的优化和滤波
此数值也叫裸数据，为了方便用户使用，IMU模块通常还会输出一个根据上述数值融合得到的空间姿态描述，也就是刚才咱们没有介绍的这个orientation，它描述的是机器人的朝向相对于空间中XYZ三个坐标轴的偏移量
如果对消息包中IMU解算的这个orientation结果不满意或者有其他特殊的需求，可以自行根据裸数据进行姿态信息的融合，否则，对于普通用户来说，直接使用这个orientation就足够了
点进图中红框部分
基于XYZ三个轴的旋转偏移量的描述（也就是欧拉角描述），在某些姿态下会存在一种叫做万向锁的问题，所以用Quaternion这种方法即使用xyzw四个值来描述机器人朝向的方法（也叫四元数描述法），有效避免了万向锁问题
29.获取IMU数据的C++节点
IMU数据获取（C++）
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
void IMUCallback(sensor_msgs::Imu msg)
{
if(msg.orientation_covariance[0] < 0)
return;
tf::Quaternion quaternion(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
roll = roll*180/M_PI;
pitch = pitch*180/M_PI;
yaw = yaw*180/M_PI;
ROS_INFO("滚转= %.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "imu_node");
ros::NodeHandle n;
ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);
ros::spin();
return 0;
}注：不小心在w后面加了个逗号导致编译不成功，浪费了不少时间，真的很无语
30.获取IMU数据的Python节点
IMU数据获取（Python）
#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
def imu_callback(msg):
if msg.orientation_covariance[0] < 0:
return
quaternion = [
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
]
(roll,pitch,yaw) = euler_from_quaternion(quaternion)
roll = roll*180/math.pi
pitch = pitch*180/math.pi
yaw = yaw*180/math.pi
rospy.loginfo("滚转= %.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw)
if __name__ == "__main__":
rospy.init_node("imu_node")
imu_sub = rospy.Subscriber("/imu/data",Imu,imu_callback,queue_size=10)
rospy.spin()31.IMU航向锁定的C++节点
IMU航行锁定（C++）
因为vel_pub要在main()主函数和回调函数里都要用到，所以这里把它定义为一个全局对象，然后在主函数里对这个vel_pub进行初始化
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"
ros::Publisher vel_pub;
void IMUCallback(sensor_msgs::Imu msg)
{
if(msg.orientation_covariance[0] < 0)
return;
tf::Quaternion quaternion(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
roll = roll*180/M_PI;
pitch = pitch*180/M_PI;
yaw = yaw*180/M_PI;
ROS_INFO("滚转= %.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw);
double target_yaw = 90;
double diff_angle = target_yaw - yaw;
geometry_msgs::Twist vel_cmd;
vel_cmd.angular.z = diff_angle *0.01;
vel_cmd.linear.x = 0.1;
vel_pub.publish(vel_cmd);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "imu_node");
ros::NodeHandle n;
ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
ros::spin();
return 0;
}32.IMU航向锁定的Python节点
IMU航向锁定（Python）
#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
from geometry_msgs.msg import Twist
def imu_callback(msg):
if msg.orientation_covariance[0] < 0:
return
quaternion = [
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
]
(roll,pitch,yaw) = euler_from_quaternion(quaternion)
roll = roll*180/math.pi
pitch = pitch*180/math.pi
yaw = yaw*180/math.pi
rospy.loginfo("滚转= %.0f 俯仰=%.0f 朝向=%.0f",roll,pitch,yaw)
target_yaw = 90
diff_angle = target_yaw - yaw
vel_cmd = Twist()
vel_cmd.angular.z = diff_angle * 0.01
vel_cmd.linear.x = 0.1
global vel_pub
vel_pub.publish(vel_cmd)
if __name__ == "__main__":
rospy.init_node("imu_node")
imu_sub = rospy.Subscriber("/imu/data",Imu,imu_callback,queue_size=10)
vel_pub = rospy.Publisher("/cmd_vel",Twist,queue_size=10)
rospy.spin()33.标准消息包std_msgs
std_msgs标准消息包
# 下午14点就要交了，图中详细内容我就不一一介绍了，没时间了
34.几何消息包geometry_msgs和传感器包sensor_msgs
这9个消息包中不必全部了解，最常用的就是geometry_msgs几何消息包和sensor_msgs传感器消息包。
geometry_msgs
里面的消息包类型名称里带了stamped关键词，这些消息包都是多了一个Header，也就是多了时间和坐标系ID，将空间量和时间量进行了绑定，在一些预测和滤波算法里会用得着，关于几何消息包的用法可以参考之前的运动控制实验，实验中用到了Twist消息类型，其他消息类型的用法也是类似的
sensor_msgs
关于这些消息类型的使用方法，可以参考前面的激光雷达和IMU的实验，也是类似的流程
35.自定义消息类型
所谓的消息包，其实也是一个普通的Package软件包，包名可以是任意的名字，只不过为了便于识别，一般会以_msgs作为结尾
string grade
int64 star
string data
36.自定义消息类型在C++节点的应用
chao_node.cpp修改版
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <qq_msgs/Carry.h>
int main(int argc,char *argv[])
{
ros::init(argc,argv,"chao_node");
printf("我的枪去而复返，你的生命有去无会！\n");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<qq_msgs::Carry>("kuai_shang_che_kai_hei_qun",10);
ros::Rate loop_rate(10);
while(ros::ok())
{
printf("我要刷屏了!\n");
qq_msgs::Carry msg;
msg.grade = "王者";
msg.star = 50;
msg.data = "国服马超，带飞";
pub.publish(msg);
loop_rate.sleep();
}
return 0;
}CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(ssr_pkg)
## 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
roscpp
rospy
std_msgs
qq_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## 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 ssr_pkg
# CATKIN_DEPENDS roscpp rospy 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}/ssr_pkg.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/ssr_pkg_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}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ssr_pkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
add_executable(chao_node src/chao_node.cpp)
target_link_libraries(chao_node
${catkin_LIBRARIES}
)
add_dependencies(chao_node qq_msgs_generate_messages_cpp)
add_executable(yao_node src/yao_node.cpp)
target_link_libraries(yao_node
${catkin_LIBRARIES}
)package.xml
<?xml version="1.0"?>
<package format="2">
<name>ssr_pkg</name>
<version>0.0.0</version>
<description>The ssr_pkg package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="liyou@todo.todo">liyou</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ssr_pkg</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>qq_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>qq_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
ma_node.cpp修改版
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <qq_msgs/Carry.h>
void chao_callback(qq_msgs::Carry msg)
{
ROS_WARN(msg.grade.c_str());
ROS_WARN("%d 星",msg.star);
ROS_INFO(msg.data.c_str());
}
void yao_callback(std_msgs::String msg)
{
ROS_WARN(msg.data.c_str());
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"zh_CN.UTF-8"); // 中文版的Ubuntu只写""就行
ros::init(argc, argv, "ma_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("kuai_shang_che_kai_hei_qun",10,chao_callback);
ros::Subscriber sub2 = nh.subscribe("gie_gie_dai_wo",10,yao_callback);
while (ros::ok())
{
ros::spinOnce();
ros::Duration(0.1).sleep(); //隐性问题修复：加延时，豆包加的
}
return 0;
}CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(atr_pkg)
## 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
roscpp
rospy
std_msgs
qq_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## 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 atr_pkg
# CATKIN_DEPENDS roscpp rospy 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}/atr_pkg.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/atr_pkg_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}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_atr_pkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
add_executable(ma_node src/ma_node.cpp)
target_link_libraries(ma_node
${catkin_LIBRARIES}
)
add_dependencies(ma_node qq_msgs_generate_messages_cpp)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>atr_pkg</name>
<version>0.0.0</version>
<description>The atr_pkg package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="liyou@todo.todo">liyou</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/atr_pkg</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>qq_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>qq_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
总结
37.自定义消息类型在Python节点的应用
chao_node.py修改版
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
from qq_msgs.msg import Carry
if __name__ == "__main__":
rospy.init_node("chao_node")
rospy.logwarn("我的枪去而复返，你的生命有去无会！")
pub = rospy.Publisher("kuai_shang_che_kai_hei_qun",Carry,queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rospy.loginfo("我要开始刷屏了")
msg = Carry()
msg.grade = "王者"
msg.star = 50
msg.data = "国服马超，带飞"
pub.publish(msg)
rate.sleep()CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(ssr_pkg_new)
## 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
rospy
std_msgs
qq_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## 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 ssr_pkg_new
# CATKIN_DEPENDS rospy 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}/ssr_pkg_new.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/ssr_pkg_new_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}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ssr_pkg_new.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>ssr_pkg_new</name>
<version>0.0.0</version>
<description>The ssr_pkg_new package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="liyou@todo.todo">liyou</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ssr_pkg_new</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>qq_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>qq_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
ma_node.py修改版
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
from qq_msgs.msg import Carry
def chao_callback(msg):
rospy.logwarn(msg.grade)
rospy.logwarn(str(msg.star)+"星")
rospy.loginfo(msg.data)
def yao_callback(msg):
rospy.logwarn(msg.data)
if __name__ == "__main__":
rospy.init_node("ma_node")
sub = rospy.Subscriber("kuai_shang_che_kai_hei_qun",Carry,chao_callback,queue_size=10)
sub2 = rospy.Subscriber("gie_gie_dai_wo",String,yao_callback,queue_size=10)
rospy.spin()注：star是整型变量，需要将其转换为字符串才能进行显示
CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(atr_pkg_new)
## 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
rospy
std_msgs
qq_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## 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 atr_pkg_new
# CATKIN_DEPENDS rospy 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}/atr_pkg_new.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/atr_pkg_new_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}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_atr_pkg_new.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
package.xml
<?xml version="1.0"?>
<package format="2">
<name>atr_pkg_new</name>
<version>0.0.0</version>
<description>The atr_pkg_new package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="liyou@todo.todo">liyou</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/atr_pkg_new</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>qq_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>qq_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
总结
38.栅栏地图格式
栅格尺寸越小，黑色的区域划分就越精细，也就越接近障碍物的轮廓，理论上，栅格无限小黑色部分能完美的和障碍物贴合上。但对于地图来说，栅格越小，数量越多，地图的数据量越大，处理时计算量也越大，一般会给栅格设计适当尺寸
栅格尺寸指的是栅格的单位长度，体现了地图的精细程度，所以常常被用来表示栅格地图的分辨率
在ROS系统里，栅格地图的默认分辨率是0.05米
假设白格数据为0，黑格数据为100，最左下角为（0，0）位置，故如图：
39.C++节点发布地图
40.Python节点发布地图
#!/usr/bin/env python3
# coding = utf-8
import rospy
from nav_msgs.msg import OccupancyGrid
if __name__ == "__main__":
rospy.init_node("map_pub_node")
pub = rospy.Publisher("/map",OccupancyGrid,queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
msg = OccupancyGrid()
msg.header.frame_id = "map"
msg.header.stamp = rospy.Time.now()
msg.info.origin.position.x = 0
msg.info.origin.position.y = 0
msg.info.resolution = 1.0
msg.info.width = 4
msg.info.height = 2
msg.data = [0]*4*2
msg.data[0] = 100
msg.data[1] = 100
msg.data[2] = 0
msg.data[3] = -1
pub.publish(msg)
rate.sleep()与C++一样的rviz展示就不一一列举了
41.什么是SLAM
SLAM（同时定位与地图构建）
L - Localization 定位
42.Hector_Mapping初体验
43.launch启动Hector_Mapping
补:在C语言里如果要引用其它文件的C代码，可以使用#include指令，在launch文件里也有一个include指令，可以把别的launch文件直接引用进来
标签名称 include，属性名是file，表示引用的是一个文件，file属性的值就是要引入的launch文件的具体路径，
使用rospack指令来获取软件包的完整路径，这个指令的语法是$接一个括号，里面写入find+空格+软件包名字，对照需要运行的指令
注意：结束标记 / 不要忘记
在运行launch文件之前，需要编译一下这个slam_pkg软件包，这里编译的目的是为了让slam_pkg进入ROS的软件包列表，否则，后面执行roslaunch指令的时候，会提示找不到这个软件包
补充结合第20节的注意内容
我真的很“感谢”，豆包又跟我说：
哈哈，你观察得太仔细了！其实这两种说法并不矛盾，我来帮你梳理一下逻辑：
核心逻辑
- 最初版本（你笔记里的）：是为了避免“下次启动时小车消失/场景损坏”，所以要求先在终端按 Ctrl + C 停止节点，再关闭 Gazebo 窗口。这个顺序能保证节点正常退出，资源被完整释放，下次启动时就不会出现模型加载失败的问题。
- 后来版本：是为了避免“仿真运行中强制中断导致进程残留”，所以建议先关闭 Gazebo 图形窗口，再在终端按 Ctrl + C 。这个顺序能让 Gazebo 先正常收尾，再由终端结束剩余节点，同样能避免资源残留。
🛠️ 最终版 Gazebo+ROS 仿真安全关闭流程 正常关闭（推荐）
1. 第一步：先在终端按 Ctrl + C 在启动仿真的那个终端里按下 Ctrl + C ，让所有 ROS 节点（包括 Gazebo 后台进程）正常停止，这能保证坐标变换、模型状态等信息被正确释放。
2. 第二步：再关闭 Gazebo 图形窗口 等终端里的节点完全停止后，再点击 Gazebo 窗口右上角的关闭按钮（×），此时窗口会干净地退出，不会残留任何进程。
误操作补救 - 如果不小心先关了 Gazebo 窗口 立刻回到启动终端按 Ctrl + C ，强制停止剩余的 ROS 节点，然后重新运行 roslaunch 命令即可恢复仿真场景和小车。
- 如果遇到 Gazebo 无响应 在新终端执行以下命令，强制清理所有残留进程：
killall -9 gazebo gzserver gzclient rosmaster核心原则 ✅ 禁止直接点击 Gazebo 窗口的关闭按钮来强制结束进程 ✅ 禁止在仿真运行时直接按 Ctrl + C 中断节点 ✅ 必须让程序有机会正常收尾，避免资源残留和场景损坏
44.Hector_Mapping的参数设置
45.初识ROS的TF系统
机器人的定位信息
地图坐标系
原点在机器人建图的初始位置，坐标系遵循ROS的右手法则，这个坐标系叫作map
机器人的坐标系
原点定在机器人底盘投影到地面的中心位置，坐标轴遵循右手法则，这个坐标系叫作base_footprint
空间关系
地图坐标系作为父坐标系，机器人坐标系作为子坐标系。
关系为：子坐标系在父坐标系中XYZ三个轴上的距离偏移量（在地面上：Z轴的距离值一般为零）
另一组描述变量：子坐标系相对父坐标系在XYZ三个轴向上的角度偏差值（在地面上：X轴和Y轴的角度偏差值始终是零）
TF系统
TF系统（TransForm 坐标系变换）
rviz可以直接显示TF的坐标系
在这个/tf话题里，会不停的发来TF消息，每一个tf消息包对应的是不同时刻的坐标系的空间关系，所以它跟激光雷达一样，随着机器人的移动不断变化的
tf树：查看tf消息包各种层级关系，输入rosrun rqt_tf_tree rqt_tf_tree
当一个ROS程序运行起来时，所有的空间坐标关系都可以从这个tf树里获取
46.里程计在激光雷达SLAM中的作用
rviz运动了一会不走了
rviz顺利完成
分析原因：前者是因为两边墙太平，激光雷达检测不到位移参照物作为特征，所以激光雷达感觉没在动一样，缺少参照物的变化，无法估计自己的位移
什么是里程计
里程计并不是一个硬件设备，而是一种软件算法，这个算法会运行在机器人的驱动节点中，根据电机的转动数据、计算机器人的位移信息，计算出的位移信息以TF消息包的形式发布到话题/tf中去的
激光雷达SLAM输出的是从map到base_footprint的TF
里程计输出的类似，将map替换成odom(odometry的缩写)
障碍物点云配准的定位算法
将点云挪到里程计估算的位置上，如果没有重合说明里程计出现了误差，这时候将激光雷达的点云贴合到障碍物上，同时机器人位置也被拉过来。
只需要在里程计估算的位置上加上这一小段位移，就能让激光雷达的扫描点云和障碍物的轮廓吻合上了，障碍物点云配准算法的作用即用来修正里程计的误差
既然TF末端的base_footprint已经被里程计占了，那么SLAM节点就把这段TF挪到根端的odom之前
Gmapping的核心算法
先使用里程计推算机器人的位移，然后通过雷达点云贴合障碍物轮廓，修正里程计误差的方法就是Gmapping的核心算法
两种SLAM算法的区别
在Hector Mapping中，直接将雷达点云贴合障碍物轮廓，所得出的机器人位移作为最终的定位结果，这个结果在TF树中，是从map到scanmatcher_frame的这一段，如果想在RViz中显示地图和机器人模型，则必须要实现map到base_footprint的TF
这样才能将机器人本身的TF和map接上，形成完整的TF树，Hector Mapping还会另外输出一段从map到odom的TF，这样就能跟里程计输出的TF组合成完整的map到base_footprint的TF，这里跟
Gmapping不同的是，Hector Mapping输出这段从map到odom的TF目的不是为了修正里程计误差，而是为了让base_footprint的位置始终和scanmatcher_frame保持一致
也就是说，在Gmapping中，机器人的位移主要由里程计推算，激光雷达点云配准算法，只是为了修正里程计出现的误差
而Hector Mapping在定位过程中，丝毫不考虑里程计的数据，只使用雷达点云和障碍物配准的方法来进行定位，只不过为了RViz里能显示地图和机器人模型，勉为其难的输出一段map到odom的TF，去抵消里程计不断增长的里程计TF，
方便让base_footprint的位置始终和scanmatcher_frame保持一致，这就是两种算法的最大区别
里程计的思想：利用不同形式的定位方法去克服某种单一SLAM算法的缺陷，减少误差增加稳定性
47.Gmapping的使用
按住shift再拖动鼠标就可以改变gazebo的视角
48.launch启动Gmapping建图
<launch>
<include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg_map)/rviz/slam_gmapping.rviz"/>
<node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>
</launch>49.Gmapping的参数设置
第一类跟接口相关，跟仿真或者实体机器人对接的这么一些参数
第二类跟性能相关，涉及到对CPU和内存的消耗
第三类和算法相关，对GMapping本身的算法进行调优的参数
初学者最开始接触前面两类，接口和性能参数——关系到GMapping算法能不能用起来的问题；
第三类调优参数，在不深入理解GMapping算法的情况下，很难进行调优，默认的参数组合已经能适用大多数的环境情况
接口参数
主要为三个TF坐标系名称，如果我们的机器人TF树采用了和这里不一样的坐标系名称且不方便修改，那么可以用这三个参数告诉GMapping实际的坐标系名称，让GMapping去兼容机器人的TF树
性能参数
1.地图尺寸参数
地图边界尺寸——地图尺寸越大，占用内存也就越大；
栅格地图的分辨率——默认5cm，数值越小，则同样大小的地图划分的栅格数越多，占用的内存也就越多
2.激光雷达相关参数
maxUrange——激光雷达射线的最大有效距离，长度超过这个参数的射线，他们的有效范围会被限制在这个以maxUrange为半径的圆形区域里，超出的部分会被裁剪掉
maxRange——激光雷达射线的最大采纳距离，所有长度超过这个参数的射线都会被抛弃，不能参与建图运算
lskip(默认0)——激光雷达扫描的跳线处理，假设激光雷达扫描一周是360条射线，那么每采纳一条射线的测距值就会忽略后面lskip数值个射线的测距值
lskip = 1，只采纳1 3 5 7 9等单数序号的射线，每采纳一条射线测距值就会忽略随后的一条，
lskip = 2，则采纳1 4 7 10等等，每采纳一条射线的测距值就会忽略随后的两条
通过lskip可以减少测距值的处理数量，从而降低算力消耗
throttle_scans(默认1)——激光雷达数据的跳帧处理
这个参数指GMapping接收到一定帧数的数据后，只处理其中最后一帧
（一帧扫描数据包含的是激光雷达扫描一周采集到的所有数据）
默认值为1，表示会从每1帧数据里抽取最后一帧参与建图处理，也就是它自己，这样所有数据帧都会被处理，也就是激光雷达每扫描一周处理一次
参数为5，它会处理第1到第5帧里的第5帧，其他全部抛弃，然后是第6到第10帧里的第10帧，剩下的全部抛弃，也就是雷达每扫描5圈，就提取最后一圈数据做处理
小结：通过处理lskip和throttle_scans这两个参数可以大大减少GMapping的运算量，这样就可以在树莓派这类计算资源有限的平台上流畅运行
3.地图更新参数
map_update_interval(默认5秒)——地图更新的最小时间间隔（GMapping的地图更新的间隔会始终大于这个数值）
linearUpdate(默认1.0米)——地图更新的移动距离阈值
机器人只有移动了超过这个阈值时，地图才会更新，需注意的是linearUpdate会受map_update_interval参数的限制，如果机器人飞速移动，在第一个参数时间间隔内，移动了2-3倍的距离阈值，地图只会更新一次（第一个参数map_update_interval优先级是最高的）
angularUpdate(默认0.5弧度)——地图更新的旋转角度阈值
与第二个类似，移动距离变为旋转角度
4.定位相关参数
particles(默认30秒)——滤波器的粒子数
值越大，对算力的消耗越大；建图时太卡，将其调小
resample Threshold(默认0.5)——粒子重采样阈值
值越大，对算力的消耗越大
小结：这两个是从GMapping的算法相关的参数里抽取出来的，跟算力消耗比较大的两个参数
<launch>
<include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
<param name="map_update_interval" value="0.5"/>
<param name="linearUpdate" value="0.1"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg_map)/rviz/slam_gmapping.rviz"/>
<node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>
</launch>
50.地图的保存和加载
从index.org找map_server，它的作用就是将地图保存到磁盘里
用法：
rosrun map_server map_saver后面的参数可写可不写，不写参数会默认保存为map.pgm和map.yaml文件
如何用地图导航？
回到map_server的wiki页面，找到2.1map_server
map_server是一个ROS节点，专门负责将磁盘里存储的地图文件加载到ROS中去，rosrun运行一个节点,节点所在的包名字叫map_server，然后这个节点的名字跟它的包同名也叫map_server，（前头的是包名，后面的是节点名）
rosrun map_server map_server mymap.yaml最后是要加载的地图文件格式为yaml的文件，里面的image会告诉map_server，真正保存地图数据的是哪个文件
51.Navigation导航系统
global_planner全局规划器相当于手机里的地图APP，它从map_server地图服务器获取全局地图数据，然后根据输入的导航目的地，生成全局导航路线，发送给local_planner(也就是负责具体运动的脑子，由脑子驱动双腿)也就是机器人的底盘通讯节点控制机器人沿着全局导航路线进行移动。在这过程中需要激光雷达（也就是机器人的眼睛），结合定位器amcl节点来实时给出机器人的自身位置，同时还需要里程计来提升定位的可靠性
在移动中遇到临时的障碍物，但这些障碍物在建图时并不存在，导航时突然出现了；所以机器人在移动过程中还会使用激光雷达、相机等传感器，实时生成机器人周围的障碍物地图（又叫局部地图）。当遇到临时障碍物挡住导肮路线时，会动脑子（也就是局部规划器里的避障算法）绕开障碍物，如果实在绕不过去，就会激活应急机制，机器会采取一系列措施尝试突破。
如果所有手段都尝试了，这时候就会将临时障碍物更新到全局地图里，然后请求全局规划器再规划一条新的路线，最终完成导航流程。
52.move_base节点
步骤：
1.将map_server运行起来，并将导航需要的地图文件加载进去，move_base就会自动来获取这些地图数据
2.机器人会安装两个及以上的激光雷达需要多个传感器节点驱动，机器人仿真模型加载到仿真环境之后，会自动输出sersor topics到相对应的话题，相当于自带了所有的传感器节点，由机器人自己提供
3.里程计节点(odometry source)，和激光雷达一样都可以由仿真机器人的模型自己提供
4.传感器位置（tf)，机器人自带同上
5.定位模块amcl节点，需要我们运行
总结：将图中这三个节点都运行起来，再指定一下导航目标点，让move_base节点控制机器人进行自主导航
环境准备工作
置顶评论如下：
【1】下载源代码
cd ~/catkin_ws/src
git clone https://github.com/6-robot/wpr_simulation.git
若github超时，试试从gitee下载： git clone https://gitee.com/s-robot/wpr_simulation.git
cd wpr_simulation/scripts/ ./ install_for_noetic.sh cd ~/catkin_ws/ catkin_make
cd ~/catkin_ws/src
git clone https://github.com/6-robot/wpb_home.git
若github超时，试试从gitee下载： git clone https://gitee.com/s-robot/wpb_home.git
cd wpb_home/wpb_home_bringup/scripts/ ./ install_for_noetic.sh
cd ~/catkin_ws/ catkin_make
【2】建图指令
roslaunch wpr_simulation
wpb_gmapping.launch
rosrun wpr_simulation keyboard_vel_ctrl
rosrun map_server map_saver -f map
【3】创建软件包
catkin_create_pkg nav_pkg roscpp rospy move_base_msgs actionlib
【4】参考代码
wpr_simulation/launch/
wpb_demo_nav.launch
【5】导航运行
roslaunch wpr_simulation
wpb_stage_robocup.launch
roslaunch wpr_simulation
wpb_demo_nav.launch
导航代码编写
<launch>
<node pkg="move_base" type="move_base" name="move_bse">
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<node pkg="amcl" type="amcl" name="amcl"/>
</launch>运行导航程序
图中的2D Nav Goal按钮就是发送导航目标点的按钮，按住左键不放
添加path显示导航路线
53.全局规划器
全局规划器相当于手机里的地图APP，主要用来生成地图的导航路线
手机里的地图APP，如：高德地图、腾讯地图、百度地图，每个APP都有多种导航模式，如：骑行模式、步行模式、车载模式等等
同样，在move_base中，也提供了多种不同名称的规划器，每种规划器也提供了多种不同风格的导航算法
Dijkstra算法（广度优先算法），从起始点到目标点沿着相邻的栅格扩散出去，扩散到目标点时，就将最近的这条搜索路径作为全图导航路线，输出给具体的控制器执行
A*算法（深度优先算法），从起始点到目标点定向进行扩散搜索，直到到达目标点，它的搜索范围相对较小，相应的计算量也比较小，但得到的路径并不是最短的，相当于通过降低路径生成质量换取较低的算力消耗
Global_planner默认使用Dijkstra算法，如果想要切换成A*算法需要额外加两个参数，Dijkstra算法更优
Carrot_planner—— 就是从起始点向目标点延伸一条路径，遇到障碍物就停止了，代码简单经常被作为自定义规划器的模板进行修改
move_base还支持自己编写全局规划器，它提供了一种Plugin插件接口，只要按照特定的格式，就能把自己的路径规划算法，编写成新的规划器，加载到move_base节点中去使用
这两张图中的路线轨迹都可以编写成自定义的全局规划器；而对于常规的导航任务，使用ROS自带的规划器就足够了
54.AMCL定位算法
定位节点 AMCL（Adaptive Monte Carlo Localization自适应蒙特卡洛定位算法）—— 是一种使用粒子滤波在已知地图中进行定位的算法，它同时使用了里程计和激光雷达数据，具有较强的自我纠错功能
AMCL的粒子数会非常多，默认是100到5000个，在机器人的导航过程中，AMCL就是通过这样不断分裂、比对，再分裂、再比对，淘汰不靠谱的分身，保留匹配效果好的粒子。通过这种适者生存的方式，保证机器人即使在各种累积误差的叠加下，依然能够持续找到相对准确的机器人位置
这是AMCL设置参数的页面
地图到机器人底盘的tf关系，这样RViz就能够在地图上正常的显示机器人的位置
注意：amcl节点切换本体和分身是通过在map到base_footprint的这段tf上，产生跳跃突变来实现的，所有在导航中经常能看到机器人的位置一蹦一蹦的，感觉像掉帧了，这就是amcl节点输出的这段tf 突变产生的结果；而里程计输出的odom到base_footprint这段tf通常是保持连续变化的，不会突然跳变。这个特性在后边生成代价地图的时候还会用到
PoseArray可以显示amcl节点的分身位置，然后为其订阅话题，话题名称为/particlecloud粒子云，
颜色由红改绿的过程，直接黑屏了（查豆包说，本质是RViz的渲染压力过大，加上ROS话题数据传输和缓存的延迟共同导致的。）
评论区说虚拟机手动改颜色值，否则，会卡死
55.代价地图Costmap
代价地图：只要走进这个区域就会撞墙
· 代价地图是对地图里的障碍点进行膨胀得到的，生成代价地图的范围越大，需要的计算就越长
怎么设置代价地图
主要给全局规划器用来生成全图范围的导航路线
然后是这个local_costmap局部代价地图（只在机器人身边的小范围内生成障碍物的掉血区域），这个主要是给局部规划器用来避障用的
注：全局规划地图只有在规划全图导航路线的时候才会用到，而在机器人运动的过程中，只需要关注它身边小范围内的障碍物就足够了
大的全局代价地图，只在规划全局导航路线的时候计算一次，计算的耗时比较长
局部代价地图，计算耗时也很短，使得计算的频率可以做到很高，一秒钟几十次，实时性好，正好可以给局部规划器用来做运动避障
56.代价地图的参数设置
参数文件的归属
后面两个文件它们顶格的第一级名称，是后面这些参数所从属的命名空间，通过命名空间就知道这些参数是为哪个代价地图所设置的，这后面两个参数文件主要是代价地图的计算范围和频率；
而第一个文件内容并没有指定命名空间，有在加载文件时所附带的ns，这个ns就是命名空间name space的首字母缩写，它的作用相当于把ns的内容直接加在了文件的开头，这样就和文件的形式变成一样了
/costmap_common_params.yaml这个参数文件主要影响的是代价地图的形状，用单一文件描述可以保证，在一个文件里修改了参数，能够同时影响两个代价地图的形状，避免分发给全局规划器和局部规划器的代价地图的形状不一致，这会在路径规划和执行的过程中，出现某种潜在的冲突。
这个区域是万万不能让机器人底盘进来的（最危险的区域），绝对不能让导航路线从这个浅蓝色小圆穿过去
外边从粉红渐变到紫色的大圆环，机器人是可以进入的，但越靠近圆心，碰撞障碍点的风险越高
只要对地图中所有的障碍点都套上这两个圆，就变成了完整的代价地图了
为什么用两层？
通过危险程度不同的两层膨胀区域，保证了导航路线在开阔空间里尽可能远离障碍物，然后在狭小的路口，又不至于被膨胀区域堵死，而且还能从最靠中间最安全的路线穿越过去，同时兼顾了安全性和可通过性
第一个参数文件
意思是在这个6米扫描范围内，所有被激光雷达射线穿透的栅格，都会被认为没有障碍物的存在。
这个主要用来清除动态障碍物的残留影子
base_lidar：底盘上的激光雷达
首先是数据消息包的类型
接收消息包的话题名称
marking是指是否将扫描到的障碍物添加到代价地图
clearing是指是否把扫描范围内的障碍物残影清除掉
注：base_lidar是可以随便起名的，只要这两个名称保持一致就行
另外，除了单线激光雷达，还可以在这里加入立体相机和多线雷达作为额外观测来源
比如机器人面对一张桌子时，底盘的单线雷达只能扫描到桌子腿，无法对高处的桌面进行探测，这就可以把头部的三位相机加入代价地图的观测源，使用相机输出的三维点云检测桌面信息，如图所示：
第二个参数文件
2：地图坐标系名称
3：机器人底盘坐标系名称
4：static_map是指是否将map_server发来的地图数据作为初始地图，这里设为true，他就会把map_server发来的地图作为代价地图的基础 ，后面检测到新的动态障碍物再往里添加
这里设为false，那么初始状态就是一张空地图，需要构建一套动态建图机制来为它提供导航依据，有的导航是在没有建图的未知区域，就可以把static_map设置为false，然后启动建图系统，一边建图一边导航
5：地图更新频率，把观测到的障碍物添加到代价地图里的频率，这里为1赫兹，也就是1秒添加1次，（只在生成全局导航路线的时候用到）
6：地图发布频率，也就是发送给RViz进行显示的频率，这个和更新频率保持一致就行
7: transform延迟的容忍值，单位是秒
这里的transform指的是传感器到地图的tf，这个tf包含了三段，一个是传感器到机器人坐标系的tf，还有里程计头tf，再加上amcl节点发布的map到odom的tf，通常这些tf刷新较快，只有在树莓派这种性能比较弱，或者远程登录的机器人网络信号不太好，就有可能造成其中的某一段tf发送出现延迟
注：导航中出现tf的timeout类的报错，要把transform_tolerance的值调大一些
第三个参数文件
2：地图坐标系名称，这里设为odom（复习：amcl定位节点，amcl是通过map到odom这段tf的跳变来切换机器人和分身的位置的，如果以map为基准坐标系，那么当机器人位置跳变的时候传感器检测到的障碍物也会跟着跳变，这个对于全局路径规划来说问题不大，即使导航路线略有偏差，后期执行的时候，还有避障功能做保底。对于局部代价地图来说，障碍物的跳变会引起机器人避障速度的不平滑，使机器人运行变得不平稳，所以局部代价地图的global_frame通常会设置为odom，毕竟里程计输出的tf，正常情况下还是相对比较平滑的）
3：同上
4：这里设置为false，不使用之前SLAM建好的全局地图，而是使用激光雷达当前扫描到的临时地图
5：是指局部代价地图的范围是否跟机器人一起移动，这里设置为true，如图
6：宽
7：高
这两个指的是这个局部代价地图的范围尺寸，单位是米
8：局部代价地图的更新频率，一般设置为激光雷达的扫描频率，这样可以保证激光雷达每扫描一圈，就更新一遍代价地图，这样可以让局部规划器及时掌握周围障碍物的最新情况，对一些危险状况能够迅速做出反应
9：地图发布频率
10;transform的延迟容忍值
57.恢复行为 | Recovery Behaviors
在导航中被动态障碍物围困找不到路线，因此在move_base中设置了一个名为recovery behaviors的应急机制，翻译过来叫作恢复行为，这个机制会在导航陷入停滞时，尝试刷新周围的障碍物信息，然后重新进行全局路径规划，试图让导航过程回到正轨
换一个成功的案例
58.恢复行为的参数设置
联系57讲，恢复行为的默认流程主要有两种行为类型：
一种重置行为代码一样参数不一样，对应的类型叫作ClearCostmapRecovery
第二种旋转清除行为，对应名称叫作RotateRecovery
这个行为不常用，它会在几个点来回走动，一不小心就容易碰撞障碍物，不如原地旋转来的安全
名字随便写
创建了新的行为类型，也可以写在这里
通过上面的列表可以创建一个新的恢复流程，取代ROS原来的默认流程
给恢复行为设置参数
其中reset_distance是清除的范围，layer_names是指要清除哪一层地图这里设为obstacle_layer(也就是障碍层)
分层浅讲一下
图1
在map_server中加载地图文件，在Rviz里，如图1，在分层结构里属于最下面Static静态地图，如果场景里临时多了一根柱子，不属于静态地图，属于第二层Obstacles障碍物地图，如图2
图2
第三层是把下面两层叠加起来后，对所有障碍栅格的边缘设置一圈掉血区域，这一层叫作Inflation膨胀地图，如图3
图3
最后最上边这个Master，如图4，就是咱们在RViz里看到的由下边三层合并而成的最终代价地图
图4
补充完毕，回到obstacle_layer指的就是补充里提到的Obstacles障碍物地图，它是由激光雷达扫描得到的动态障碍物，重置行为只清除它
实际上在清除激光雷达过去探测到的障碍物残影
而新检测到的障碍物信息，会立刻刷新进来，叠加上map_server发来的静态地图，作为重新路径规划的地图依据，这就是重置行为的工作内容
障碍物清除范围如何划定？
以机器人为中心构建一个正方形，单位：米
默认参数下是清除这个正方形以外的障碍层信息
特别说明：clear_costmap_recovery重置行为它的layer_names参数的默认值是obstacles，在它的代码中也是这样；但在costmap_2d的代码中，默认的障碍物层名字是obstacle_layer
如果想让这个重置行为能够正常的清除障碍物，需要名称保持一致
59.局部规划器 | Local Planner
部分开源的局部规划器
第一个内部实现是DWA算法，但代码质量不高
第二个代码可读性更好，运行效率更高（相比于第一个，一般选第二个）
第三个和第四个，实现思路很相似，TEB Planner加入了时间因素的考虑，同时还提供了代价地图的优化插件，运动平滑性和执行效率更高（2选1，会使用TEB Planner）
60.DWA规划器
DWA（Dynamic Window Approach动态窗口方案）—— 机器人的运动控制中线路轨迹和运动速度的可选择的空间，比如全局规划器已经规划出一条导航路线，周围有一些障碍物，机器人会走出一些轨迹，其中会撞到障碍物的轨迹会被抛弃掉，剩下的这些可能的轨迹以及在轨迹上可能的速度变化，就是所谓的窗口，而DWA算法就是生成这样的一系列轨迹和速度的方案，从中挑选最合适的一条轨迹然后按照它来驱动机器人运动
这里面涉及两个内容：生成轨迹和挑选轨迹
生成轨迹
是以机器人当前的运动速度为基础规划未来一段时间机器人的运动状态和移动线路
给图中这两个量取不同的值，然后组合起来，就能让机器人走出不一样的弧线，DWA的轨迹生成就是用这种方法实现的，其中对速度分量的取值操作在这里称为采样。
速度的取值会综合考虑底盘加速度的限制与障碍车保持有效的刹车距离，以及尽快运动到轨迹终点这三个因素
挑选轨迹
三个标准
1.运动轨迹和全局导航路线的贴合程度
2.轨迹末端和目标点的距离
3.轨迹路线和障碍物之间的距离
小结：综合考虑过程、目标和风险这三个因素，从一堆轨迹中挑出最符合这三个标准的作为最终的执行路线，从以上描述中可得，DWA算法规划的运动轨迹以平滑的弧线为主，是否贴合全局路线，只是它考虑的因素之一不是全部，所以大概率不会沿着全局路线去行走
<launch>
<node pkg="move_base" type="move_base" name="move_bse">
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="base_local_planner" value="dwa_localplanner/DWAPlannerROS" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/dwa_local_planner_params.yaml" command="load"/>
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<node pkg="amcl" type="amcl" name="amcl"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_pkg)/rviz/nav.rviz"/>
</launch> 
白色的是DWA生成的备选轨迹，绿色的这条是挑选出来的执行路线，在导航过程中轨迹会不断的生成，直到机器人到达终点
DWA可调节参数
DWAPlannerROS:
# 速度参数
max_vel_x: 0.3 # 最大x方向速度
min_vel_x: -0.05 # 最小x方向速度（设置负数将会允许倒车）
max_vel_y: 0.0 # 差分驱动机器人的最大y方向速度为 0.0
min_vel_y: 0.0 # 差分驱动机器人的最小y方向速度为 0.0
max_vel_trans: 0.3 # 最大平移速度
min_vel_trans: 0.01 # 最小平移速度（建议不要设置为 0.0 ）
trans_stopped_vel: 0.1 # 当平移速度小于这个值，就让机器人停止
acc_lim_trans: 2.5 # 最大平移加速度
acc_lim_x: 2.5 # x方向的最大加速度上限
acc_lim_y: 0.0 # y方向的加速度上限（差分驱动机器人应该设置为 0.0 ）
max_vel_theta: 1.0 # 最大旋转速度，略小于基座的功能
min_vel_theta: -0.01 # 当平移速度可以忽略时的最小角速度
theta_stopped_vel: 0.1 # 当旋转速度小于这个值，就让机器人停止
acc_lim_theta: 6.0 # 旋转的加速度上限
# 目标容差参数
yaw_goal_tolerance: 0.1 # 目标航向容差
xy_goal_tolerance: 0.05 # 目标xy容差
latch_xy_goal_tolerance: false # 到达目标容差范围后，停止移动，只旋转调整航向
# 向前模拟参数
sim_time: 1.7 # 模拟时间，默认值 1.7
vx_samples: 3 # x方向速度采样数，默认值 3
vy_samples: 1 # 差分驱动机器人y方向速度采样数，只有一个样本
vtheta_samples: 20 # 旋转速度采样数，默认值 20
# 轨迹评分参数
path_distance_bias: 32.0 # 靠近全局路径的权重，默认值 32.0
goal_distance_bias: 24.0 # 接近导航目标点的权重，默认值 24.0
occdist_scale: 0.01 # 控制器避障的权重，默认值 0.01
forward_point_distance: 0.325 # 从机器人到评分点的位置，默认值 0.325
stop_time_buffer: 0.2 # 在碰撞前机器人必须停止的时间长度，留出缓冲空间，默认值 0.2
scaling_speed: 0.25 # 缩放机器人速度的绝对值，默认值 0.25
max_scaling_factor: 0.2 # 机器人足迹在高速时能缩放的最大系数，默认值 0.2
# 防振动参数
oscillation_reset_dist: 1.05 # 重置振动标志前需要行进的距离，默认值 0.05
# 辅助调试选项
publish_traj_pc : true # 是否在 RViz 里发布轨迹
publish_cost_grid_pc: true # 是否在 RViz 里发布代价网格
global_frame_id: odom # 基础坐标系
# 差分驱动机器人配置
holonomic_robot: false # 是否全向移动机器人
如上代码：
速度参数
这些需要按照机器人的真实性能来设置
目标容差参数
对机器人到达轨迹终点的判定条件
向前模拟参数
决定了生成的轨迹长度和数量
轨迹评分参数
这个主要影响的是最终挑选的执行路线是哪一条
防振荡运动参数
辅助调试选项
在RViz里显示备选轨迹和障碍物代价图的选项，DWA的基础坐标系
（局部规划器一般设置为里程计坐标系）
差分驱动机器人配置
切换全向底盘和差分驱动底盘的参数，这里为了兼容差分底盘，设置成了false
每次修改完文件里的参数，都要重新运行一遍launch文件，为了提高效率，在运行DWA导航的时候，再额外启动一个工具
点开move_base，点击DWAPlanner
直接改，在RViz里就能实时观察调整的效果了，通过rqt_reconfigure调出完美的参数之后，可以把调整后的结果写到参数文件里保存，下次启动直接加载这套完美参数
总结：在DWA的众多参数中，执行路线的挑选标准（# 轨迹评分参数）最为关键，最重要的是前三个参数，分别是轨迹贴合全局路径的权重、轨迹的终点和全局路径在局部地图的最远点的距离的权重，还有轨迹曲线和障碍物的最近距离的权重。想让其中的某个因素的评价优先级提升，就增大其对应的权重值
补充：最小x方向速度，X方向是机器人的前后运动方向，这个最小速度，如果设为大于等于0的值，机器人就只能前进不能倒退；设为负值时，就会往后倒。但这种倒车动作只适合激光雷达能扫到后方的机器人，如果机器人后方没有雷达的视野，则需要慎重考虑，否则容易撞到身后或者侧面的障碍物
61.TEB规划器
TEB（Timed Elastic Band时间弹力带）
算法原理：首先是全局规划器发来的全图导航路线，TEB会在这条全图路线上选取一段进行优化，在起点和终点之间拉一条橡皮筋弹力带，然后，让这个全局路径对弹力带产生吸引力，拉动它产生形变，假如身边有障碍物，这些障碍物会对弹力带产生排斥力，推动它产生形变，全局路径的吸引力和障碍物的排斥力，同时作用在这条弹力带上，就得到TEB生成的局部路径。 TEB会在这个弹力带上预测，未来连续的几个时间单位，机器人会运动到哪个位置
如果存在多条路线时，就会按照相同时间内，机器人移动的距离远近来挑选运动最快的路线
TEB的安装
不属于ROS官方的导航组件，所以需要另外进行安装
安装时出现点问题，检查虚拟机网络连接
ping 8.8.8.8· 如果能通，说明网络是好的，只是DNS有问题
· 如果不通，检查VMware的网络设置（NAT/桥接），或重启虚拟机网络
ping 8.8.8.8
PING 8.8.8.8 (8.8.8.8) 56(84) bytes of data.
64 bytes from 8.8.8.8: icmp_seq=1 ttl=128 time=214 ms
64 bytes from 8.8.8.8: icmp_seq=2 ttl=128 time=309 ms
64 bytes from 8.8.8.8: icmp_seq=3 ttl=128 time=296 ms
64 bytes from 8.8.8.8: icmp_seq=4 ttl=128 time=219 ms
64 bytes from 8.8.8.8: icmp_seq=5 ttl=128 time=275 ms
64 bytes from 8.8.8.8: icmp_seq=6 ttl=128 time=299 ms
64 bytes from 8.8.8.8: icmp_seq=7 ttl=128 time=303 ms
64 bytes from 8.8.8.8: icmp_seq=8 ttl=128 time=241 ms
64 bytes from 8.8.8.8: icmp_seq=9 ttl=128 time=263 ms
64 bytes from 8.8.8.8: icmp_seq=10 ttl=128 time=285 ms
64 bytes from 8.8.8.8: icmp_seq=11 ttl=128 time=307 ms
64 bytes from 8.8.8.8: icmp_seq=12 ttl=128 time=330 ms
输出这样就可以了
代码将60讲DWA的部分改为TEB即可
<launch>
<node pkg="move_base" type="move_base" name="move_bse">
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/teb_local_planner_params.yaml" command="load"/>
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<node pkg="amcl" type="amcl" name="amcl"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_pkg)/rviz/nav.rviz"/>
</launch>添加TEB的显示项目，选择PoseArray，然后导航一下看看
可以看到这个绿色的线条，就是TEB规划出来的局部导航路径，在上面有一长串红色箭头，这些就是TEB预测的未来连续几个时间单位机器人将会到达的位置，这条局部导航路径会不断变化，引导机器人到达全局路线的终点，在终点附近机器人会调整自身的朝向以符合咱们刚才设置的导航目标点的目标姿态
补充：TEB在调整机器人朝向的时候，会倾向于使用这种走弧线倒车的方式，而不是直接原地旋转，哪怕转弯半径设为0也是如此。如果机器人后方没有雷达视野，那么就需要谨慎选择TEB规划器，否则在倒车的过程中，极容易与背面和侧面的障碍物发生碰撞
TEB可调节参数
TebLocalPlannerROS:
odom_topic: odom
# 策略相关
teb_autosize: True # 是否允许改变轨迹的时域长度，也就是改变 dt_ref
dt_ref: 0.5 # 路径上的两个相邻姿态的默认距离
dt_hysteresis: 0.1 # 允许改变的时域解析度的浮动范围
global_plan_overwrite_orientation: True # 是否修正全局路径中的临时局部路径点的朝向
max_global_plan_lookahead_dist: 2.0 # 最大向前看距离
feasibility_check_no_poses: 2 #在判断生成的轨迹是否冲突时使用，此时设置为2，即从轨迹起点开始逐个检查轨迹上的2个点，若2个点均不发生碰撞，则认为本次轨迹有效。
# 运动相关
max_vel_x: 0.4 # 最大速度
max_vel_x_backwards: 0.2 # 最大倒车速度，设置为0或者负数将导致错误。减少倒车应该修改倒车权重，不改这里。
max_vel_theta: 1.0 # 最大转向角速度，跟 min_turning_radius 相关 (r = v / omega)
acc_lim_x: 0.5 # 最大线加速度
acc_lim_theta: 1.0 # 最大角加速度
# ********************** 转弯半径相关 ********************
min_turning_radius: 0.0 # 小转弯半径。如果设为 0，表示可以原地转弯。
wheelbase: 0.31 # 只有在 cmd_angle_instead_rotvel为true时才有效
cmd_angle_instead_rotvel: False # 是否将收到的角速度消息转换为操作上的角度变化。设置成 True 时，话题 vel_msg.angular.z 内的数据是转轴角度。
# ********************************************************************
# 车体轮廓
footprint_model: # types可选项: "point", "circular", "two_circles", "line", "polygon"
type: "circular"
# 对 type "circular" 有效的参数：
radius: 0.17
# 对 type "line" 有效的参数：
line_start: [0.0, 0.0]
line_end: [0.35, 0.0]
# 对 type "two_circles" 有效的参数：
front_offset: 0.35
front_radius: 0.35
rear_offset: 0.35
rear_radius: 0.35
# 对 type "polygon" 有效的参数：
vertices: [ [0.35, 0.0], [-0.2, -0.25], [0.2, -0.25]]
# 到达目标点的判断容差
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
# 障碍物相关
min_obstacle_dist: 0.1 # 与障碍物的最小间距
inflation_dist: 0.4 # 障碍物膨胀距离
include_costmap_obstacles: True # 是否检测动态障碍物
costmap_obstacles_behind_robot_dist: 1.0 # 身后多远距离内障碍物加入检测范围
obstacle_poses_affected: 25 # 障碍物对附近多少个关键点产生影响
costmap_converter_plugin: "" # costmap_converter 插件名称，这里不使用
# 路径优化相关
no_inner_iterations: 3 # 图优化optimizer的迭代次数
no_outer_iterations: 3 # 外循环迭代次数
penalty_epsilon: 0.1 # 为所有的惩罚项增加一个小的安全余量
weight_max_vel_x: 2 # 平移速度的优化权重
weight_max_vel_theta: 1 # 角速度的优化权重
weight_acc_lim_x: 1 # 平移加速度的优化权重
weight_acc_lim_theta: 1 # 角加速度的优化重量
weight_kinematics_nh: 1000 # 非完整运动学的优化权重
weight_kinematics_forward_drive: 1 # 往前移动的权重
weight_optimaltime: 1 # 耗时权重
weight_obstacle: 50 # 与障碍物保持距离的权重
# 多线规划
enable_homotopy_class_planning: True # 激活多线规划
enable_multithreading: True # 多线程计算
max_number_classes: 2 # 规划的路径线数上限
selection_cost_hysteresis: 1.0 # 路径轨迹入选的评价上限
selection_obst_cost_scale: 1.0 # 障碍物评价在入选标准中的缩放倍率
selection_alternative_time_cost: False # 时间成本是否要进行平方计算
roadmap_graph_no_samples: 15 # 为创建 roadmap graph 而生成的样本数
roadmap_graph_area_width: 5 # 关键点采样的宽度，单位为米。除了在参数文件里修改数值，和DWA一样也有更便捷的方式
同60讲
参数里面最小转弯半径就是影响机器人倒车路线弧长的参数
将新参数填写到TEB保存，下次启动TEB，就能直接加载新的参数了
注：里面包含一个costmap_converter插件，默认是没有启用的，启用这个插件可以获得更好的规划性能和避障效果，简单应用使用默认参数，深入了解参考详细文献
62.导航的Action编程接口
机器人需要编写程序代码来实现导航功能，需要使用Navigation的导航接口，导航接口有好几个，
ROS官方推荐使用Action接口来实现这个功能，Action是ROS里使用的一种通讯方式
Action和Topic话题通讯不同点：
在Topic话题通讯中，消息包的传输是单向的，只能从发布者节点发送到订阅者节点
在Action通讯中，消息包的传输是双向的，将通讯双方分为Server节点和 Client节点，如Navigation系统中的move_base就相当于一个Server，咱们要白那些一个 Client节点，向move_base这个Server发送导航请求，这个请求包含了导航目标点的坐标值和期望的姿态，move_base接收到这个导航请求后，按照要求正常完成导航，与Topic话题通讯不同，Action通讯是可以持续不断的返回信息的；如：move_base就会向咱们编写的Client节点，实时反馈导航进程，在导航结束时，可以给Client节点返回导航完成的信息；如果导航失败了，也会反应相应的失败消息，这种一次请求就能获得连续返回信息的方式，就是Action通讯和Topic话题通讯的最大区别
63.坐标导航的C++编程实现
64.坐标导航的Python编程实现
编写节点文件
#!/usr/bin/env python3
# coding=utf-8
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
if __name__ == "__main__":
rospy.init_node("nav_client")
# 生成一个导航请求客户端
ac = actionlib.SimpleActionClient('move_base',MoveBaseAction)
# 等待服务器端启动
ac.wait_for_server()
# 构建目标航点消息
goal = MoveBaseGoal()
# 目标航点的参考坐标系
goal.target_pose.header.frame_id="map"
# 目标航点在参考坐标系里的三维数值
goal.target_pose.pose.position.x = -3.0
goal.target_pose.pose.position.y = 2.0
goal.target_pose.pose.position.z = 0.0
# 目标航点在参考坐标系里的朝向信息
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
# 发送目标航点去执行
ac.send_goal(goal)
rospy.loginfo("开始导航…")
# 等待 move_base 完成导航
ac.wait_for_result()
# 获取导航结果
if ac.get_state() == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("导航成功！")
else:
rospy.loginfo("导航失败…")
增加可执行权限
liyou@ubuntu:~$ cd ~/catkin_ws/src/nav_pkg/scripts/
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ ls
nav_client.py
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ chmod +x nav_client.py
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ ls
nav_client.py
运行节点文件
不可直接将demo_nav_client.py直接复制过去，否则第三条指令无法执行，原因是缺少ROS节点初始化的关键步骤，详细代码请看上文代码
65.航点导航插件介绍
开源的ROS导航插件，能够在地图上设置多个目标点，然后可以通过编写节点代码来指定机器人导航去往哪个航点
从Git.Hub上下载，搜索waterplus_map_tools
点Add Waypoint，左键转圈
这四个标牌就是咱们设置的航点位置，箭头方向就是航点姿态的朝向，头上的黄色字符就是航点名称，咱们可以把这些航点像地图一样保存到文件里，后边运行导航的时候就可以直接加载进来使用了
保存到waypoints.xml，用文本编辑器打开，可以看到这里保存的航点信息
Name标签就是航点标牌上的黄色文字
Position的x y z三个数值，就是航点在地图里的坐标值
Orientation就是航点姿态朝向的四元数
使用这些航点信息进行导航
重置初始位置，运行一个例子程序，让它自动导航到1号航点
roslaunch wpr_simulation wpb_map_tool.launch
机器人到达位置后会调整方向，转到航点标牌箭头所指的方向
调整导航目标点的位置和朝向，在终端里再次运行添加航点的launch文件
roslaunch waterplus_map_tools add_waypoint_simulation.launch将1号航点换位置保存航点信息，再次执行航点保存指令
rosrun waterplus_map_tools wp_saver接着运行之前航点导航的例子程序
66.航点导航插件的集成和启动
启动wp_navi_server节点就可调用move_base的导航功能，wp_navi_server节点前面有wp_manager节点（航点管理员节点），waypoints.xml文件用wp_manager节点去加载，wp_navi_server节点会与wp_manager节点建立通讯，从waypoints.xml文件里获得航点的坐标和姿态信息
wp_navi_server节点订阅一个话题用来接收导航的目标航点名称，同时它还发布一个话题用来报告导航执行的结果，咱们成为demo_map_tool的发布者就可以给wp_navi_server节点发送要进行导航的目标航点名称，然后再订阅导航结果话题，就可获得导航执行的结果
在nav.launch中添加一个wp_navi_server节点的启动项和一个wp_manager节点的启动项，以及修改一下RViz的显示配置文件，代码如下：
<launch>
<node pkg="move_base" type="move_base" name="move_bse">
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<rosparam file="$(find wpb_home_tutorials)/nav_lidar/teb_local_planner_params.yaml" command="load"/>
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<node pkg="amcl" type="amcl" name="amcl"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_pkg)/rviz/map_tool.rviz"/>
<node pkg="waterplus_map_tools" type="wp_navi_server" name="wp_navi_server" output="screen"/>
<node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen"/>
</launch>执行程序指令，启动仿真环境——启动航点导航系统——测试用的例子程序
67.航点导航功能的C++实现
68.航点导航功能的Python实现
编写节点文件
#!/usr/bin/env python3
# coding=utf-8
import rospy
from std_msgs.msg import String
# 导航结果回调函数
def NavResultCallback(msg):
rospy.loginfo("导航结果 = %s",msg.data)
if __name__ == "__main__":
rospy.init_node("wp_map")
# 发布航点名称话题
navi_pub = rospy.Publisher("/waterplus/navi_waypoint",String,queue_size=10)
# 订阅导航结果话题
result_sub = rospy.Subscriber("/waterplus/navi_result",String,NavResultCallback ,queue_size=10)
# 延时1秒钟，让后台的话题发布操作能够完成
rospy.sleep(1)
# 构建航点名称消息包
navi_msg = String()
navi_msg.data = '1'
# 发送航点名称消息包
navi_pub.publish(navi_msg)
# 构建循环让程序别退出，等待导航结果
rospy.spin()增加可执行权限
liyou@ubuntu:~$ cd ~/catkin_ws/src/nav_pkg/scripts/
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ ls
nav_client.py wp_node.py
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ chmod +x wp_node.py
liyou@ubuntu:~/catkin_ws/src/nav_pkg/scripts$ ls
nav_client.py wp_node.py
运行节点文件
启动仿真环境——启动航点导航系统——运行刚才编写的wp_node.py
让机器人沿着1 2 3 4四个航点连续导航一遍代码
#!/usr/bin/env python
# coding: utf-8
import rospy
from std_msgs.msg import String
import queue
# 全局变量
navi_msg = String()
target_points = queue.Queue()
def NavResultCallback(msg):
rospy.logwarn("到达导航目标点 %s", msg.data)
# 如果队列还有目标点，就发送下一个
if not target_points.empty():
navi_msg.data = target_points.get()
navi_pub.publish(navi_msg)
if __name__ == "__main__":
try:
rospy.init_node("wp_node")
# 发布导航目标点话题
navi_pub = rospy.Publisher("waterplus/navi_waypoint", String, queue_size=10)
# 订阅导航结果话题
res_sub = rospy.Subscriber("waterplus/navi_result", String, NavResultCallback, queue_size=10)
rospy.sleep(1) # 等待连接建立
# 把四个航点加入队列
target_points.put("1")
target_points.put("2")
target_points.put("3")
target_points.put("4")
# 发送第一个航点，启动整个流程
navi_msg.data = target_points.get()
navi_pub.publish(navi_msg)
rospy.spin()
except rospy.ROSInterruptException:
pass69.ROS中的相机话题
除了激光雷达，机器人还可以用相机来感知世界，有不同类型的相机
普通的彩色相机
具有立体感知能力的RGB-D相机
与激光雷达和IMU一样，直接通过订阅话题就能获取数据
相机数据的话题非常多
在ROS中，彩色图像通常会在三个话题中进行发布
在相机硬件的发展过程中，为了降低成本相机的感光芯片一次只能检测每个像素的光强，也就是它的原始输出——黑白图像
后来在感光芯片上覆盖一层红绿蓝RGB栅格滤镜，光线透过滤镜后，感光芯片上的每个像素，就只能感知其中一种颜色的强度
右边彩色图像的每个像素都由红绿蓝RGB三种颜色组成；
左图这个原始数据里，每个像素只有一种颜色，每个像素周围总有一个像素检测到缺失的两种颜色之一
可以通过插值的方法猜出咱们这个像素所缺失的两种颜色的大概数值，通过这种跟邻居借颜色的方法就可以将原始的Bayer阵列图像里每个像素缺失的另外两个颜色数值都补齐 了
图中第三个话题，rect(rectify的缩写)也就是相机的畸变矫正，在机器人的视觉图像里，也存在类似的畸变矫正，进行rectify畸变矫正就可获得一个正常比例的图像，这样能够显著提高目标识别的成功率，在视觉SLAM中矫正操作更为重要，会直接影响特征点的匹配效果
最后一个话题可以自行进行畸变矫正
总结：正常使用，只需要订阅/image_color_rect话题，获取矫正后的彩色图像；/image_color_rect话题中的图像消息包的发送频率和相机帧率有关，常见的多是30fps的倍数，这个可以使用rostopic hz指令查看
另外这个话题的消息格式是sensor_msgs/Image类型，在ROS Index中可以查到这个消息类型的数据格式，但是一般使用的时候，都会直接将其转换为OpenCV的Mat类型，使用OpenCV的函数来进行图像处理，所以图像数据在ROS中的消息格式可以不用具体了解，只要知道转换方法就行
70.相机图像获取的C++实现
71.颜色目标识别与定位的C++实现
72.ROS颜色目标跟随的C++实现
73.ROS人脸检测的C++实现
74.ROS相机图像获取的Python实现
创建新的软件包
liyou@ubuntu:~/catkin_ws/src$ catkin_create_pkg image_pkg_new rospy sensor_msgs cv_bridge
Created file image_pkg_new/package.xml
Created file image_pkg_new/CMakeLists.txt
Created folder image_pkg_new/src
Successfully created files in /home/liyou/catkin_ws/src/image_pkg_new. Please adjust the values in package.xml.
编写节点文件
#!/usr/bin/env python3
# coding=utf-8
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
# 彩色图像回调函数
def Cam_RGB_Callback(msg):
bridge = CvBridge()
try:
cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
rospy.logerr("格式转换错误: %s", e)
return
# 弹出窗口显示图片
cv2.imshow("RGB", cv_image)
cv2.waitKey(1)
# 主函数
if __name__ == "__main__":
rospy.init_node("demo_cv_image")
# 订阅机器人视觉传感器Kinect2的图像话题
rgb_sub = rospy.Subscriber("/kinect2/qhd/image_color_rect",Image,Cam_RGB_Callback,queue_size=10)
rospy.spin()
增加可执行权限
iyou@ubuntu:~/catkin_ws/src$ cd ~/catkin_ws/src/image_pkg_new/scripts/
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ ls
image_node.py
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ chmod +x image_node.py
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ ls
image_node.py
因为是新的软件包，返回工作空间目录进行catkin_make编译
运行节点文件
运行第三条指令可以看到球滚起来了，RGB窗口可以看到咱们从相机话题中获取的图像是在实时更新的
75.ROS颜色目标识别与定位的Python实现
颜色空间转换 RGB->HSV
所有的颜色都可以使用不同数值的三原色进行描述，就构成了RGB颜色空间，所有的颜色都可以表示成这个正方体空间中的一个点
但实际应用中直接对RGB进行数值分割会受到光照条件的影响，不同光照下，颜色的饱和度和亮度会有所不同，导致RGB数值产生较大变化，直接对RGB进行数值分割的方法就会失效。
一般是将颜色的描述从RGB空间转换到HSV空间去进行数值分割
HSV（Hue Saturation Value ）
色调H：颜色点围绕圆锥中心轴旋转的角度
饱和度S：颜色点与圆锥中心轴的距离
亮度V：颜色点与圆锥顶点的距离
方便切割会把圆锥底面拿出来，相当于一个颜色的极坐标系，极点为圆的中心点，沿着红色半径切开
H轴设置最值就能将某种特定颜色分割出来
S轴越往上值越大，颜色越鲜艳；反之，越往下值越小，颜色越暗淡，用最大值和最小值限定颜色的鲜艳程度
V轴把光照条件对颜色的影响给剥离出来了，给它也设置一组最值
二值化 分割提取目标物
通过这三组最值，将某种纯度的颜色从颜色空间里分割出来，前面的颜色空间转换在OpenCV里有现成的函数，不需要咱们进行计算转换。通过转换颜色空间设置分割阈值，就能把具有某种颜色特征的像素，从相机图像中提取出来就得到一张黑白照片
白色区域为符合阈值特征的像素点集合，黑色区域为其他像素，这就是图像特征二值化
计算目标物的质点坐标
假如把这些符号阈值条件的像素点集合认定为某个物体的形状，那么通过对这些像素点进行坐标值的均值计算，就能得到这个物体的质心。对圆形物体质心就是中心，就可实现对图像中具备某种颜色特征的物体进行空间定位的效果
编写节点文件
#!/usr/bin/env python3
# coding=utf-8
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
hue_min = 10
hue_max = 40
satu_min = 90
satu_max = 255
val_min = 1
val_max = 255
def Cam_RGB_Callback(msg):
global hue_min, hue_max, satu_min, satu_max, val_min, val_max
bridge = CvBridge()
try:
cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
rospy.logerr("格式转换错误: %s", e)
return
# 将RGB图片转换成HSV
hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
# 在HSV空间做直方图均衡化
h, s, v = cv2.split(hsv_image)
v = cv2.equalizeHist(v)
hsv_image = cv2.merge([h, s, v])
# 使用Hue,Saturation和Value的阈值范围对图像进行二值化
th_image = cv2.inRange(hsv_image, (hue_min, satu_min, val_min), (hue_max, satu_max, val_max))
# 开操作 (去除一些噪点)
element = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
th_image = cv2.morphologyEx(th_image, cv2.MORPH_OPEN, element)
# 闭操作 (连接一些连通域)
th_image = cv2.morphologyEx(th_image, cv2.MORPH_CLOSE, element)
# 遍历二值化后的图像数据
target_x, target_y, pix_count = 0, 0, 0
image_height, image_width = th_image.shape[:2]
for y in range(image_height):
for x in range(image_width):
if th_image[y, x] == 255:
target_x += x
target_y += y
pix_count += 1
if pix_count > 0:
target_x //= pix_count
target_y //= pix_count
print(f"颜色质心坐标( {target_x} , {target_y} ) 点数 = {pix_count}")
# 画坐标
cv2.line(cv_image, (target_x-10, target_y), (target_x+10, target_y), (255, 0, 0), 2)
cv2.line(cv_image, (target_x, target_y-10), (target_x, target_y+10), (255, 0, 0), 2)
else:
print("目标颜色消失...")
# 显示处理结果
cv2.imshow("RGB", cv_image)
cv2.imshow("HSV", hsv_image)
cv2.imshow("Result", th_image)
cv2.waitKey(5)
def nothing(x):
pass
if __name__ == "__main__":
rospy.init_node("demo_cv_hsv", anonymous=True)
rgb_sub = rospy.Subscriber("kinect2/qhd/image_color_rect", Image, Cam_RGB_Callback, queue_size=1)
cv2.namedWindow("Threshold")
cv2.createTrackbar("hue_min", "Threshold", hue_min, 179, nothing)
cv2.createTrackbar("hue_max", "Threshold", hue_max, 179, nothing)
cv2.createTrackbar("satu_min", "Threshold", satu_min, 255, nothing)
cv2.createTrackbar("satu_max", "Threshold", satu_max, 255, nothing)
cv2.createTrackbar("val_min", "Threshold", val_min, 255, nothing)
cv2.createTrackbar("val_max", "Threshold", val_max, 255, nothing)
cv2.namedWindow("RGB")
cv2.namedWindow("HSV")
cv2.namedWindow("Result")
rate = rospy.Rate(30)
while not rospy.is_shutdown():
hue_min = cv2.getTrackbarPos("hue_min", "Threshold")
hue_max = cv2.getTrackbarPos("hue_max", "Threshold")
satu_min = cv2.getTrackbarPos("satu_min", "Threshold")
satu_max = cv2.getTrackbarPos("satu_max", "Threshold")
val_min = cv2.getTrackbarPos("val_min", "Threshold")
val_max = cv2.getTrackbarPos("val_max", "Threshold")
rate.sleep()
cv2.destroyAllWindows()增加可执行权限
liyou@ubuntu:~$ cd ~/catkin_ws/src/image_pkg_new/scripts/
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ ls
hsv_node.py image_node.py
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ chmod +x hsv_node.py
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$ ls
hsv_node.py image_node.py
liyou@ubuntu:~/catkin_ws/src/image_pkg_new/scripts$
76.ROS颜色目标跟随的Python实现
77.ROS人脸检测的Python实现
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献2条内容
所有评论(0)