ubuntu—ROS入门

kangjianing1

434人浏览 · 2026-04-17 19:46:57

kangjianing1 · 2026-04-17 19:46:57 发布

Ubuntu ROS 入门教程

本文档根据 B 站「ROS 入门」合集视频笔记整理，部分原始图片已用文字、表格或代码补全。

一、基础命令与环境准备

1.1 Ubuntu 常用命令

命令	说明	示例
`ls`	列出当前目录下的文件和文件夹	`ls -la`
`mkdir`	创建新目录	`mkdir catkin_ws`
`cd`	切换当前目录	`cd ~/catkin_ws`
`cd ..`	返回上一级目录	`cd ..`
`cd ~`	回到当前用户主目录	`cd ~`
`ls -a`	显示所有文件（包括隐藏文件）	`ls -a`
`gedit`	文本编辑器	`gedit ~/.bashrc`
`echo`	在终端输出文本	`echo "Hello ROS"`
`source`	执行脚本文件，加载环境变量	`source ~/.bashrc`
`sudo`	以管理员权限执行命令	`sudo apt update`

说明：~/.bashrc 是 Bash 终端的配置脚本，每次打开新终端时会自动执行其中的命令。

1.2 ROS 安装步骤

ROS 官方网站：www.ros.org

安装步骤概览：

进入 ROS 官网 → noetic → 镜像下载
设置密钥（在终端一行一行运行命令，或运行视频第一条指令）
安装 → 选择 full install 指令
环境设置 bash（在终端一行一行运行命令）
初始化 rosdep

安装 Python 软件包管理工具 pip：

# 安装pip
sudo apt-get install python3-pip

# 安装配置修改工具
sudo pip3 install 6-rosdep

# 运行配置工具
sudo 6-rosdep

# 初始化rosdep
sudo rosdep init

# 更新rosdep
rosdep update

1.3 APT 软件包安装

通过 ros.org 安装软件包

终端执行：

# 安装软件包（以 rqt_robot_steering 为例）
sudo apt install ros-noetic-rqt_robot_steering

# 新终端启动ROS核心
roscore

# 旧终端运行节点
rosrun rqt_robot_steering rqt_robot_steering

仿真小乌龟下载安装：

# 安装小乌龟仿真包
sudo apt install ros-noetic-turtlesim

# 运行小乌龟节点
rosrun turtlesim turtlesim_node

通过 GitHub 源码安装

# 创建工作空间
mkdir catkin_ws
cd catkin_ws
mkdir src
cd src

# 安装git工具
sudo apt install git

# 克隆仓库（GitHub 搜索 wpr_simulation，复制 code 里的网址）
git clone <仓库网址>

scripts目录用于放置脚本文件和python程序

编译与运行

右键空白区域在终端打开，安装编译需要的依赖库：

# 安装依赖库（在 wpr_simulation/scripts 目录下）
./install_for_noetic.sh

# 编译工作空间
cd ~/catkin_ws
catkin_make

# 载入工作空间环境设置
source ~/catkin_ws/devel/setup.bash

# 运行编译好的ROS程序
roslaunch wpr_simulation wpb_simple.launch

仿真环境Gazebo
编写的源代码会放到 catkin_ws 工作空间

设置 .bashrc 自动加载环境

将 source 指令添加到 .bashrc 脚本，这样每次打开终端会自动加载环境：

# 编辑 .bashrc
gedit ~/.bashrc

# 在文件末尾添加（替换为你的工作空间路径）
source ~/catkin_ws/devel/setup.bash

# 保存退出

ROS 软件包的源代码

从 ros.org 找到软件包，复制 Checkout URL 中的 GitHub 网址，在终端中克隆：

cd ~/catkin_ws/src
git clone <仓库网址>

小海龟版本需切换回 ROS1：

# 新终端
cd ros_tutorials/
git checkout noetic devel

编译并测试：

# 编译
cd ~/catkin_ws/
catkin_make

# 启动 ROS 核心
roscore

# 新终端运行小乌龟
rosrun turtlesim turtlesim_node

二、开发工具配置

2.1 VS Code 安装与配置

.deb 包下载好后，在下载目录右键在终端打开：

# 安装VS Code（Tab键补全包名）
sudo dpkg -i code_xxx.deb

工作空间导入：

打开vscode—>添加工作空间目录—>catkin_ws—>src确认

编译快捷键设置

ctrl+shift+B
上方工作栏选择 catkin_make:build 右侧齿轮
左侧文件栏 .vs code—>taks.json
“groop”:{“kind”:“build”,“isDefault”:true},然后保存
再ctrl+shift+B 就会直接用catkin_make进行编译

拼写错误检查配置

打开一个src源码文件发现报错

左侧文件列表.vscode中c_cpp_properties.json右键将其删除
关闭VS code再打开

还有报错
ctrl+shift+P
上方搜索栏输入 error squiggles
选择禁用错误波形曲线

如何恢复
.vscode—>settings.json
将Disabled改成Enabled然后保存

CMake Tools启动会自动扫描工作空间
关闭
左下角齿轮—>设置—>搜索栏填入cmake:config
Edit和Open关闭
删除左侧bulid目录

2.2 Terminator 超级终端

安装与基本使用

使用示例（三个终端分别运行）：

# 终端1：启动ROS系统
roscore

# 终端2：启动仿真
roslaunch wpr_simulation wpb_simple.launch

# 终端3：启动键盘控制
rosrun rqt_robot_steering rqt_robot_steering

# 按 Ctrl+C 关闭

安装 Terminator：

sudo apt install terminator

使用方法：

# Ctrl+Alt+T 启动 Terminator
# 然后在终端中操作：
roscore
# 右键选择水平分割
roslaunch wpr_simulation wpb_simple.launch
# 再右键选择水平分割
rosrun rqt_robot_steering rqt_robot_steering

常用快捷键

桌面ctrl+alt+T

按住ctrl和shift不放右手按一下E
会分成左右两个终端

按住ctrl和shift不放右手按一下O

分为上下两个终端

Alt键不放右手敲一下方向键的左

操作焦点切换为左侧终端
按住ctrl和shift不放右手按一下W可以把刚分出来的窗口关闭

常见问题：分屏失败

ibus-setup

在设置中选择：表情符号 → 表情符号注释 → 删除

三、ROS 核心概念

3.1 Package 与 Node

【示意图说明】Package 与 Node 的包含关系：

Package（软件包）是 ROS 代码的基本组织单元，一个包可以包含多个 Node（节点）
包与包之间通过依赖项（dependency）建立引用关系
编译工具（catkin/cmake）负责将包源码编译成可执行程序

  ┌─────────────────────────────────────┐
  │          catkin_ws 工作空间           │
  │  ┌─────────────┐  ┌──────────────┐  │
  │  │  ssr_pkg    │  │  atr_pkg     │  │
  │  │ ┌─────────┐ │  │ ┌──────────┐ │  │
  │  │ │ chao_node │ │  │ │ ma_node  │ │  │
  │  │ └─────────┘ │  │ └──────────┘ │  │
  │  │ ┌─────────┐ │  │              │  │
  │  │ │ yao_node  │ │  │              │  │
  │  │ └─────────┘ │  │              │  │
  │  └─────────────┘  └──────────────┘  │
  │        ↑ 依赖  ↑                     │
  └────────┴──────┴─────────────────────┘
         std_msgs / roscpp / rospy
         （系统内置的通用软件包）

CMake和catkin作为编译工具

包为结点的容器

ssr_pkg—>超声波_Node

创建 Package 包

# 启动终端
cd catkin_ws/src/

# 创建软件包
catkin_create_pkg ssr_pkg rospy roscpp std_msgs

就是在~/catkin_ws/src/文件夹里
catkin_create_pkg<包名><依赖项列表>
通用的结点或资源单独方在一个包，这个包就成了一个通用的依赖项
rospy roscpp对python roscpp语言的支持
std_msgs标准消息包

运行后前两个是文件，后两个是目录
vs code中ssr_pkg文件—>CMakeLists.txt

编译器最低版本要求
工程名
寻找依赖包
##对文件中指令内容的说明
#与注释对应的指令示例

package.xml文件（catkin软件包的必备文件）

包名
版本号
包的内容描述
维护者信息
开源协议
网址
依赖项

roscd = 在终端中进入指定软件包的文件地址

# 终端输入
roscd roscpp
code package.xml

可以看到内容与之前的 package.xml 很像。

文件管理器—>其他位置—>计算机—>/opt/ros/noetic/share/
该地址存放的全都是ROS的package包
通过apt下载的软件包都是现成的可执行文件可直接运行

工作空间catkin_ws的软件包都是源码文件需要编译成可执行程序才能运行

.bashrc两句source指令
第一条加载的是apt下载的软件包地址
第二条指令对应的是catkin工作空间里的软件包地址

创建新软件包设置的依赖项就是两地址的某一个软件包

3.2 创建 Node 节点

vs code —>ssr_pkg—>右键src文件夹新建文件
输入文件名chao_node.cpp

#include<ros/ros.h>
main+ENter自动补全

出现误报先保存
左侧文件列表.vscode中c_cpp_properties.json右键将其删除
关闭VS code再打开
printf(“Hello World!\n”);
保存

编译配置

CMakeLists.txt文件
找到##Build##章节
找到Declare a C++ executable
##对文件中指令内容的说明
#与注释对应的指令示例
复制#后面的注释
在文件末尾粘贴
括号里第一项是可执行文件的名字改为 chao_node
括号里第二项是指定从哪个代码文件进行编译改为 chao_node.cpp
保存
ctrl+shift+B编译

如果没有设置编译快捷键
ctrl+alt+T打开新的终端程序
cd catkin_ws
catkin_make

运行节点

# 一、启动ROS系统
roscore

# Ctrl+Shift+O 分出新的终端（Terminator快捷键）

# 二、运行node节点
rosrun ssr_pkg chao_node
# 格式：rosrun <包名> <节点文件名>

# 如果出错，加载工作空间环境
source ~/catkin_ws/devel/setup.bash

# 按方向键上键，再次运行
rosrun ssr_pkg chao_node

将source ~/catkin_ws/devel/setup.bash添加到.bashrc这样每次打开终端程序会自动加载环境参数，
终端输入 code~/.bashrch
.bashrch在vs code打开
文件末尾添加source ~/catkin_ws/devel/setup.bash
保存

3.3 Node 节点完善

main函数的括号开头里添加
ros::init(agc, agv, “chao_node”)函数
出现红色下划线
删除main函数中的const
编译出现报错
编译器找不到函数的实现
CMakeLists.txt文件
找到##Build##章节
specify libraries项
复制#注释的命令在文件末尾粘贴
节点名称改为chao_node
保存后再编译

rosrun ssr_pkg chao_node

chao_node.cpp文件
printf后面加一个while(true)循环
终端再运行
ctrl+c终止
ctrl+shift+w强制关掉终端
代码改为while(ros::ok())

3.4 Topic 话题与 Message 消息

topic是节点间持续通讯的一种形式
话题通讯的两个节点通过话题的名称建立起话题通讯连接
话题中通讯的数据，叫做消息Message
消息Message通常会按照一定的频率持续不断的发送，以保证消息数据的实时性
消息的发送方叫做话题的发布者Publisher
消息的接收方叫做话题的订阅者Subsciber

一个ROS节点网络中，可以同时存在多个话题
一个话题可以有多个发布者，也可以有多个订阅者
一个节点可以对多个话题进行订阅，也可以发布多个话题
不同的传感器消息通常会拥有各自独立话题名称，每个话题只有一个发布者
机器人速度指令话题通常会有多个发布者，但是同一时间只能有一个发言人
ros index 网站看std_msgs介绍

发布者实现（C++）

ssr_pkg—>src—>chao_node.cpp
发布的话题名称
发布的消息类型
ros index 搜std_msgs然后website
在右侧的Msg API里查看消息类型

chao_node.cpp
printf下一行
ros::NodeHandle nh;
这个对象是我们节点和ros通讯的关键

ros::Publisher pub;
这个Publisher对象是我们发送消息的工具

ros::Publisher pub = nh.advertise<std_msgs::String>(“kuai_shang_che_kai_kei_qun”, 10);
头文件加上#include <std_msgs/String.h>
while函数里加上
std_msgs::String msg;
msg.data = “王者启动！”；
pub.publish(msg);

保存然后编译
在终端运行

# 终端1：启动ROS核心
roscore

# 终端2：运行发布者节点
rosrun ssr_pkg chao_node

# 终端3：查看话题列表
rostopic list

# 查看消息内容
rostopic echo /kuai_shang_che_kai_hei_qun

# 终端4：统计消息发送频率
rostopic hz /kuai_shang_che_kai_hei_qun

在while循环前面
ros::Rate loop_rate(10);
里面
loop_rate.sleep();

小结

发布者开发步骤：

确定话题名称和消息类型
在代码文件中 include 消息类型对应的头文件
在 main 函数中通过 NodeHandler 发布一个话题并得到消息发送对象
生成要发送的消息包并赋值
调用消息发送对象的 publish() 函数将消息包发送到话题中

常用 rostopic 命令：

rostopic list — 列出当前系统中所有活跃的话题
rostopic echo <话题名> — 显示指定话题中发送的消息包内容
rostopic hz <话题名> — 统计指定话题中消息包发送频率

复制节点yao_node

订阅者实现（C++）

创建订阅者节点后运行测试：

# 查看话题列表
rostopic list

# 查看运行着的节点
rosnode list

# 运行订阅者节点
rosrun atr_pkg ma_node

小结

确定话题名称和消息类型

在代码文件中include<ros.h>和消息类型对应的头文件

在main函数中通过 NodeHandler大管家订阅一个话题并设置消息接收回调函数。
定义一个回调函数，对接收到的消息包进行处理
main函数中需要执行 ros::spinOnce(), 让回调函数能够响应接收到的消息包。
工具rqt_graph
图形化显示当前系统活跃的节点以及节点间的话题通讯关系

四、launch 启动文件

【launch 文件示例】

<launch>
  <!-- 启动发布者节点 -->
  <node pkg="ssr_pkg" type="chao_node" name="chao_node" output="screen"/>
  
  <!-- 启动订阅者节点 -->
  <node pkg="atr_pkg" type="ma_node" name="ma_node" output="screen"/>
</launch>

使用 roslaunch 包名文件名.launch 即可一次启动多个节点。

4.1 编写与运行 launch 文件

launch 文件放在某个软件包的文件夹里即可。

# 查看节点通讯图
rqt_graph

小结
1.使用launch文件，可以通过roslaunch指令一次启动多个节点。
2.在launch文件中，为节点添加 output=“screen"属性，可以让节点信息输出在终端中。(ROS WARN不受该属性控制)
在launch文件中，为节点添加launch-prefix=“gnome-terminal -e属性，可以让节点单独运行在一个独立终端中。

4.2 Python 发布者节点

新建软件包：

cd catkin_ws/src/
catkin_create_pkg ssr_pkg rospy std_msgs

# 编译
cd ..
catkin_make

注意：Python 节点新增或修改代码后不需要再次编译。

在ssr_pkg中新建scripts文件夹在文件夹里新建chao_node.py

【Python 发布者节点代码 chao_node.py】

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    # 初始化ROS节点
    rospy.init_node("chao_node")
    
    # 创建发布者对象（话题名、消息类型、队列大小）
    pub = rospy.Publisher("kuai_shang_che_kai_hei_qun", String, queue_size=10)
    
    # 设置循环频率（10Hz）
    rate = rospy.Rate(10)
    
    while not rospy.is_shutdown():
        # 构建消息包
        msg = String()
        msg.data = "王者启动！"
        
        # 发布消息
        pub.publish(msg)
        
        # 延时
        rate.sleep()

右键 .py 文件在文件夹打开，再右键空白区域终端打开：

# 添加可执行权限
chmod +x chao_node.py

运行测试：

# 终端1：启动ROS核心
roscore

# 终端2：运行发布者节点
rosrun ssr_pkg chao_node.py

# 终端3：查看话题列表
rostopic list

# 查看话题消息内容
rostopic echo /kuai_shang_che_kai_hei_qun

新建 yao_node.py 步骤同上。

4.3 Python 订阅者节点

新建软件包：

cd catkin_ws/src/
catkin_create_pkg atr_pkg rospy std_msgs

# 编译
cd ..
catkin_make

注意：Python 节点新增或修改代码后不需要再次编译。

在atr_pkg下新建scripts文件夹在文件夹里新建ma_node.py

【Python 订阅者节点代码 ma_node.py】

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from std_msgs.msg import String

# 消息回调函数：收到消息时自动调用
def callback(msg):
    rospy.loginfo("收到消息：%s", msg.data)

if __name__ == "__main__":
    # 初始化ROS节点
    rospy.init_node("ma_node")
    
    # 创建订阅者对象（话题名、消息类型、回调函数）
    sub = rospy.Subscriber("kuai_shang_che_kai_hei_qun", String, callback, queue_size=10)
    
    # 循环等待回调（保持节点运行）
    rospy.spin()

右键 .py 文件在文件夹打开，再右键空白区域终端打开：

# 添加可执行权限
chmod +x ma_node.py

运行测试：

# 终端1：启动ROS核心
roscore

# 终端2：运行发布者节点
rosrun ssr_pkg chao_node.py

# 终端3：运行订阅者节点
rosrun atr_pkg ma_node

在 atr_pkg 下新建 launch 文件夹，再新建 kai_hei.launch（节点名带 .py 后缀）。

运行测试：

# 运行 launch 文件
roslaunch /home/ucar/catkin_ws/src/atr_pkg/launch/kai_hei.launch

# 分屏查看节点通讯图
rqt_graph

五、机器人运动控制

5.1 速度控制话题 /cmd_vel

矢量运动（右手坐标手势食指为X轴正方向）m/s
旋转运动（点赞受伤分别指向坐标轴正方向）rad/s
x轴滚转运动
y轴俯仰运动
z轴自转运动

geometry_msgs/Twist消息包类型
话题名称/cmd_vel = command_velocity

速度控制节点—>速度控制话题—>机器人核心节点

仿真软件
cd catkin_ws/src/wpr_simulation
git pull
cd ~/catkin_ws
catkin_make
cd …
roslaunch wpr_simulation wpb_simple.launch

# 分屏运行示例程序
rosrun wpr_simulation demo_vel_ctrl.py

实现思路

构建一个新的软件包，包名叫做 vel_pkg
在软件包中新建一个节点，节点名叫做 vel_node.py
在节点中，向 ROS 大管家 rospy 申请发布话题 /cmd_vel，并拿到发布对象 vel_pub
构建一个 geometry_msgs/Twist 类型的消息包 vel_msg，用来承载要发送的速度值
开启一个 while 循环，不停的使用 vel_pub 对象发送速度消息包 vel_msg

最后同样的方式添加可执行权限：

chmod +x vel_node.py

运行测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_simple.launch

# 终端2：运行速度控制节点
rosrun vel_pkg vel_node.py

激光雷达工作原理
时长*光速 = 飞行长度
飞行长度/2 = 障碍物距离

5.2 激光雷达数据获取

运行示例程序：

# 编译（首次使用）
cd ~/catkin_ws
catkin_make

# 启动仿真
roslaunch wpr_simulation wpb_simple.launch

# 运行示例程序（新终端）
rosrun wpr_simulation demo_lidar_data.py

数据流向：

电路系统 → 激光雷达节点 → 雷达数据话题 /scan → 数据获取节点

消息包格式：sensor_msgs/LaserScan
话题名：/scan

实现步骤

构建软件包 lidar_pkg
新建节点 lidar_node.py
订阅话题 /scan，设置回调函数 LidarCallback()
在回调函数中接收和处理雷达数据
用 loginfo() 显示前方障碍物距离

# 创建软件包
cd ~/catkin_ws/src/
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs

# 编译
cd ~/catkin_ws
catkin_make

在 lidar_pkg/scripts 下新建 lidar_node.py：

# 添加可执行权限
cd ~/catkin_ws/src/lidar_pkg/scripts
chmod +x lidar_node.py

# 运行测试
roslaunch wpr_simulation wpb_simple.launch
rosrun lidar_pkg lidar_node.py

5.3 激光雷达避障

【激光雷达避障代码 lidar_node.py】

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

def lidar_callback(msg):
    global vel_pub
    
    # 获取正前方距离（雷达中间角度的数据）
    front_dist = msg.ranges[len(msg.ranges)//2]
    
    # 构建速度消息包
    vel_cmd = Twist()
    
    # 根据前方距离调整速度
    if front_dist > 1.0:
        # 前方无障碍，前进
        vel_cmd.linear.x = 0.5
        vel_cmd.angular.z = 0.0
    else:
        # 前方有障碍，旋转避障
        vel_cmd.linear.x = 0.0
        vel_cmd.angular.z = 0.5
    
    # 发布速度指令
    vel_pub.publish(vel_cmd)

if __name__ == "__main__":
    rospy.init_node("lidar_avoid_node")
    
    # 发布速度控制话题 /cmd_vel
    vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
    
    # 订阅激光雷达话题 /scan
    lidar_sub = rospy.Subscriber("/scan", LaserScan, lidar_callback, queue_size=10)
    
    rospy.spin()

在lidar_node.py引入速度消息的格式

1.让大管家 rospy 发布速度控制话题 /cmd _vel
2.
构建速度控制消息包 vel cmd.
3
根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物。

六、传感器数据与 RViz

6.1 RViz 可视化工具

可视化
传感器数据
机器人运算处理的中间结果和将要执行的目标指示

启动 RViz：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_simple.launch

# 终端2：启动 RViz
rviz

RViz 配置步骤：

左侧 Fixed Frame 修改成 base_footprint
左下角 Add 按钮 → 选中 RobotModel（机器人模型）
再选中 LaserScan → 左侧 Topic 选 /scan → Size(m) 改成 0.03
保存设置：File → Save Config As → 主目录 → 文件名 lidar.rviz

使用配置文件启动：

roslaunch wpr_simulation wpb_rviz.launch

sensor_msgs查看消息类型

【sensor_msgs/LaserScan 消息结构】

字段名	数据类型	说明	量纲/单位
header	std_msgs/Header	消息头（时间戳+坐标系ID）	-
angle_min	float32	雷达起始角度	rad
angle_max	float32	雷达终止角度	rad
angle_increment	float32	相邻激光束角度差	rad
time_increment	float32	相邻激光束时间间隔	s
scan_time	float32	扫描一周所需时间	s
range_min	float32	最小检测距离	m
range_max	float32	最大检测距离	m
ranges	float32[]	各角度对应的距离值数组	m
intensities	float32[]	各角度对应的反射强度数组	-

查看激光雷达数据：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_simple.launch

# 终端2：启动 RViz
roslaunch wpr_simulation wpb_rviz.launch

# 终端3：查看激光雷达数据（不含数组）
rostopic echo /scan --noarr

6.2 IMU 惯性测量单元

sensor_msgs—>websit—>Imu

orientation空间姿态描述
angular_velocity角速度
linear_acceleration矢量加速度

Python 实现 IMU 数据获取

【sensor_msgs/Imu 消息结构】

字段名	数据类型	说明	量纲/单位
header	std_msgs/Header	消息头（时间戳+坐标系ID）	-
orientation	geometry_msgs/Quaternion	空间姿态（四元数）	-
orientation_covariance	float64[9]	姿态协方差矩阵（3×3）	-
angular_velocity	geometry_msgs/Vector3	角速度（x, y, z）	rad/s
angular_velocity_covariance	float64[9]	角速度协方差矩阵	-
linear_acceleration	geometry_msgs/Vector3	线加速度（x, y, z）	m/s²
linear_acceleration_covariance	float64[9]	线加速度协方差矩阵	-

实现步骤：

构建软件包 imu_pkg
新建节点 imu_node.py
订阅话题 /imu/data，设置回调函数 imu_callback()
在回调函数中处理 IMU 数据，使用 TF 工具将四元数转换为欧拉角
用 loginfo() 显示欧拉角数值

# 添加可执行权限
chmod +x imu_node.py

# 运行测试
roslaunch wpr_simulation wpb_simple.launch
rosrun imu_pkg imu_node.py

6.3 IMU 航向锁定

imu_node.py中
让大管家 rospy 发布速度控制话题 /cmd_vel

设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角

启动仿真

6.4 标准消息包 std_msgs

【std_msgs 标准消息包常用类型】

消息类型	含义	主要字段	典型用途
Bool	布尔值	data (bool)	开关状态、标志位
String	字符串	data (string)	文本消息传递
Int8/16/32/64	有符号整数	data (int)	计数值、编号
UInt8/16/32/64	无符号整数	data (uint)	无符号数值
Float32/64	浮点数	data (float)	传感器数值、比例系数
Header	消息头	seq, stamp, frame_id	时间戳和坐标系标识
Time	时间	data (time)	时间戳
Duration	时长	data (duration)	时间间隔
ColorRGBA	颜色	r, g, b, a	颜色定义（带透明度）
MultiArray*	多维数组	layout, data	矩阵、数组数据

6.5 几何包与传感器消息包

【geometry_msgs 几何消息包常用类型】

消息类型	含义	主要字段	典型用途
Point	空间点	x, y, z	三维坐标点
Vector3	三维向量	x, y, z	速度、力向量
Quaternion	四元数	x, y, z, w	姿态表示
Pose	位姿	position, orientation	位置+姿态
Twist	速度	linear, angular	线速度+角速度
Transform	坐标变换	translation, rotation	坐标系变换
Polygon	多边形	points[]	区域、轮廓描述
Wrench	力/力矩	force, torque	力学量

带Stamped的消息包多了一个Header就是多了一个时间和坐标系ID

【带 Stamped 与不带 Stamped 的消息对比】

带 Stamped 的消息包多了一个 header 字段（std_msgs/Header），包含时间戳和坐标系ID。

基础消息类型	Stamped版本	新增字段	适用场景
Point	PointStamped	header	需要时间和坐标系的空间点
Pose	PoseStamped	header	带时间戳的位姿数据
Twist	TwistStamped	header	带时间戳的速度数据
Vector3	Vector3Stamped	header	带坐标系的向量数据
Wrench	WrenchStamped	header	带时间戳的力/力矩数据
Polygon	PolygonStamped	header	带坐标系的多边形

使用时先在ROS index中搜索消息名称

找到Msg API中对应消息类型的定义

按照定义的消息包结构进行数据的装填和读取

剩下就是发布或订阅相关话题进行消息包的发送和接收

【sensor_msgs 传感器消息包常用类型】

消息类型	含义	主要字段	典型传感器
LaserScan	激光扫描	angle_min/max, ranges[]	单线激光雷达
Imu	惯性测量	orientation, angular_velocity, linear_acceleration	IMU陀螺仪
Image	图像	height, width, encoding, data	相机/摄像头
CameraInfo	相机参数	height, width, K, D, P	相机内参
PointCloud2	点云	height, width, fields, data	深度相机/激光
JointState	关节状态	name[], position[], velocity[], effort[]	机械臂关节
BatteryState	电池状态	voltage, percentage, power_supply_status	电池
NavSatFix	GPS定位	status, latitude, longitude, altitude	GPS模块
Range	距离传感器	radiation_type, field_of_view, min_range, max_range, range	超声波/红外

参考之前的激光雷达和IMU的实验

使用时先在ROS index中搜索消息名称

找到wiki中对应消息类型的定义

注释中把每个变量的含义和量纲都解释清楚

按照定义的消息包结构进行数据的装填和读取

剩下就是发布或订阅相关话题进行消息包的发送和接收

七、自定义消息类型

7.1 生成自定义消息

【自定义消息 Carry.msg 文件内容】

# 自定义消息 Carry.msg
string name      # 名称
int32 star       # 星级
float32 data     # 数据

消息定义格式：数据类型变量名 # 注释说明

编译后可通过 rosmsg show qq_msgs/Carry 查看消息结构。

创建消息包：

cd ~/catkin_ws/src
catkin_create_pkg qq_msgs roscpp rospy std_msgs message_generation message_runtime

message_generation 和 message_runtime 是消息包生成和运行时所需要的依赖项。

然后在vscode中在刚才的消息包中创建一个msg文件夹
msg文件创Carry.msg
文件中定义消息结构
数据类型变量名

在CMakeLists.txt中
取消add_message_files的注释里面换成Carry.msg
generate_messages取消注释
catkin_package的 CATKIN_DEPENDS这行取消注释
保存
打开package.xml文件
确保<build_depend>和<exec_depend>都有

message_generation和message_runtime

编译并查看消息：

# 进入工作空间并编译
cd catkin_ws/
catkin_make

# 查看消息结构
rosmsg show qq_msgs/Carry

格式：rosmsg show <消息包名称>/<消息类型>

7.2 自定义消息步骤汇总

.创建新软件包，依赖项message_generation、message runtime软件包添加msg目录

2.，新建自定义消息文件，以.msg结尾。

2.1 在msg文件中定义消息成员

3在CMakeLists.txt中，将新建的.msg文件加入add_message_files()

4去掉generate_messages()注释符号，将依在CMakeLists.txt中，赖的其他消息包名称添加进去。
5在CMakeLists.txt中，将message_runtime 加入 catkin_package()的CATKIN DEPENDS。

6在package.xml中，将message_generation、message_runtime加入和

7编译软件包，生成新的自定义消息类型。

7.3 Python 节点中使用自定义消息

打开发布者节点chao_node.py
从qq消息包中引入Carry消息类型
将话题发布里的消息类型改成Carry
将要发送的消息包类型修改成Carry

然后在编译规则里添加消息包的依赖

CMakeLists.txt

find_package里面添加qq_msgs

pacage.xml

qq_msgs添加到和

保险起见对软件包再进行一次编译，确保自定义消息包和软件包都进入了 ROS 的消息列表：

cd catkin_ws/
catkin_make

打开订阅者节点 ma_node.py
从qq消息包中引入Carry消息类型
将话题订阅的消息类型改成Carry
回调函数中按照Carry.msg消息包的结构对消息包内容进行逐个显示
star是整形变量需要转换成字符串

修改编译规则
CMakeLists.txt
find_package里面添加qq_msgs
pacage.xml
qq_msgs添加到和
保险起见对软件包再进行一次编译

运行测试：

# 终端1：启动ROS核心
roscore

# 终端2：运行发布者节点
rosrun ssr_pkg chao_node.py

# 终端3：运行订阅者节点
rosrun atr_pkg ma_node.py

总结

1.在节点代码中，先 import 新定义的消息类型。
2.在发布或订阅话题的时候，将话题中的消息类型设置为新的消息类型。
3.按照新的消息结构，对消息包进行赋值发送或读取解析，
4.在CMakeList.txt文件的find package()中，添加新消息包名称作为依赖项。
5.在 package.xml 中，将新消息包添加到和中去。
6.重新编译，确保软件包进入ROS的包列表中

八、栅格地图与 SLAM

8.1 ROS 栅格地图格式

栅格边长—>地图分辨率（默认0.05米）
白色无障碍设为0，黑色有障碍物设100

将栅格一行行拼接起来就变成了一个数组
这就是ROS中OccupancyGrid消息包的数据内容

在rosindex网站搜 map_server 节点
websit—>在wiki业面主目录找到map_server的Published Topics子目录—>OccupancyGrid消息类型
header、（时间戳、坐标系ID）

地图参数信息、（地图加载时间、地图分辨率（米）、地图长度（栅格列数）、地图高度（栅格行数）、地图（0,0）位置与真实世界原点的偏差量（位移偏差：米，角度偏差：弧度））

八位整形的数组（地图的数据按照行优先的顺序从栅格矩阵的（0,0）位置开始排列）（栅格里的障碍物占据的取值范围是从0到100，未知则是-1）

8.2 Python 节点发布自定义地图

4×2的地图方便观察只对地图第一行赋值第二行保持空白状态

【示意图说明】4×2 自定义栅格地图：

地图共 2 行 4 列，第一行（y=1）全部设为占据状态（障碍物），第二行（y=0）全部为空白（可通行）。地图原点 (0,0) 位于左下角。

    列0    列1    列2    列3
  ┌─────┬─────┬─────┬─────┐
行1│  ▓  │  ▓  │  ▓  │  ▓  │  ← 障碍物（值=100）
  ├─────┼─────┼─────┼─────┤
行0│  ░  │  ░  │  ░  │  ░  │  ← 可通行（值=0）
  └─────┴─────┴─────┴─────┘
    ↑
  原点(0,0)

RViz 中显示效果：上方一排黑色栅格代表障碍物，下方一排灰色/白色栅格代表可通行区域，左下角有坐标轴标识世界坐标系原点。

地图中两个颜色相同且相邻的栅格就是地图起始位置

构建一个软件包map_pkg，依赖项里加上nav_msgs。
编译软件包，让其进入ROS的包列表。
在map_pkg里创建一个节点map_pub_node.py。
在节点中发布话题/map，消息类型为OccupancyGrid。
构建一个OccupancyGrid地图消息包，并对其进行赋值。
将地图消息包发送到话题/map。
为节点map_pub_node.py添加可执行权限。
运行map_pub_node.py节点。
启动RViz，订阅话题/map，显示地图。

添加一个坐标轴标识（标识的位置就是世界坐标系的原点）
添加地图显示将其话题名称设置为实验节点发布的/map

8.3 SLAM 原理简介

定位
雷达扫描一周记录已经探明的栅格状态和机器人的当前位置
到第二个地点再次进行扫描记录栅格状态和机器人的当前位置
与初始位置的局部地图尝试拼合起来
第三次同样扫描、记录、拼合地图

最终得到观测位置和参照物的全局地图
移动轨迹就是SLAM的定位问题

只留下参照物的位置就是SLAM最终建立的特征地图
其他未被激光雷达扫描到的区域属于未知区域颜色保持灰色（-1）

8.4 Hector Mapping 建图

激光雷达节点—>雷达数据话题/scan—>SLAM节点—>地图数据话题/map—>Rviz

SLAM节点的建图算法 Hector_Mapping

ROS index网站搜 hector_mapping

wiki页面找到ROS API
订阅话题
scan 获取激光雷达数据(主要的输入)
syscommand 用于介绍reset这类重新建图的指令
发布话题
map_metadata 地图的描述信息同上
map 栅格地图数据
slam_out_pose 原始定位信息
poseupdate 矫正后的定位信息

安装与运行

# 安装 hector_mapping
sudo apt install ros-noetic-hector-mapping

运行测试：

# 终端1：运行仿真环境
roslaunch wpr_simulation wpb_stage_slam.launch

# 终端2：运行 hector_mapping
rosrun hector_mapping hector_mapping

# 终端3：启动 RViz
rosrun rviz rviz

在 RViz 中添加显示项目：

添加 RobotModel（机器人模型）
添加 LaserScan（激光雷达），Topic 选 /scan，Size 调大一点
添加 Map（地图），Topic 选 /map

# 终端4：启动键盘控制
rosrun rqt_robot_steering rqt_robot_steering

launch 文件配置

# 创建软件包
cd catkin_ws/src/
catkin_create_pkg slam_pkg roscpp rospy std_msgs

在 VS Code 中右键创建 launch 子文件夹，新建 hector.launch：

<launch>
  <!-- 引入仿真环境 launch 文件 -->
  <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch"/>
  
  <!-- 启动 hector_mapping 节点 -->
  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"/>
  
  <!-- 启动 RViz -->
  <node pkg="rviz" type="rviz" name="rviz"/>
  
  <!-- 启动键盘控制 -->
  <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>
</launch>

# 编译
cd ~/catkin_ws/
catkin_make

# 运行
roslaunch slam_pkg hector.launch

提示：如果在实体机器人上运行 hector_mapping，将仿真环境的 launch 文件替换为控制实体机器人底盘和雷达的 launch 文件即可。
RViz 配置文件路径：/home/ucar/catkin_ws/src/slam_pkg/rviz/slam.rviz

参数设置

先在ros index中查看hector_maping
3.1.4Parameters
~参数名（参数类型，默认值）
内容说明
~map_update_distance_thresh (double, default: 0.4)
~map_update_angle_thresh (double, default: 0.9)
~map_pub_period (double, default: 2.0)

对比
roslaunch wpr_simulation wpb_hector_comparison.launch

8.5 TF 坐标变换系统

原点坐标系map 父坐标系
机器人坐标系base_footprint(底盘底部中心) 子坐标系
距离偏移量，角度偏差值
有效的只有X轴和Y轴距离值以及Z轴的角度值

【示意图说明】TF 坐标变换关系：

TF（Transform）系统描述了各个坐标系之间的平移和旋转变换关系，形成一棵坐标树。

          map（世界坐标系/父坐标系）
            │
            │ 平移：(x, y, 0)
            │ 旋转：绕Z轴旋转 θ
            ▼
    base_footprint（机器人底盘/子坐标系）

X轴偏移：机器人在地图X方向上的位置（米）
Y轴偏移：机器人在地图Y方向上的位置（米）
Z轴旋转：机器人朝向（偏航角 yaw，弧度）
平面移动机器人通常只有 X/Y 平移和 Z 轴旋转三个有效自由度

查看 TF 关系命令：

rosrun rqt_tf_tree rqt_tf_tree — 图形化显示 TF 树
rostopic echo /tf — 查看实时 TF 数据
tf_echo 父坐标系子坐标系 — 查看两个坐标系间的变换

运行建图程序：

roslaunch wpr_simulation wpb_hector.launch

在 RViz 中添加 TF 显示，TF 的 Frames 只保留 base_footprint 和 map。

查看 TF 数值：

# 查看话题列表
rostopic list

# 查看话题消息类型
rostopic type /tf
# 输出：tf2_msgs/TFMessage

查看 TF 数据和树结构：

# 查看实时 TF 数据
rostopic echo /tf

# 图形化显示 TF 树
rosrun rqt_tf_tree rqt_tf_tree

TF 树中每一个椭圆代表一个坐标系，上方椭圆是下方椭圆的父级坐标系。

8.6 里程计 odom

Hector Mapping 测试：

roslaunch wpr_simulation wpb_corridor_hector.launch

启动后发现 Gazebo 的机器人还在动但是 RViz 的机器人已经不走了（似乎定位出了问题）。

GMapping 测试：

roslaunch wpr_simulation wpb_corridor_gmapping.launch

机器人在长直走廊里建图，对于机器人出发端的特征超出雷达的检测范围，机器人再往前走只能检测左右的平行墙面，没有作为位移参照物的特征，激光雷达看来感觉就跟没在移动一样。

通过轮子的转动圈数 × 轮子周长 = 距离（这种方法就是电机里程计）。

里程计输出 odom 到 base_footprint 的 TF：

map → odom → base_footprint

先用里程计推算机器人的位移，再通过雷达点云贴合障碍物轮廓修正里程计误差的方法就是 GMapping 的核心算法。

对比两种算法的差别：

Hector Mapping：

roslaunch wpr_simulation wpb_corridor_hector.launch

RViz 里添加 TF 显示，TF 的 Frames 只保留 scanmatcher_frame 和 map。启动后发现 RViz 的机器人进入走廊不能继续走了。再切换到 odom 和 map。

hector_mapping 对里程计的处理只考虑机器人在 RViz 里的显示，没有考虑定位。

GMapping：

roslaunch wpr_simulation wpb_corridor_gmapping.launch

RViz 里添加 TF 显示，TF 的 Frames 只保留 base_footprint、odom 和 map。

8.7 GMapping 建图

先在 ROS Index 中查看 gmapping。

原理与数据接口

【GMapping 订阅话题】

话题名	消息类型	说明
/scan	sensor_msgs/LaserScan	激光雷达数据（必需输入）
/tf	tf2_msgs/TFMessage	坐标系变换关系（odom→base_link）

注意：雷达坐标系的名称需要跟雷达数据包 header 里的 frame_id 保持一致。

【GMapping 发布话题与 TF】

名称	类型	消息/TF类型	说明
map_metadata	话题	nav_msgs/MapMetaData	地图描述信息
map	话题	nav_msgs/OccupancyGrid	栅格地图数据
entropy	话题	std_msgs/Float64	定位误差度（熵值）
map → odom	TF	transform	地图到里程计的坐标变换

输出内容：

map_metadata — 地图信息
map — 栅格地图数据
entropy — 定位的误差度
map → odom 的 TF 关系

运行测试

运行仿真环境：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：查看激光雷达话题
rostopic list
rostopic echo /scan --noarr
# 可以看到 frame_id: "laser"，这就是激光雷达坐标系的名称

# 查看 TF 树
rosrun rqt_tf_tree rqt_tf_tree

运行 GMapping：

# 终端2：运行 GMapping
rosrun gmapping slam_gmapping

# 终端3：启动 RViz
rosrun rviz rviz

在 RViz 中添加显示：

RobotModel（机器人模型）
LaserScan，Topic 选 /scan，调整显示点大小
Map，Topic 选 /map

# 终端4：键盘控制移动，扫描所有房间
rosrun wpr_simulation keyboard_vel_ctrl

launch 文件配置

# 创建软件包（如果已有则跳过）
cd catkin_ws/src/
catkin_create_pkg slam_pkg roscpp rospy std_msgs

新建 gmapping.launch，将上面四条指令添加到 launch 文件中。

# 编译
cd ~/catkin_ws/
catkin_make

# 运行
roslaunch slam_pkg gmapping.launch

提示：如果在实体机器人上运行，将仿真环境的 launch 文件替换为控制实体机器人底盘和雷达的 launch 文件即可。

参数设置

ros index搜索Gmapping
先在ros index中查看hector_maping
4.1.4Parameters
~参数名（参数类型，默认值）
内容说明

【GMapping 主要参数列表（上）】

参数名	类型	默认值	说明
~maxUrange	float	80.0	激光最大可用测距范围（m）
~sigma	float	0.05	端点匹配噪声（m）
~kernelSize	int	1	搜索窗口大小
~lambda	float	0.1	平滑参数
~ogain	float	3.0	似然增益
~lskip	int	0	跳过的激光束数量
~srr	float	0.1	平移误差中的平移分量
~srt	float	0.2	平移误差中的旋转分量
~str	float	0.1	旋转误差中的平移分量
~stt	float	0.2	旋转误差中的旋转分量
~linearUpdate	float	1.0	机器人平移多少距离更新一次（m）
~angularUpdate	float	0.5	机器人旋转多少角度更新一次（rad）

【GMapping 主要参数列表（下）】

参数名	类型	默认值	说明
~temporalUpdate	float	-1.0	定时更新间隔（s），-1禁用
~resampleThreshold	float	0.5	重采样阈值
~particles	int	30	粒子数量
~xmin	float	-100.0	地图X轴最小值（m）
~ymin	float	-100.0	地图Y轴最小值（m）
~xmax	float	100.0	地图X轴最大值（m）
~ymax	float	100.0	地图Y轴最大值（m）
~delta	float	0.05	地图分辨率（m/栅格）
~occ_thresh	float	0.25	占据概率阈值
~maxRange	float	-1.0	激光最大测距（m），-1使用传感器最大值

打开gmapping.launch标签
添加

修改后终端执行 roslaunch slam_pkg gmapping.launch

8.8 地图的保存与加载

地图保存工具 map_saver：

# 用法
rosrun map_server map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] map:=/your/costmap/topic
# 默认保存成 map.pgm 和 map.yaml
# -f 参数指定地图文件名前缀

保存地图：

# 终端1：启动建图
roslaunch slam_pkg gmapping.launch

# 控制机器人移动扫描...

# 终端2：保存地图
rosrun map_server map_saver -f map

当前目录会创建 map.pgm 和 map.yaml 两个文件。

加载使用地图：

# 终端1：启动ROS核心
roscore

# 终端2：启动地图服务
rosrun map_server map_server map.yaml

# 终端3：启动 RViz 查看
rosrun rviz rviz
# 在 RViz 中添加 Map 显示

九、Navigation 导航系统

9.1 导航系统架构

【Navigation 导航系统组件清单】

组件名称	所属模块	功能说明	输入	输出
move_base	核心	导航调度中心，协调各模块	目标点、传感器数据	速度指令 /cmd_vel
global_planner	全局规划	规划全局路径	地图、起点、目标点	全局路径 plan
local_planner	局部规划	实时避障与速度计算	全局路径、局部代价地图	速度指令
global_costmap	全局代价地图	基于静态地图的障碍物膨胀	静态地图	全局代价地图
local_costmap	局部代价地图	基于实时传感器的障碍物检测	激光雷达/点云数据	局部代价地图
recovery_behaviors	恢复行为	机器人被困时的脱困策略	规划失败信号	恢复动作
amcl	定位模块	自适应蒙特卡洛定位	激光数据、地图、里程计	位姿估计（map→odom TF）
map_server	地图服务	提供静态地图数据	-	栅格地图 /map

9.2 move_base 导航配置

ros index搜索move_base

清单
1.move_base导航节点
2.map_server地图服务节点
3.amcl定位节点

3.sensor sources传感器节点
4.odometry source里程计节点
5.sensor transforms 传感器位置的TF节点
6.amcl 定位模块
指定导航目标点

环境准备

仿真环境准备：

cd ~/catkin_ws/src/
git clone https://github.com/6-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
cd wpb_home/wpb_home_bringup/scripts/
./install_for_noetic.sh
cd ~/catkin_ws/
catkin_make

导航代码节点编写

# 创建导航包
cd ~/catkin_ws/src/
catkin_create_pkg nav_pkg roscpp rospy move_base_msgs actionlib

在 VS Code 中，nav_pkg 下新建 launch 文件夹，创建 nav.launch 文件，依照组件清单编写。

# 编译
cd ~/catkin_ws/
catkin_make

运行测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

# 终端3：启动 RViz
rosrun rviz rviz
# 添加地图和机器人模型显示

RViz 工具栏中的 2D Nav Goal 按钮就是发送导航目标点的按钮。

9.3 全局规划器

全局路径规划算法
广度优先 Dijkstra算法
深度优先 A*算法

【全局规划器算法对比】

规划器/算法	核心原理	优点	缺点	适用场景
Dijkstra	广度优先，从起点向外扩展	保证最短路径、无启发式误差	搜索范围大，速度较慢	对路径长度要求高的场景
A*	启发式搜索，结合当前代价+预估代价	搜索效率高，速度快	启发函数设计影响最优性	大多数导航场景（推荐）
navfn	ROS默认全局规划器，基于Dijkstra/A*	内置默认，开箱即用	已知bug，部分情况规划异常	简单环境快速测试
global_planner	改进版规划器，支持Dijkstra/A*	更稳定，支持更多配置	需手动指定启用	生产环境推荐使用
carrot_planner	胡萝卜规划器，沿障碍边缘走	可作为自定义规划器模板	功能简单	学习和二次开发

navfn默认使用 Dijkstra A*算法有BUG
global_planner更好默认使用 Dijkstra
move_base默认使用navfn

设置新参数使用global_planner

使用A*算法添加在ros index搜global_planner Carrot_planner作为自定义规划器的模板进行修改

9.4 AMCL 定位算法

不断分裂比对再分裂比对淘汰不靠谱的分身保留匹配效果好的例子
在ros index搜amcl
3.1.5参数列表

vs Code中wpb_home文件夹wpb_home_tutorials的nav_lidar的两个launch文件

3.1.6提到了amcl节点和里程计的tf输出机制

【AMCL 主要参数列表】

参数名	类型	默认值	说明
~min_particles	int	100	最少粒子数
~max_particles	int	5000	最多粒子数
~kld_err	double	0.01	KLD采样最大误差
~kld_z	double	0.99	KLD采样上分位数
~odom_model_type	string	diff	里程计模型类型（diff/omni）
~odom_alpha1	double	0.2	旋转误差由旋转引起的分量
~odom_alpha2	double	0.2	旋转误差由平移引起的分量
~odom_alpha3	double	0.2	平移误差由平移引起的分量
~odom_alpha4	double	0.2	平移误差由旋转引起的分量
~laser_z_hit	double	0.95	激光模型命中障碍物权重
~laser_z_short	double	0.1	激光模型短时测量权重
~laser_z_max	double	0.05	激光模型最大测距权重
~laser_z_rand	double	0.05	激光模型随机测量权重
~update_min_d	double	0.2	最小平移更新距离（m）
~update_min_a	double	0.17	最小旋转更新角度（rad）
~transform_tolerance	double	0.1	TF变换容差（s）

TF 关系说明：

amcl 负责 map → odom 的 TF
里程计负责 odom → base_footprint 的 TF（通常连续变化）
机器人切换分身通过 map → base_footprint 这段 TF 上的跳跃突变实现

amcl负责map到odom的tf
里程计负责odom到base_footprint的tf通常保持连续变幻

机器人切换本体和分身是通过map到base_footprint这段tf上产生跳跃突变来实现
查看粒子云（分身）：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

# 终端3：启动 RViz
rosrun rviz rviz

在 RViz 中添加显示：

RobotModel（机器人模型）
Map（地图）
PoseArray，Topic 选 /particlecloud，颜色设为绿色

9.5 代价地图 Costmap

全局代价地图：基于静态地图，用于全局规划
局部代价地图：实时传感器数据构建，可能出现之前建图时没有的障碍物

运行测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

# 终端3：启动 RViz

rosrun rviz rviz
添加机器人模型和地图
添加path将Line style改为Billboards
颜色改成紫色

添加全局代价地图map，名称改为GlobalCostMap
话题名称选择有global_costmap这一项
Color Scheme 改为costmap
这样就看到全局代价地图的颜色了

在 RViz 中添加局部代价地图：

添加 Map，名称改为 LocalCostMap
Topic 选择带有 local_costmap 的项
Color Scheme 改为 costmap

将这些 RViz 显示项目保存为配置文件：

在 nav_pkg 里新建文件夹 rviz
File → Save Config As → 文件名 nav.rviz

在 nav.launch 中加载 RViz 配置文件：

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_pkg)/rviz/nav.rviz"/>

运行测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航（含RViz）
roslaunch nav_pkg nav.launch

参数设置

找到nav.launch文件中参数文件的位置
/home/ucar/catkin_ws/src/wpb_home/wpb_home_tutorials/nav_lidar
costmap_common_params.yaml主要影响代价地图形状，用单一文件描述保证全局代价地图和局部代价地图一致
和global_costmap_params.yaml、local_costmap_params.yaml这两个文件是代价地图的计算范围和频率
nav.launch文件ns参数是name space的意思相当于把ns的内容直接加在了文件开头

代价地图的观测来源除了地盘的单线雷达还可以添加头部三维相机
观测源添加一个head_kinect2

第二个文件为全局代价地图的参数文件
第三个文件为局部代价地图的参数文件

更多的参数可以在ros index搜 costmap_2d
8.1Parameters进行查阅

9.6 恢复行为 Recovery Behaviors

【恢复行为 Recovery Behaviors 类型】

行为名称	类型	功能说明	触发条件
清除代价地图	clear_costmap_recovery	清除代价地图中的障碍物层，重新感知	规划失败且认为代价地图过时
原地旋转	rotate_recovery	机器人原地旋转360度，帮助重新定位	清除代价地图后仍无法规划
缓慢移动	move_slow_and_clear	缓慢向前移动同时清除前方障碍	旋转后仍无法脱困

恢复行为按顺序依次执行，若某一步成功恢复规划能力，则不再执行后续行为。

参数设置

【恢复行为参数设置】

参数名	所属行为	类型	默认值	说明
reset_distance	clear_costmap_recovery	double	3.0	清除机器人周围多大范围外的障碍（m）
layer_names	clear_costmap_recovery	string list	[“obstacles”]	要清除的层名称列表
sim_granularity	rotate_recovery	double	0.017	旋转模拟步长（rad）
min_rot_vel	rotate_recovery	double	0.4	最小旋转速度（rad/s）
max_rot_vel	rotate_recovery	double	1.0	最大旋转速度（rad/s）
acc_lim_th	rotate_recovery	double	3.2	旋转加速度限制（rad/s²）
frequency	move_slow_and_clear	double	10.0	更新频率（Hz）
max_trans_vel	move_slow_and_clear	double	0.3	最大平移速度（m/s）
max_rot_vel	move_slow_and_clear	double	0.5	最大旋转速度（rad/s）
width	move_slow_and_clear	double	1.0	清除区域宽度（m）
clearing_distance	move_slow_and_clear	double	0.5	清除距离（m）

三种行为类型可以在ros index中搜索

恢复行为主要是为全局路径规划服务，把参数写到全局代价地图的参数文件里
vsCode中打开global_costmap_params.yaml

recovery_behaviors指定参数生效的空间
name:每个行为的名字
type: 每个行为的类型（从上面三个选）也可以是创建的新行为类型

地图的分层结构

【代价地图分层结构】

静态地图 → 障碍物地图 → 膨胀地图 → 代价地图（层层叠加）

层级名称	英文标识	数据来源	作用说明
静态地图层	static_layer	预先建好的地图	提供环境基本障碍物信息
障碍物层	obstacle_layer	激光雷达/深度相机	实时检测动态和静态障碍物
膨胀层	inflation_layer	障碍物层输出	将障碍物向外膨胀，保持安全距离
代价地图	costmap	各层叠加	最终用于规划的代价地图
静态地图——障碍物地图——膨胀地图——代价地图
测试恢复行为：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

注意：clear_costmap_recovery 的 layer_names 参数默认值是 "obstacles"，但其代码中默认值与 costmap_2d 中默认的障碍物层名 obstacle_layer 不一致。

想要清除行为能正常清除障碍物，需要让这两个名称保持一致：

在 costmap_common_params.yaml 中将障碍层名改成 obstacles
或者在 global_costmap_params.yaml 中将重置行为改成 layer_names: ["obstacle_layer"]

想要重置清除行为能正常的清除障碍物需要这两个代码中的名称一致
costmap_common_params.yaml中给代价地图设置参数

【costmap_common_params.yaml 配置示例】

# costmap_common_params.yaml
obstacle_range: 2.5       # 障碍物检测范围（m）
raytrace_range: 3.0       # 光线追踪范围（m）

# 机器人 footprint（机器人轮廓，单位m）
footprint: [[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]

inflation_radius: 0.5     # 膨胀半径（m）
cost_scaling_factor: 3.0  # 代价缩放因子

# 观测源
observation_sources: laser_scan_sensor

laser_scan_sensor:
  sensor_frame: laser
  data_type: LaserScan
  topic: /scan
  marking: true           # 标记障碍物
  clearing: true          # 清除无障碍区域

# 插件配置（注意层名称要与recovery_behaviors中的layer_names一致）
plugins:
  - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation, type: "costmap_2d::InflationLayer"}

注意：障碍物层名称（obstacles）要与恢复行为中的 layer_names 参数保持一致，否则清除行为无效。

将障碍层名字改成 obstacles

或者global_costmap_params.yaml中把重置行为改成
layer_names: [“obstacle_layer”]

9.7 局部规划器

【局部规划器对比】

规划器名称	全称	核心原理	优点	缺点	适用底盘类型
base_local_planner	Trajectory Rollout	轨迹采样+打分	经典稳定，参数成熟	脱困能力一般	差分、全向
dwa_local_planner	Dynamic Window Approach	动态窗口法，速度空间采样	计算快，实时性好	复杂环境易陷入局部最优	差分、全向
teb_local_planner	Timed Elastic Band	时间弹力带优化	脱困能力强，轨迹平滑	计算量大，参数复杂	差分、全向、阿克曼

常见的一些局部规划器

使用时在在launch文件中修改nav.launch文件中的move_base的base_local_planner参数就行
软件包在wpb_home中，编译时就会安装到ros系统然后就可以直接使用了

DWA 规划器

全名dynamic window approach动态窗口方法
生成轨迹，挑选轨迹
矢量运动和旋转运动取不同的值然后组合起来就能让机器人走出不一样的弧线
如何挑选
1运动轨迹和全局导航路线的贴合程度（过程）
2轨迹末端和目标点的距离（目标）
3轨迹路线和障碍物之间的距离（风险）

查看dwa在
nav.launch的局部规划器参数改成DWA的名变量字
然后加载一个参数文件

测试 DWA 规划器：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

在 RViz 中添加显示：

Path，Topic 选 DWA 的 local_plan（显示局部规划路径）
PointCloud2，Topic 选 DWA 的 trajectory_cloud（显示备选轨迹）

选择一个目标点后，机器人前方白色的是备选轨迹，绿色的是执行路线。

参数调节：

参数文件：wpb_home/wpb_home_tutorials/nav_lidar/dwa_*.yaml
其余参数可在 ROS Index 搜索 dwa

每改一次参数都要重新运行 launch 文件，或者在线调参：

# 新终端启动动态参数配置
rosrun rqt_reconfigure rqt_reconfigure

在窗口中点击 move_base → DWAPlanner，右侧就是可以动态调节的参数，调整后可以保存到参数文件。

轨迹评分参数最重要

TEB Planner

timed elastic band 时间弹力带
起点和终点生成弹力带—全局路径对其吸引，障碍物对其排斥
跟具相同时间内机器人运动距离的远近挑选运动最快的路线

查看teb在
nav.launch的局部规划器参数改成TEB的变量名字
然后将局部规划器的参数文件修改为TEB的参数文件
测试 TEB 规划器：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

在 RViz 中添加显示：

Path，Topic 选 TEB 的 local_plan（绿色线条为局部导航路径）
PoseArray，Topic 选 TEB 的 poses（红色箭头为预测位置）

选择一个目标点进行导航，按 Ctrl+S 保存 RViz 显示设置。

TEB vs DWA：

TEB 具有更强的脱困能力
TEB 在最后调整机器人朝向时倾向于使用弧线倒车
更适合阿克曼底盘这种不能原地旋转的机器人
如果机器人后方没有雷达视野，需要谨慎选择 TEB

参数调节：

参数文件：wpb_home/wpb_home_tutorials/nav_lidar/teb_*.yaml
其余参数可在 ROS Index 搜索 teb

在线调参：

# 新终端启动动态参数配置
rosrun rqt_reconfigure rqt_reconfigure

在窗口中点击 move_base → TEBLocalPlanner，右侧就是可以动态调节的参数。

提示：YAML 文件中 costmap_converter 插件默认没有启用，启用该插件可以获得更好的规划性能和避障效果。

9.8 Action 编程接口

机器人需要自主进行导航需要编写程序代码实现导航功能
需要Navigation的导航接口
Action是ROS的一种通讯方式
Action中消息包的传输是双向的

9.9 Python 导航编程实现

【Action 导航客户端 Python 代码 nav_client.py】

#!/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_node")
    
    # 创建Action客户端
    client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    
    # 等待服务端启动
    client.wait_for_server()
    
    # 构建导航目标点
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    
    # 设置目标位置和朝向
    goal.target_pose.pose.position.x = 1.0
    goal.target_pose.pose.position.y = 1.0
    goal.target_pose.pose.orientation.w = 1.0
    
    # 发送导航目标
    client.send_goal(goal)
    
    # 等待导航结果
    client.wait_for_result()
    
    # 输出结果
    rospy.loginfo("导航结果：%s", client.get_state())

实现步骤：

编写节点文件：在 nav_pkg/scripts 下新建 nav_client.py
添加可执行权限
运行测试

# 添加可执行权限
cd ~/catkin_ws/src/nav_pkg/scripts/
chmod +x nav_client.py

运行测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航
roslaunch nav_pkg nav.launch

# 终端3：运行导航客户端
rosrun nav_pkg nav_client.py

例子程序参考：wpr_simulation/scripts/demo_nav_client.py

9.10 开源导航插件

安装航点导航插件：

cd ~/catkin_ws/src/
git clone https://github.com/6-robot/waterplus_map_tools.git
cd waterplus_map_tools/scripts/
./install_for_noetic.sh
cd ~/catkin_ws/
catkin_make

航点设置：

# 将建好的地图放到 wpr_simulation/maps 目录
# 然后执行航点设置程序
roslaunch waterplus_map_tools add_waypoint_simulation.launch

RViz 工具栏有个 Add Waypoint 图标，点击后在地图上点击添加航点。

# 保存航点
rosrun waterplus_map_tools wp_saver

航点信息保存到 ~/waypoints.xml 文件中。

航点导航插件

# 启动航点导航仿真
roslaunch wpr_simulation wpb_map_tool.launch

仿真环境中机器人在门外，RViz 中机器人在地图中央。用 RViz 的 2D Pose Estimate（绿色箭头）工具设置初始位置。

# 运行例子程序
rosrun wpr_simulation demo_map_tool
# 机器人会导航前往 1 号导航点

调整航点位置和朝向

再次运行航点设置程序：

roslaunch waterplus_map_tools add_waypoint_simulation.launch

按航点位置的箭头进行调整，调整好后保存：

# 保存航点
rosrun waterplus_map_tools wp_saver

退出航点设置程序，执行航点导航：

# 运行航点导航例子程序
rosrun wpr_simulation demo_map_tool

机器人会导航去往新的航点位置。

插件集成与启动

【航点导航插件节点清单】

节点名	包名	功能说明	订阅话题	发布话题	提供服务
wp_navi_server	waterplus_map_tools	航点导航服务端	/move_base/result	/move_base/goal	航点导航服务
wp_manager	waterplus_map_tools	航点管理服务端	-	-	航点增删改查服务

【航点导航 launch 节点配置】

在 nav.launch 文件中添加以下两个节点：

<!-- 航点导航服务端 -->
<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"/>

同时将 RViz 配置文件替换为 map_tool.rviz，即可在 RViz 中使用航点工具栏。

添加这两个节点
在nav.launch文件夹里添加wp_navi_server节点和wp_manager节点的启动项

/home/ucar/catkin_ws/src/wpr_simulation/rviz/map_tool.rviz
就是航点导航的显示配置文件将其粘贴到nav_pkg的rviz文件夹里
将nav.launch的rviz文件修改为map_tool.rviz

用例子测试：

# 终端1：启动仿真环境
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动导航（含航点插件）
roslaunch nav_pkg nav.launch

# 终端3：运行航点导航例子
rosrun wpr_simulation demo_map_tool

Python 实现航点导航

在 nav_pkg/scripts 下新建 wp_node.py，参考例子程序 demo_map_tools.py 编写。

# 添加可执行权限
cd ~/catkin_ws/src/nav_pkg/scripts/
chmod +x wp_node.py

测试：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_stage_robocup.launch

# 终端2：启动航点导航
roslaunch nav_pkg nav.launch

# 终端3：运行航点节点
rosrun nav_pkg wp_node.py

测试其他导航点：修改 wp_node.py 中的目标航点编号，重新运行即可。

十、相机与视觉处理

10.1 ROS 相机话题

彩色图像
bayer阵列

【示意图说明】Bayer 滤光阵列：

单芯片彩色相机通过在传感器像素前覆盖 Bayer 滤光片阵列来获取彩色信息。每个像素只透过一种颜色（红、绿、蓝），再通过插值算法还原出每个像素的完整 RGB 值。

  ┌───┬───┬───┬───┐
  │ R │ G │ R │ G │  ← 奇数行
  ├───┼───┼───┼───┤
  │ G │ B │ G │ B │  ← 偶数行
  ├───┼───┼───┼───┤
  │ R │ G │ R │ G │
  ├───┼───┼───┼───┤
  │ G │ B │ G │ B │
  └───┴───┴───┴───┘
  
  RGGB 排列模式（最常见）
  绿色像素数量是红/蓝的2倍（人眼对绿色更敏感）

R（Red）：红色滤光片，只透过红光
G（Green）：绿色滤光片，只透过绿光
B（Blue）：蓝色滤光片，只透过蓝光
通过 demosaicing（去马赛克）算法将 Bayer 原始数据转换为 RGB 彩色图像

# 查看相机帧率
rostopic hz /kinect2/qhd/image_color_rect

# 查看消息类型
rostopic type /kinect2/qhd/image_color_rect
# 输出：sensor_msgs/Image

在 ROS index 中搜索 sensor_msgs 查看消息定义。使用时一般将其转化为 OpenCV 的 Mat 类型处理。

10.2 Python 图像获取

【cv_bridge 图像获取 Python 代码 image_node.py】

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

def image_callback(img_msg):
    bridge = CvBridge()
    try:
        # 将ROS图像消息转换为OpenCV格式（bgr8 = 8位BGR彩色图）
        cv_image = bridge.imgmsg_to_cv2(img_msg, "bgr8")
    except CvBridgeError as e:
        rospy.logerr(e)
        return
    
    # 显示图像窗口
    cv2.imshow("RGB Image", cv_image)
    cv2.waitKey(1)  # 等待1ms，刷新窗口

if __name__ == "__main__":
    rospy.init_node("image_node")
    
    # 订阅相机图像话题
    img_sub = rospy.Subscriber(
        "/kinect2/qhd/image_color_rect", 
        Image, 
        image_callback
    )
    
    rospy.spin()
    cv2.destroyAllWindows()

# 创建图像包
cd ~/catkin_ws/src/
catkin_create_pkg image_pkg rospy sensor_msgs cv_bridge

在 image_pkg 下创建 scripts 文件夹，新建 image_node.py（参考 demo_cv_image.py）。

# 添加可执行权限
cd ~/catkin_ws/src/image_pkg/scripts/
chmod +x image_node.py

# 编译
cd ~/catkin_ws
catkin_make

运行测试：

# 终端1：启动仿真（球场景）
roslaunch wpr_simulation wpb_balls.launch

# 终端2：运行图像节点
rosrun image_pkg image_node.py
# 会弹出 RGB 窗口显示机器人看到的图像

# 终端3：让球动起来
rosrun wpr_simulation ball_random_move

10.3 颜色目标识别与定位

1、颜色空间转换 RGB——> HSV

【示意图说明】HSV 颜色空间：

HSV 将颜色表示为三个分量，比 RGB 更适合做颜色分割：

分量	全称	含义	取值范围（OpenCV）
H	Hue	色相/色调，代表颜色种类	0-179
S	Saturation	饱和度，颜色的鲜艳程度	0-255
V	Value	明度，颜色的明亮程度	0-255

H=0 红色，H=30 橙色，H=60 黄色，H=120 蓝色，H=150 紫色
S=0 灰度（无颜色），S=255 最鲜艳
V=0 纯黑，V=255 最亮

颜色识别时，设置目标颜色的 H/S/V 上下限，通过 cv2.inRange() 函数提取符合条件的像素区域。

【HSV 颜色分割阈值参数】

参数名	含义	取值范围	橙色球推荐值	说明
H_min	色相下限	0-179	0-15	橙色偏红区域起始
H_max	色相上限	0-179	15-30	橙色偏黄区域结束
S_min	饱和度下限	0-255	100-150	排除浅灰色干扰
S_max	饱和度上限	0-255	255	高饱和度保留
V_min	明度下限	0-255	100-150	排除过暗区域
V_max	明度上限	0-255	255	高亮度保留

使用 cv2.inRange(hsv_img, lower_hsv, upper_hsv) 进行二值化分割。

设置好六个分割阈值

2、二值化分割提取目标物
把具有某种颜色特征的像素从相机图像中提取出来
得到黑白图片
白色区域
为符合阈值的像素点集合
黑色区域为其他像素

如果把符合阈值的像素点集合认定为就是某个物体的形状
通过对这些像素点进行坐标值均值计算就可以得到物体的质心
这样就实现了对具有某种颜色特征的物体进行空间定位的效果

3、计算目标物的质心坐标

在 image_pkg/scripts 中创建 hsv_node.py（参考 demo_cv_hsv.py）。

# 添加可执行权限
cd ~/catkin_ws/src/image_pkg/scripts/
chmod +x hsv_node.py

# 编译
cd ~/catkin_ws
catkin_make

运行测试：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_balls.launch

# 终端2：运行颜色识别节点
rosrun image_pkg hsv_node.py
# 会弹出 RGB、HSV、Result、Threshold 四个窗口

# 终端3：让球滚动
rosrun wpr_simulation ball_random_move

10.4 颜色目标跟随

【颜色目标跟随控制逻辑】

图像参数	对应机器人运动	控制逻辑	比例系数（参考）
目标球中心X偏移	旋转速度（angular.z）	偏移越大转得越快，负左正右	0.005 rad/s 每像素
目标球中心Y偏移	前进速度（linear.x）	目标偏上→前进，偏下→后退	0.003 m/s 每像素
目标球面积/高度	前后距离调整	球太小→靠近，球太大→后退	比例控制

控制原理：将相机图像中目标球的横纵坐标与图像中点做差值，分别和机器人的旋转速度、前后速度挂钩，实现自动跟踪。

可以把相机图像中目标球和竖直中线的横向偏差和机器人的旋转速度挂钩
这样就能驱使机器人旋转对准目标球

目标球距离的远近和它在相机图像中的高度有关系
将目标球的纵坐标和机器人的前后速度挂钩
一般会在相机图像中给目标球的上下流出余量
这样当目标球上下移动时还能继续追踪

操作选相机图像的中点作为对准目标球的位置
只要将目标球的横纵坐标与相机图像的中点做差值
和机器人的旋转和前后运动速度挂钩
就能让机器人始终对准目标球并和目标球保持特定的距离

在 image_pkg/scripts 中新建 follow_node.py（参考 demo_cv_follow.py）。

# 添加可执行权限
cd ~/catkin_ws/src/image_pkg/scripts/
chmod +x follow_node.py

运行测试：

# 终端1：启动仿真
roslaunch wpr_simulation wpb_balls.launch

# 终端2：运行跟随节点
rosrun image_pkg follow_node.py
# 会弹出 RGB、HSV、Result、Threshold 四个窗口

# 终端3：让球滚动
rosrun wpr_simulation ball_random_move
# 观察机器人是否会跟随橘色球移动

10.5 人脸检测

【效果说明】人脸检测运行效果：

程序接收相机图像输入，使用 OpenCV 内置的 Haar 级联分类器检测人脸
检测到人脸后，在图像中用蓝色矩形框标出人脸位置
矩形框左上角坐标为 (x, y)，宽度 w，高度 h
可同时检测画面中的多个人脸
检测速度与图像分辨率和人脸大小有关

基于Haar特征的级联分类器

【示意图说明】Haar-like 特征：

Haar 特征是人脸检测中使用的基础特征，通过相邻区域像素值的差异来描述图像的纹理模式。

特征类型	形态	描述
边缘特征	░▓ / ▓░	左右/上下两区域亮度差异
线特征	░▓░ / ▓░▓	三区域中间亮/暗
中心环绕特征	中心暗四周亮	中心与周围的差异

白色区域像素和减去黑色区域像素和 = 特征值
人脸的某些部位（如眼睛比脸颊暗、鼻梁比两侧亮）符合特定的 Haar 特征模式
级联分类器将多个简单的 Haar 特征组合起来，层层筛选，最终定位人脸

【人脸检测 Python 代码 face_node.py】

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

def face_detect_callback(img_msg):
    bridge = CvBridge()
    cv_image = bridge.imgmsg_to_cv2(img_msg, "bgr8")
    
    # 加载Haar级联分类器（人脸检测模型）
    face_cascade = cv2.CascadeClassifier(
        '/usr/share/opencv4/haarcascades/haarcascade_frontalface_default.xml')
    
    # 转换为灰度图（Haar特征基于灰度计算）
    gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
    
    # 检测人脸
    faces = face_cascade.detectMultiScale(gray, 1.3, 5)
    
    # 在原图上画出人脸矩形框
    for (x, y, w, h) in faces:
        cv2.rectangle(cv_image, (x, y), (x+w, y+h), (255, 0, 0), 2)
    
    # 显示结果窗口
    cv2.imshow("Face Detection", cv_image)
    cv2.waitKey(1)

if __name__ == "__main__":
    rospy.init_node("face_detect_node")
    
    img_sub = rospy.Subscriber(
        "/kinect2/qhd/image_color_rect", 
        Image, 
        face_detect_callback
    )
    
    rospy.spin()
    cv2.destroyAllWindows()

在 image_pkg/scripts 中新建 face_node.py（参考 demo_cv_face_detect.py）。

# 添加可执行权限
cd ~/catkin_ws/src/image_pkg/scripts/
chmod +x face_node.py

运行测试：

# 终端1：启动仿真（单人脸场景）
roslaunch wpr_simulation wpb_single_face.launch

# 终端2：运行人脸检测节点
rosrun image_pkg face_node.py

# 终端3：键盘控制机器人移动
rosrun wpr_simulation keyboard_vel_ctrl

A.3 其他零散笔记

# 激活 YOLOv8 环境
conda activate yolov8

# 运行脚本
bash fin2.sh

C++ 中的 push_back() 函数。

附录

A.1 ROS 常用命令速查

命令	说明	示例
`roscore`	启动 ROS 核心	`roscore`
`rosrun`	运行指定包中的节点	`rosrun turtlesim turtlesim_node`
`roslaunch`	通过 launch 文件启动多个节点	`roslaunch wpr_simulation wpb_simple.launch`
`catkin_make`	编译 catkin 工作空间	`cd ~/catkin_ws && catkin_make`
`catkin_create_pkg`	创建 ROS 软件包	`catkin_create_pkg my_pkg rospy std_msgs`
`rostopic list`	列出当前所有活跃话题	`rostopic list`
`rostopic echo`	实时显示话题消息内容	`rostopic echo /cmd_vel`
`rostopic hz`	统计话题消息发布频率	`rostopic hz /scan`
`rosnode list`	列出当前所有活跃节点	`rosnode list`
`rosmsg show`	查看消息类型结构定义	`rosmsg show std_msgs/String`
`roscd`	跳转到指定软件包目录	`roscd std_msgs`
`rqt_graph`	图形化显示节点和话题连接	`rqt_graph`
`rviz`	启动 RViz 可视化工具	`rviz`

A.2 文档补全说明

本笔记中的 37 张图片因引用本地路径而丢失，已根据文档上下文和 ROS 专业知识进行补全。

补全图片清单

序号	原图片文件名	所在章节	补全方式
1	image-20250707105335745	Node和Package	文字描述+ASCII图
2	image-20250710121923548	launch文件启动节点	XML代码块
3	image-20250712103540266	Python发布者节点	Python代码块
4	image-20250712110934764	Python订阅者节点	Python代码块
5	image-20250713170413619	LaserScan消息结构	Markdown表格
6	image-20250718161837390	激光雷达避障	Python代码块
7	image-20250718214825638	IMU消息结构	Markdown表格
8	image-20250720105046421	std_msgs消息列表	Markdown表格
9	image-20250720111017566	geometry_msgs消息列表	Markdown表格
10	image-20250720111505239	Stamped消息对比	Markdown表格
11	image-20250720112752575	sensor_msgs消息列表	Markdown表格
12	image-20250720113915294	自定义消息Carry.msg	msg代码块
13	image-20250722162153970	自定义地图发布	文字描述+ASCII图
14	image-20250723215848647	TF系统	文字描述+ASCII图
15	image-20250724152510437	GMapping订阅话题	Markdown表格
16	image-20250724152958887	GMapping发布话题/TF	Markdown表格
17	image-20250724170613851	GMapping参数（上）	Markdown表格
18	image-20250724175025139	GMapping参数（下）	Markdown表格
19	image-20250725112225390	Navigation导航系统	Markdown表格
20	image-20250725204627200	全局规划器对比	Markdown表格
21	image-20250725220835099	AMCL定位算法	Markdown表格
22	image-20250726170140556	恢复行为类型	Markdown表格
23	image-20250726214001058	恢复行为参数设置	Markdown表格
24	image-20250726215635077	代价地图分层结构	Markdown表格
25	image-20250726223815757	costmap配置文件	YAML代码块
26	image-20250727123122741	局部规划器对比	Markdown表格
27	image-20250727220747767	Action导航编程	Python代码块
28	image-20250728182510725	航点导航插件架构	Markdown表格
29	image-20250728192003112	航点导航launch配置	XML代码块
30	image-20250728210614779	Bayer阵列原理	文字描述+ASCII图
31	image-20250728211502333	相机图像获取	Python代码块
32	image-20250728230027757	HSV颜色空间	文字描述+表格
33	image-20250728230442780	HSV阈值参数	Markdown表格
34	image-20250729154059001	颜色目标跟随	Markdown表格
35	image-20250729172408507	人脸检测效果	文字描述
36	image-20250729174138259	Haar特征原理	文字描述+表格
37	image-20250729175323960	人脸检测代码	Python代码块

补全方式统计

补全方式	数量	占比
Markdown表格	19张	51%
代码块（Python/XML/YAML/msg）	10张	27%
文字描述+ASCII示意图	8张	22%

使用提示

所有代码示例基于 ROS Noetic + Python3
消息结构和参数定义参考 ROS 官方 Wiki
补全内容为根据上下文推断的结果，原图可能略有差异
每张补全内容前均有注释标记，可搜索"补全替代图片"快速定位

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

GPT-6 Astra 微信、企业微信和飞书机器人接入教程

从账号绑定、模型选择到第一轮真实调用，演示 GPT-6 Astra 接入微信、企业微信、飞书、钉钉与 QQ 前的最小可核验链路，并附各平台连接步骤。

DAMO开发者矩阵

数字炸弹小游戏：李昱辰，1.0版本

cout<<"炸弹："<<bomb<<endl;cout<<"正在解压中"<<endl;cout<<"请输入数字："<<endl;cout<<"大了"<<endl;cout<<"小了"<<endl;cout<<"机器人已经想好炸弹"<<endl;cout<<"按下其余任意键：退出"<<endl;cout<<"按下2：游戏规则"<<endl;cout<<"按下3：退出游戏"<<endl;cout<<

DAMO开发者矩阵

白酒食品饮料智能仓储：从装卸到码垛的自动化改造方案

DAMO开发者矩阵

所有评论(0)

查看更多评论

kangjianing1

@kangjianing1

已为社区贡献1条内容

ubuntu—ROS入门

kangjianing1

Ubuntu ROS 入门教程

目录

一、基础命令与环境准备

1.1 Ubuntu 常用命令

1.2 ROS 安装步骤

1.3 APT 软件包安装

通过 ros.org 安装软件包

通过 GitHub 源码安装

编译与运行

设置 .bashrc 自动加载环境

ROS 软件包的源代码

二、开发工具配置

2.1 VS Code 安装与配置

推荐插件

编译快捷键设置

拼写错误检查配置

2.2 Terminator 超级终端

安装与基本使用

常用快捷键

常见问题：分屏失败

三、ROS 核心概念

3.1 Package 与 Node

创建 Package 包

3.2 创建 Node 节点

编译配置

运行节点

3.3 Node 节点完善

3.4 Topic 话题与 Message 消息

发布者实现（C++）

小结

订阅者实现（C++）

四、launch 启动文件

4.1 编写与运行 launch 文件

4.2 Python 发布者节点

4.3 Python 订阅者节点

五、机器人运动控制

5.1 速度控制话题 /cmd_vel

实现思路

5.2 激光雷达数据获取

实现步骤

5.3 激光雷达避障

六、传感器数据与 RViz

6.1 RViz 可视化工具

6.2 IMU 惯性测量单元

Python 实现 IMU 数据获取

6.3 IMU 航向锁定

6.4 标准消息包 std_msgs

6.5 几何包与传感器消息包

七、自定义消息类型

7.1 生成自定义消息

7.2 自定义消息步骤汇总

7.3 Python 节点中使用自定义消息

总结

八、栅格地图与 SLAM

8.1 ROS 栅格地图格式

8.2 Python 节点发布自定义地图

8.3 SLAM 原理简介

8.4 Hector Mapping 建图

安装与运行

launch 文件配置

参数设置

8.5 TF 坐标变换系统

8.6 里程计 odom

8.7 GMapping 建图

原理与数据接口

运行测试

launch 文件配置

参数设置

8.8 地图的保存与加载

九、Navigation 导航系统

9.1 导航系统架构

9.2 move_base 导航配置

环境准备

导航代码节点编写

9.3 全局规划器

9.4 AMCL 定位算法

9.5 代价地图 Costmap

参数设置