🍻 正在加载今日诗词....
字数统计: 23.5k字   |   阅读时长≈ 108分

ROS Noetic入门完整版

一、ROS基础概述

1、操作系统安装

ROS目前只能在基于Unix的平台上运行，因此我们使用Ubuntu来作为ROS的系统。这里我们安装了Ubuntu20.04和ROS Noetic Ninjemys。本机和虚拟机安装详见以下文章，选其中一种即可

笔记本安装 Windows10 和 Ubuntu20.04 双系统

VMWare虚拟机安装Ubuntu20.04详细过程

2、ROS Noetic安装

官网ROS Noetic安装教程参考：https://wiki.ros.org/cn/noetic/Installation/Ubuntu

1、配置系统软件源

打开“软件更新”，进入到“Ubuntu软件“页面，允许universe、restricted、multiverse三项，源换成国内源即可。
在这里插入图片描述

2、添加ROS软件源

1
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

3、设置密钥

1
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

4、安装ROS

1
2
3
4
#更新apt索引包
sudo apt update
#安装桌面完整版，若网络原因失败可多安装几次或使用手机热点
sudo apt install ros-noetic-desktop-full

5、设置环境

1
2
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

注意: 在 ROS 版本 noetic 中无需构建软件包的依赖关系，没有rosdep的相关安装与配置，其他版本需要安装。

6、小海龟测试

终端输入roscore可以查看安装好的ros的发行版名称为noetic和版本号；ctrl+Alt+t再次打开一个新的终端，输入rosrun turtlesim turtlesim_node，即会出现小海龟的仿真界面；再次打开新终端，输入rosrun turtlesim turtle_teleop_key，即可控制小海龟移动了。
在这里插入图片描述

7、ROS的卸载

1
sudo apt remove ros-noetic-*

8、安装构建依赖

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#安装构建依赖相关工具
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
#初始化rosdep
sudo rosdep init
rosdep update
#若一切顺利即可ok
#--------------------------------
#出现问题后找备份 https://gitee.com/zhao-xuzuo/rosdistro
cd /usr/lib/python3/dist-packages/
find . -type f | xargs grep "raw.githubusercontent"
#修改相关文件，使用sudo gedit
./rosdistro/__init__.py
./rosdep2/gbpdistro_support.py
./rosdep2/sources_list.py 
./rosdep2/rep3.py
#文件中涉及的 URL 内容，如果是:raw.githubusercontent.com/ros/rosdistro/master都替换成gitee.com/zhao-xuzuo/rosdistro/raw/master
#修改完毕，再重新执行命令
sudo rosdep init
rosdep update

3、常用工具安装

1、Terminator

1
sudo apt install terminator
1
2
3
4
5
6
7
8
9
10
11
Alt+Up                          //移动到上面的终端
Alt+Down                        //移动到下面的终端
Alt+Left                        //移动到左边的终端
Alt+Right                       //移动到右边的终端
Ctrl+Shift+O                    //水平分割终端
Ctrl+Shift+E                    //垂直分割终端
Ctrl+Shift+C                    //复制选中的内容到剪贴板
Ctrl+Shift+V                    //粘贴剪贴板的内容到此处
Ctrl+Shift+W                    //关闭当前终端
Ctrl+Shift+Q                    //退出当前窗口，当前窗口的所有终端都将被关闭
Ctrl+Shift+X                    //最大化显示当前终端

2、VScode

下载地址:https://code.visualstudio.com/docs?start=true，下载deb，双击进行安装。

1
2
3
4
#第二种方式安装
sudo dpkg -i xxxx.deb
#卸载
sudo dpkg --purge  code

VScode插件安装

1
2
3
4
5
6
7
C/C++
python
CMake Tools
ROS
#-------
#在当前目录打开VScode
code .

vscode 中编译 ros

快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build

可以点击配置设置为默认，修改.vscode/tasks.json 文件，此后ctrl + shift + B可直接编译

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

添加功能包，选定 src 右击 —> create catkin package

PS1: 如果没有代码提示

如果VS没有智能提示，修改 .vscode/c_cpp_properties.json

设置 "cppStandard": "c++17"

PS2: main 函数的参数不可以被 const 修饰

PS3: 当ROS__INFO 终端输出有中文时，会出现乱码

1
2
3
#添加任意一句即可
setlocale(LC_CTYPE, "zh_CN.utf8");
setlocale(LC_ALL, "");

PS4：运行python文件可能会找不到文件或目录

因为ubuntu20.04默认使用python3，有三种解决方法

解决1: #!/usr/bin/env python3 直接使用 python3 (不推荐)

解决2: 创建一个链接符号到 python 命令:sudo ln -s /usr/bin/python3 /usr/bin/python

解决3:像C++一样进行链接编译，让程序自动寻找合适解释器

二、核心概念

1、ROS介绍

ROS (Robot Operating System, 机器人操作系统) 提供一系列程序库和工具以帮助软件开发者创建机器人应用软件。它提供了硬件抽象、设备驱动、函数库、可视化工具、消息传递和软件包管理等诸多功能。

ROS中文官网：https://wiki.ros.org/cn

2、ROS核心概念

节点与节点管理器

  • 节点（Node）—— 执行单元
    执行具体任务的进程、独立运行的可执行文件；不同节点可使用不同的编程语言，可分布式运行在不同的主机；节点在系统中的名称必须是唯一的。

  • 节点管理器 （ROS Master）—— 控制中心
    为节点提供命名和注册服务；跟踪和记录话题/服务通信，辅助节点相互查找、 建立连接；提供参数服务器，节点使用此服务器存储和检索 运行时的参数。

话题通信

  • 话题（Topic）—— 异步通信机制
    节点间用来传输数据的重要总线；使用发布（Publisher）/订阅（Subscriber）模型，数据由发布者传输到订阅者，同一个话题的订阅者或发布者可以不唯一。

  • 消息（Message）——话题数据

    话题的具体数据称为消息（Message），使用.msg文件定义；消息用来描述话题当中具体的数据类型，在ROS中，有些消息已经被预定义了，比如雷达、图像等，也可以自定义消息。

服务通信

  • 服务（Service）—— 同步通信机制
    使用客户端（Service）/服务器（Client）模型，客户端发送请求数据，服务器完成 处理后返回应答数据。使用.src文件定义。

话题与服务区别

话题 服务
同步性 异步 同步
通信模型 发布/订阅 服务器/客户端
底层协议 ROSTCP/ROSUDP ROSTCP/ROSUDP
反馈机制 无 有
缓冲区 有 无
实时性 弱 强
节点关系 多对多 一对多(一个server)
适用场景 数据传输，连续高频的数据发布与接收:雷达、里程计 逻辑处理，偶尔调用或执行某一项特定功能：拍照、语音识别
通信数据 msg srv

参数

  • 参数（Parameter）—— 全局共享字典
    可通过网络访问的共享、多变量字典；节点使用此服务器来存储和检索运行时的参数；适合存储静态、非二进制的配置参数，不适合存储动态配置的数据。

文件系统

  • 功能包（Package）： ROS软件中的基本单元，包含节点源码、配置文件、数据定义等。

  • 功能包清单（Package manifest）：记录功能包的基本信息，包含作者信息、许可信息、依赖选项、编译标志等。

  • 元功能包（Meta Packages）：组织多个用于同一目的功能包。

3、ROS常用命令行工具

http://wiki.ros.org/ROS/CommandLineTools

1
2
3
4
5
6
7
8
9
#分别在四个终端打开运行
#启动ROS Master
roscore
#启动海龟仿真节点
rosrun turtlesim turtlesim_node
#启动海龟控制节点
rosrun turtlesim turtle_teleop_key
#查看系统运行计算图
rqt_graph

rosnode节点命令

1
2
3
4
#查看活动节点
rosnode list
#查看某一节点具体信息
rosnode info /turtlesim

rostopic话题命令

1
2
3
4
5
6
7
8
9
10
11
12
#打印pub发布的数据
rostopic echo pub
#以10Hz的频率发布话题移动海龟，这里命令用双击tab补全后修改
#速度为m/s，角度为弧度/s
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0"

rosmsg消息命令

1
2
#查看消息
rosmsg show geometry_msgs/Twist

rosservice服务命令

1
2
3
4
5
6
7
#打印可用服务
rosservice list
#创建一个新海龟
rosservice call /spawn "x: 2.0
y: 2.0
theta: 0.0
name: 'turtle2'"

rosbag命令

1
2
3
4
#保存所有步骤，-O代表保存的压缩包路径
rosbag record -a -O cmd_record
#复原所有步骤
rosbag play cmd_record.bag

rossrv

1
2
3
4
5
6
7
#rossrv是用于显示有关ROS服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同。
rossrv show    #显示服务消息详情
rossrv info    #显示服务消息相关信息
rossrv list    #列出所有服务信息
rossrv md5    #显示 md5 加密后的服务消息
rossrv package   #显示某个包下所有服务消息
rossrv packages    #显示包含服务消息的所有包

rosparam

1
2
3
4
5
6
7
8
9
10
11
12
13
#rosparam包含rosparam命令行工具，用于使用YAML编码文件在参数服务器上获取和设置ROS参数。
#列出当前多有参数
rosparam list
#显示某个参数值
rosparam get param_key
#设置某个参数值
rosparam set param_key param_value
#保存参数到文件
rosparam dump file_name
#从文件读取参数
rosparam load file_name
#删除参数
rosparam delete param_key
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#增
catkin_create_pkg #自定义包名 依赖包 === 创建新的ROS功能包
sudo apt install xxx #=== 安装 ROS功能包
#删
sudo apt purge xxx #==== 删除某个功能包
#查
rospack list #=== 列出所有功能包
rospack find 包名 #=== 查找某个功能包是否存在，如果存在返回安装路径
roscd 包名 #=== 进入某个功能包
rosls 包名 #=== 列出某个包下的文件
apt search xxx #=== 搜索某个功能包
#改
rosed 包名 文件名 #=== 修改功能包文件
#需要安装 vim
#比如:rosed turtlesim Color.msg
#执行
roscore

三、编程基础

步骤

  • 先创建一个工作空间；
  • 创建一个功能包；
  • 编辑源文件；
  • 编辑配置文件；
  • 编译并执行。

1、工作空间创建

工作空间（workspace）是一个存放工程开发相关文件的文件夹。

在这里插入图片描述

  • src：代码空间（Source Space）
  • build：编译空间（Build Space）
  • devel：开发空间（Development Space）
  • install：安装空间（Install Space）

1、创建工作空间catkin_ws

1
2
3
4
5
6
#创建目录
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
#初始化文件夹
catkin_init_workspace
#src文件中创建了一个 CMakeLists.txt 的文件，目的是告诉系统，这个是ROS的工作空间。

**2、编译工作空间 **

1
2
3
4
5
6
#需要在整个工作目录下编译
#编译完成后，会出现build 和 devel两个文件夹
cd ~/catkin_ws/
catkin_make
#生成install文件夹
catkin_make install

3、设置环境变量

1
2
3
4
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
#查看当前环境变量
echo $ROS_PACKAGE_PATH

4、创建功能包

1
2
3
4
5
6
#std_msgs：包含常见消息类型,标准消息库
#roscpp：使用C++实现ROS各种功能
#rospy：使用python实现ROS各种功能
#test_pkg就是其中一个功能包，还可以下载其他功能包
cd ~/catkin_ws/src
catkin_create_pkg test_pkg std_msgs roscpp rospy

package.xml是功能包的描述文件，CMakeLists.txt定义编译规则，编译出的文件在devel目录下的include文件夹(头文件)和lib文件夹(可执行文件)

5、编译功能包

1
2
cd ~/catkin_ws
catkin_make

2、Topic话题通信

在这里插入图片描述

下载功能包

1
2
3
#下面的服务功能都需要依赖以下功能包
cd ~/catkin_ws/srcs
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

1、发布者publisher编程实现

指发布程序代码给海龟仿真器，控制海龟运动

1、编写发布者代码

发布者流程

  • 初始化ROS节点；
  • 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
  • 创建消息数据；
  • 按照一定频率循环发布消息。

以下两种语言二选一即可

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 * 将代码保存在功能包下的src目录下，并命名为velocity_publisher.cpp
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
	// ROS节点初始化
    /*param argc 参数个数
    *param argv 参数列表
    *param name 节点名称，需要保证其唯一性，不允许包含命名空间
    *param options 节点启动选项，被封装进了ros::init_options
    */
	ros::init(argc, argv, "velocity_publisher");
	// 创建节点句柄
	ros::NodeHandle n;
	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    #休眠的原因是注册未完成可能已经发出消息了，导致消息丢失
    ros::Duration(3).sleep();
	// 设置循环的频率
	ros::Rate loop_rate(10);
	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;
	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);
	    // 按照循环频率延时
	    loop_rate.sleep(); 
	}
	return 0;
}

python文件需要确保文件拥有可执行权限，这里我把python文件都放在了scripts文件夹下，在此打开终端，运行chmod +x *.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#python程序，区分与cpp文件，将代码保存在功能包下的scripts目录下，并命名为velocity_publisher.py
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
import rospy
from geometry_msgs.msg import Twist
def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)
	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    #休眠的原因是注册未完成可能已经发出消息了，导致消息丢失
    rospy.sleep(3)
	#设置循环的频率
    rate = rospy.Rate(10) 
    while not rospy.is_shutdown():
		# 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2
		# 发布消息
        turtle_vel_pub.publish(vel_msg)
    	rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z)
		# 按照循环频率延时
        rate.sleep()
if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

2、定义编译规则

仅C++代码需要定义编译规则，python代码可直接运行

1
2
3
4
5
#打开功能包的CMakeLists.txt文件,在build下添加以下规则
#编译
add_executable(velocity_publisher src/velocity_publisher.cpp)
#链接
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

3、编译

仅C++代码需要编译，python代码可直接运行

1
2
3
#要在工作空间下编译
cd ~/catkin_ws
catkin_make

4、运行

运行成功后海龟按照发布者的程序进行运动

1
2
3
4
5
6
7
8
9
10
#首先刷新一下环境变量，放入本地环境变量了也可以不刷新
source ~/catkin_ws/devel/setup.bash
#分别在三个终端打开，就可以看见海龟运行了
roscore
rosrun turtlesim turtlesim_node
#python运行以下命令
#若出现“python3”: 没有那个文件或目录，则需要更改py文件中的python解释器
rosrun learning_topic velocity_publisher.py
#C++运行下面命令
rosrun learning_topic velocity_publisher

2、订阅者subscriber编程实现

流程步骤与发布者相似，订阅者可以监听海龟的位置信息变化并打印在屏幕

订阅者流程

  • 初始化ROS节点；
  • 订阅需要的话题；
  • 循环等待话题消息，接收到消息后进入回调函数；
  • 在回调函数中完成消息处理。

C++订阅代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
#include <ros/ros.h>
#include "turtlesim/Pose.h"
// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
}

python订阅代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
import rospy
from turtlesim.msg import Pose
def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)
def pose_subscriber():
	# ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)
	# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)
	# 循环等待回调函数
    rospy.spin()
if __name__ == '__main__':
    pose_subscriber()

3、自定义话题通信

在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty… 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，就会显得力不从心，这时候就需要自定义消息。

ROS Master作为节点管理者，负责节点之间的通信连接，一旦发布者和消息者之间连通后，master关机后两者仍可通信，但无法更改对象
在这里插入图片描述

在learning_topic文件夹下创建msg文件夹，在里面创建Person.msg文件(注意一定要叫msg文件夹，在CMakeLists.txt规则有写)，定义话题消息

1
2
3
4
5
6
7
string name
uint8 sex
uint8 age

uint8 unknown=0
uint8 male=1
uint8 female=2

在功能包的package.xml文件下添加依赖

1
2
<build_depend>message_generation</build_depend> 
<exec_depend>message_runtime</exec_depend>

在CMakeLists.txt文件下添加编译规则，C++和python都需要。找到对应的配置文件下添加编译选项

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
  #添加
  message_generation
)
#添加，配置 msg 源文件
add_message_files(
   FILES
   Person.msg
)
#添加，生成消息时依赖于 std_msgs
generate_messages(DEPENDENCIES
  std_msgs
)
catkin_package(
  #添加，执行时依赖
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
)

最后编译生成相关依赖

1
2
3
cd ~/catkin_ws
#编译
catkin_make

C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)

Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/msg)

1、C++文件

C++文件放置在功能包learning_topic下的src文件夹

为了方便代码提示，可在vscode中的c_cpp_properties.json下的includePath中添加编译好的头文件目录，若存在就无须配置，我的是"/home/shawn/catkin_ws/src/learning_parameter/include/**",

发布者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
/**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
 */
#include <ros/ros.h>
#include "learning_topic/Person.h"
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_publisher");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);
    // 设置循环的频率
    ros::Rate loop_rate(1);
    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_topic::Person类型的消息
    	learning_topic::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = learning_topic::Person::male;
        // 发布消息
		person_info_pub.publish(person_msg);
       	ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", 
				  person_msg.name.c_str(), person_msg.age, person_msg.sex);
        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;
}

订阅者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
 */
#include <ros/ros.h>
#include "learning_topic/Person.h"
// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg->name.c_str(), msg->age, msg->sex);
}
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);
    // 循环等待回调函数
    ros::spin();
    return 0;
}

在CMakeLists.txt文件下的build下添加编译规则

1
2
3
4
5
6
7
add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

编译与运行

1
2
3
4
5
6
7
cd ~/catkin_ws
catkin_make
roscore
rosrun learning_topic person_publisher
rosrun learning_topic person_subscriber
#查看节点图
rosrun rqt_graph rqt_graph
2、python文件

py文件都放置在功能包learning_topic下的scripts文件夹

为了方便代码提示，可在vscode中的settings.json下的python.autoComplete.extraPaths中添加编译好的头文件目录，若存在就无须配置，我的是"/home/shawn/catkin_ws/devel/lib/python3/dist-packages"

发布者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person
import rospy
from learning_topic.msg import Person

def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('person_publisher', anonymous=True)
	# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)
	#设置循环的频率
    rate = rospy.Rate(10) 
    while not rospy.is_shutdown():
		# 初始化learning_topic::Person类型的消息
    	person_msg = Person()
    	person_msg.name = "Tom"
    	person_msg.age  = 18
    	person_msg.sex  = Person.male
		# 发布消息
    	person_info_pub.publish(person_msg)
    	rospy.loginfo("Publsh person message[%s, %d, %d]", 
				person_msg.name, person_msg.age, person_msg.sex)
		# 按照循环频率延时
    	rate.sleep()
if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

订阅者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person
import rospy
from learning_topic.msg import Person

def personInfoCallback(msg):
    rospy.loginfo("Subcribe Person Info: name:%s  age:%d  sex:%d", 
			 msg.name, msg.age, msg.sex)
def person_subscriber():
	# ROS节点初始化
    rospy.init_node('person_subscriber', anonymous=True)
	# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    rospy.Subscriber("/person_info", Person, personInfoCallback)
	# 循环等待回调函数
    rospy.spin()
if __name__ == '__main__':
    person_subscriber()

运行

1
2
3
4
5
6
7
#获得可执行权限
chmod +x ~/catkin_ws/src/learning_topic/scripts/*.py
cd ~/catkin_ws
rosrun learning_topic person_subscriber.py
rosrun learning_topic person_publisher.py 
#查看节点图
rosrun rqt_graph rqt_graph

3、Service服务通信

在这里插入图片描述

下载功能包

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

1、客户端Client编程实现

C++代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
/**
 * 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
 */
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "turtle_spawn");
    // 创建节点句柄
	ros::NodeHandle node;
    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    // 初始化turtlesim::Spawn的请求数据
	turtlesim::Spawn srv;
	srv.request.x = 2.0;
	srv.request.y = 2.0;
	srv.request.name = "turtle2";
    // 请求服务调用
	ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", 
			 srv.request.x, srv.request.y, srv.request.name.c_str());
	add_turtle.call(srv);
	// 显示服务调用结果
	ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());
	return 0;
};

在CMakeLists.txt增加编译规则

1
2
add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

运行

1
2
3
4
5
6
7
cd ~/catkin_ws
#编译
catkin_make
#三个窗口打开
roscore
rosrun turtlesim turtlesim_node
osrun learning_service turtle_spawn

python代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn
import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
	# ROS节点初始化
    rospy.init_node('turtle_spawn')
	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/spawn')
    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn)
		# 请求服务调用，输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")
        return response.name
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)
if __name__ == "__main__":
	#服务调用并显示调用结果
    print("Spwan turtle successfully [name:%s]" %(turtle_spawn()))

运行

1
2
3
4
5
cd ~/catkin_ws
#三个窗口打开
roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_spawn.py

2、服务端Service编程实现

C++代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
/**
 * 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
 */
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>
ros::Publisher turtle_vel_pub;
bool pubCommand = false;
// service回调函数，输入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
         			std_srvs::Trigger::Response &res)
{
	pubCommand = !pubCommand;
    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");
	// 设置反馈数据
	res.success = true;
	res.message = "Change turtle command state!"
    return true;
}
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "turtle_command_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个名为/turtle_command的server，注册回调函数commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);
	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    // 循环等待回调函数
    ROS_INFO("Ready to receive turtle command.");
	// 设置循环的频率
	ros::Rate loop_rate(10);
	while(ros::ok())
	{
		// 查看一次回调函数队列
    	ros::spinOnce();
		// 如果标志为true，则发布速度指令
		if(pubCommand)
		{
			geometry_msgs::Twist vel_msg;
			vel_msg.linear.x = 0.5;
			vel_msg.angular.z = 0.2;
			turtle_vel_pub.publish(vel_msg);
		}
		//按照循环频率延时
	    loop_rate.sleep();
	}
    return 0;
}

python代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger
import rospy
import _thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
def command_thread():	
	while True:
		if pubCommand:
			vel_msg = Twist()
			vel_msg.linear.x = 0.5
			vel_msg.angular.z = 0.2
			turtle_vel_pub.publish(vel_msg)
			
		time.sleep(0.1)
def commandCallback(req):
	global pubCommand
	pubCommand = bool(1-pubCommand)
	# 显示请求数据
	rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)
	# 反馈数据
	return TriggerResponse(1, "Change turtle command state!")
def turtle_command_server():
	# ROS节点初始化
    rospy.init_node('turtle_command_server')
	# 创建一个名为/turtle_command的server，注册回调函数commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)
	# 循环等待回调函数
    print("Ready to receive turtle command.")
    _thread.start_new_thread(command_thread, ())
    rospy.spin()
if __name__ == "__main__":
    turtle_command_server()

其他操作和客户端相似，海龟启动后通过rosservice call /turtle_command命令改变海龟运动状态

3、自定义数据服务

自定义请求和应答内容，通过请求进行控制

在这里插入图片描述

在learning_service文件夹下创建srv文件夹，在里面创建Person.srv文件(注意一定要叫srv文件夹，在CMakeLists.txt规则有写)，定义数据。三杠上方代表Request参数，下方代表Response参数

1
2
3
4
5
6
7
8
string name
uint8 age
uint8 sex
uint8 unknown=0
uint8 male=1
uint8 female=2
---
string result

在功能包的package.xml文件下添加依赖

1
2
<build_depend>message_generation</build_depend> 
<exec_depend>message_runtime</exec_depend>

在CMakeLists.txt文件下添加编译规则，C++和python都需要。找到对应的配置文件下添加编译选项

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
  #找到功能包作为依赖
  message_generation
)
#需要让编译器找到文件在哪
add_service_files(
   FILES
   Person.srv
)
#产生文件依赖
generate_messages(DEPENDENCIES
  std_msgs
)
catkin_package(
  #添加
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
)

最后编译生成相关依赖

1
2
3
cd ~/catkin_ws
#编译
catkin_make
1、C++文件

服务器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/**
 * 该例程将执行/show_person服务，服务数据类型learning_service::Person
 */

#include <ros/ros.h>
#include "learning_service/Person.h"
// service回调函数，输入参数req，输出参数res
bool personCallback(learning_service::Person::Request  &req,
         			learning_service::Person::Response &res)
{
    // 显示请求数据
    ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);
	// 设置反馈数据
	res.result = "OK";
    return true;
}
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个名为/show_person的server，注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);
    // 循环等待回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();
    return 0;
}

客户端

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
/**
 * 该例程将请求/show_person服务，服务数据类型learning_service::Person
 */
#include <ros/ros.h>
#include "learning_service/Person.h"

int main(int argc, char** argv)
{
    // 初始化ROS节点
	ros::init(argc, argv, "person_client");
    // 创建节点句柄
	ros::NodeHandle node;
    // 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
	ros::service::waitForService("/show_person");
	ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");
    // 初始化learning_service::Person的请求数据
	learning_service::Person srv;
	srv.request.name = "Tom";
	srv.request.age  = 20;
	srv.request.sex  = learning_service::Person::Request::male;
    // 请求服务调用
	ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", 
			 srv.request.name.c_str(), srv.request.age, srv.request.sex);
	person_client.call(srv);
	// 显示服务调用结果
	ROS_INFO("Show person result : %s", srv.response.result.c_str());
	return 0;
};

在CMakeLists.txt文件下的build下添加编译规则

1
2
3
4
5
6
7
add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp)

add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

编译运行

1
2
3
4
5
cd ~/catkin_ws
catkin_make
roscore
rosrun learning_service person_server
rosrun learning_service person_client
2、python文件

服务器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将执行/show_person服务，服务数据类型learning_service::Person
import rospy
from learning_service.srv import Person, PersonResponse
def personCallback(req):
	# 显示请求数据
    rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)
	# 反馈数据
    return PersonResponse("OK")
def person_server():
	# ROS节点初始化
    rospy.init_node('person_server')
	# 创建一个名为/show_person的server，注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)
	# 循环等待回调函数
    print("Ready to show person informtion.")
    rospy.spin()
if __name__ == "__main__":
    person_server()

客户端

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务，服务数据类型learning_service::Person
import sys
import rospy
from learning_service.srv import Person, PersonRequest
def person_client():
	# ROS节点初始化
    rospy.init_node('person_client')
	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/show_person')
    try:
        person_client = rospy.ServiceProxy('/show_person', Person)

		# 请求服务调用，输入请求数据
        response = person_client("Tom", 20, PersonRequest.male)
        return response.result
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)
if __name__ == "__main__":
	#服务调用并显示调用结果
    print(Show person result : %s" %(person_client()))

运行

1
2
3
4
5
6
#获得可执行权限
chmod +x ~/catkin_ws/src/learning_service/scripts/*.py
#三个终端打开
roscore
rosrun learning_service person_server
rosrun learning_service person_client

4、全局参数使用与编程

参数服务器以共享的方式实现不同节点之间数据交互的通信模式，存储一些多节点共享的数据，类似于全局变量。

下载功能包

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs
1、C++文件

和前面一样，需要添加编译规则

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
/**
 * 该例程设置/读取海龟例程中的参数
 */
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv)
{
	int red, green, blue;
    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");
    // 创建节点句柄
    ros::NodeHandle node;
    //第二种修改方法
    //node.setParam("")
    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);
	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);
	// 设置背景颜色参数
	ros::param::set("/turtlesim/background_r", 255);
	ros::param::set("/turtlesim/background_g", 255);
	ros::param::set("/turtlesim/background_b", 255);
	ROS_INFO("Set Backgroud Color[255, 255, 255]");
    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);
	ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);
	// 调用服务，刷新背景颜色
	ros::service::waitForService("/clear");
	ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
	std_srvs::Empty srv;
	clear_background.call(srv);
	sleep(1);
    return 0;
}

添加编译规则

1
2
add_executable(parameter_config src/parameter_config.cpp)
target_link_libraries(parameter_config ${catkin_LIBRARIES})

运行

1
2
3
4
5
cd ~/catkin_ws
catkin_make 
roscore 
rosrun turtlesim turtlesim_node 
rosrun learning_parameter parameter_config
2、python文件
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程设置/读取海龟例程中的参数
import sys
import rospy
from std_srvs.srv import Empty
def parameter_config():
	# ROS节点初始化
    rospy.init_node('parameter_config', anonymous=True)
	# 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')
    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)
	# 设置背景颜色参数
    rospy.set_param("/turtlesim/background_r", 255);
    rospy.set_param("/turtlesim/background_g", 255);
    rospy.set_param("/turtlesim/background_b", 255);
    rospy.loginfo("Set Backgroud Color[255, 255, 255]");
	# 读取背景颜色参数
    red   = rospy.get_param('/turtlesim/background_r')
    green = rospy.get_param('/turtlesim/background_g')
    blue  = rospy.get_param('/turtlesim/background_b')
    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)
	# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/clear')
    try:
        clear_background = rospy.ServiceProxy('/clear', Empty)
		# 请求服务调用，输入请求数据
        response = clear_background()
        return response
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)
if __name__ == "__main__":
    parameter_config()

运行

1
2
3
4
cd ~/catkin_ws
roscore
rosrun turtlesim turtlesim_node 
rosrun learning_parameter parameter_config.py

5、launch启动文件

传统启动节点的方法：每启动一个节点都要打开一个新的终端运行一个新的命令，当系统中的节点数量许多时，显然是很不方便的，而且命令的输入也可能会发生错误。
launch启动文件的引入：可以同时启动多个节点，并且可以自动启动ROS Master节点管理器以及实现每个节点的各种配置，为多个节点的操作提供很大的便利。

http://wiki.ros.org/roslaunch/XML

1、基本介绍

< launch >
launch文件中的根元素采用< launch >标签定义,里面包含所有节点的启动配置内容

1
2
#该属性告知用户当前 launch 文件已经弃用
deprecated = "弃用声明"

< node >

启动节点

1
<node pkg="turtlesim" name="sim1" type="turtlesim_node"/>
  • pkg：节点所在的功能包名称
  • type：节点的可执行文件名称
  • name：节点运行时的名称
  • output：“log | screen” (可选)日志发送目标，可以设置为 log 日志文件，或 screen 屏幕,默认是 log
  • respawn, required, ns, args

< param > / < rosparam > 参数
设置ROS系统运行中的参数，存储在参数服务器中。其中不同层次的参数是不一样的，在标签中时，相当于私有命名空间。

1
<param name="output_frame" value="odom"/>
  • name：参数名
  • value：参数值

加载参数文件中的多个参数:

1
2
#command="load | dump | delete" (可选，默认 load)
<rosparam file="params.yaml" command="load" ns="params" />

< arg > 参数
launch文件内部的局部变量，仅限于launch文件使用

1
<arg name="arg-name" default="arg-value" />
  • name：参数名
  • value：参数值

==param设置的参数是存储在ROS参数服务器中的，而arg设置的参数仅限于launch文件中==
< remap > 重映射
重映射ROS计算图资源的命名。

1
<remap from="/turtlebot/cmd_vel" to="/cmd_vel"/>
  • from：原命名
  • to：映射之后的命名

< include > 嵌套
包含其他launch文件，类似C语言中的头文件包含。

1
<include file="$(dirname)/other.launch" />
  • file：包含的其他launch文件路径

< group > 组

  • 标签可以对节点分组，具有 ns 属性，可以让节点归属某个命名空间

< arg > 参数

  • 标签是用于动态传参，类似于函数的参数，可以增强launch文件的灵活性
1
2
3
4
<launch>
    <arg name="xxx" default="xxx"/>
    <param name="param" value="$(arg xxx)" />
</launch>

2、launch简单示例

首先创建功能包，在里面创建launch文件夹，一般launch都放在里面(可自定义文件夹名)

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_launch

简单订阅者发布者

名为simple.launch

1
2
3
4
<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
</launch>
1
2
3
4
5
6
7
8
#需要编译才能识别
cd ~/catkin_ws
catkin_make
#若找不到功能包
#运行rospack profile

#一键启动
roslaunch learning_launch simple.launch

config的launch文件

在config/param.yml文件下

1
2
3
4
5
A: 123
B: "hello"
group:
  C: 456
  D: "hello"

turtlesim_parameter_config.launch文件

1
2
3
4
5
6
7
8
9
<launch>
	<param name="/turtle_number"   value="2"/>
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<param name="turtle_name1"   value="Tom"/>
		<param name="turtle_name2"   value="Jerry"/>
		<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
	</node>
    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>
</launch>

运行，可以看见节点参数的不同，不同位置显示的参数位置也不同

1
2
#回到工作空间目录运行
roslaunch learning_launch turtlesim_parameter_config.launch

在这里插入图片描述

remap使用

1
2
3
4
5
6
<launch>
	<include file="$(find learning_launch)/launch/simple.launch" />
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
	</node>
</launch>

四、通信机制进阶

API文档: http://wiki.ros.org/APIs

1、常用API

1、初始化

C++

1
2
3
4
5
6
7
8
9
10
11
12
13
/*
 * 该函数可以解析并使用节点启动时传入的参数(通过参数设置节点名称、命名空间...) 
 *
 * 该函数有多个重载版本，如果使用NodeHandle建议调用该版本。 
 * \param argc 参数个数
 * \param argv 参数列表
 * \param name 节点名称，需要保证其唯一性，不允许包含命名空间
 * \param options 节点启动选项，被封装进了ros::init_options
 *
 */
void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);
// 如果要一个节点启动多个
ros::init(argc,argv,"cppserver",ros::init_options::AnonymousName);

python

1
2
3
4
5
6
7
8
 """
    在ROS msater中注册节点
    @param name: 节点名称，必须保证节点名称唯一，节点名称中不能使用命名空间(不能包含 '/')
    @type  name: str
    @param anonymous: 取值为 true 时，为节点名称后缀随机编号
    @type anonymous: bool
    """
def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=0, tcpros_port=0)

2、话题和服务

1
2
3
4
5
6
7
8
9
/**
* 在 ROS master 注册并返回一个发布者对象，该对象可以发布消息
* 使用示例如下:
*   ros::Publisher pub = handle.advertise<std_msgs::Empty>("my_topic", 1);
* \param queue_size 等待发送给订阅者的最大消息数量
* \param latch (optional) 如果为 true,该话题发布的最后一条消息将被保存，并且后期当有订阅者连接时会将该消息发送给订阅者
* \return 调用成功时，会返回一个发布对象
*/
Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)

3、回旋函数

spin()和spinOnce()异同

**相同点:**二者都用于处理回调函数；

**不同点:**ros::spin() 是进入了循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数(没有循环)，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行。

4、时间

C++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
//时刻
ros::init(argc,argv,"hello_time");
ros::NodeHandle nh;//必须创建句柄，否则时间没有初始化，导致后续API调用失败
ros::Time right_now = ros::Time::now();//将当前时刻封装成对象
ROS_INFO("当前时刻:%.2f",right_now.toSec());//获取距离 1970年01月01日 00:00:00 的秒数
ROS_INFO("当前时刻:%d",right_now.sec);//获取距离 1970年01月01日 00:00:00 的秒数
ros::Time someTime(100,100000000);// 参数1:秒数  参数2:纳秒
ROS_INFO("时刻:%.2f",someTime.toSec()); //100.10
ros::Time someTime2(100.3);//直接传入 double 类型的秒数
ROS_INFO("时刻:%.2f",someTime2.toSec()); //100.30
//持续时间
ros::Duration du(10);//持续10秒钟,参数是double类型的，以秒为单位
du.sleep();//按照指定的持续时间休眠
ROS_INFO("持续时间:%.2f",du.toSec());//将持续时间换算成秒
ROS_INFO("当前时刻:%.2f",ros::Time::now().toSec());
//PS: time 与 time 不可以相加运算，可以相减
ros::Rate rate(1);//指定频率
rate.sleep();
//定时器
//ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing); 默认自动启动
ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing,true);//只执行一次
// ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing,false,false);//需要手动启动
// timer.start();
ros::spin(); //必须 spin

python

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
# 获取当前时刻
right_now = rospy.Time.now()
rospy.loginfo("当前时刻:%.2f",right_now.to_sec())
rospy.loginfo("当前时刻:%.2f",right_now.to_nsec())
# 自定义时刻
some_time1 = rospy.Time(1234.567891011)
# 从时间创建对象
some_time3 = rospy.Time.from_sec(543.21) # from_sec 替换了 from_seconds
rospy.loginfo("设置时刻3:%.2f",some_time3.to_sec())
#持续时间
du = rospy.Duration(3.3)
# 设置执行频率
rate = rospy.Rate(0.5)
rate.sleep()
#定时器
rospy.Timer(rospy.Duration(1),doMsg)
# rospy.Timer(rospy.Duration(1),doMsg,True) # 只执行一次
rospy.spin()

5、其他函数

在发布实现时，一般会循环发布消息，循环的判断条件一般由节点状态来控制，C++中可以通过 ros::ok() 来判断节点状态是否正常，而 python 中则通过 rospy.is_shutdown() 来实现判断，导致节点退出的原因主要有如下几种:

  • 节点接收到了关闭信息，比如常用的 ctrl + c 快捷键就是关闭节点的信号；
  • 同名节点启动，导致现有节点退出；
  • 程序中的其他部分调用了节点关闭相关的API(C++中是ros::shutdown()，python中是rospy.signal_shutdown())
1
2
3
4
5
6
7
8
9
10
11
12
//C++
ROS_DEBUG("hello,DEBUG"); //不会输出
ROS_INFO("hello,INFO"); //默认白色字体
ROS_WARN("Hello,WARN"); //默认黄色字体
ROS_ERROR("hello,ERROR");//默认红色字体
ROS_FATAL("hello,FATAL");//默认红色字体
//python
rospy.logdebug("hello,debug")  #不会输出
rospy.loginfo("hello,info")  #默认白色字体
rospy.logwarn("hello,warn")  #默认黄色字体
rospy.logerr("hello,error")  #默认红色字体
rospy.logfatal("hello,fatal") #默认红色字体

2、自定义头文件和源文件

1、自定义头文件

在功能包下的 include/功能包名 目录下新建头文件: hello.h

1
2
3
4
5
6
7
8
9
#ifndef _HELLO_H
#define _HELLO_H
namespace hello_ns{
class HelloPub {
public:
    void run();
};
}
#endif

在 VScode 中，为了后续包含头文件时不抛出异常，配置 .vscode 下 c_cpp_properties.json 的 includepath属性

1
"/home/用户/工作空间/src/功能包/include/**"

在 src 目录下新建文件hello.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#include "ros/ros.h"
#include "test_pkg/hello.h"
namespace hello_ns {
void HelloPub::run(){
    ROS_INFO("自定义头文件的使用....");
}
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"test_head_node");
    hello_ns::HelloPub helloPub;
    helloPub.run();
    return 0;
}

最后进行文件的配置和运行

1
2
3
4
5
6
7
8
9
10
11
#取消include注释
include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

add_executable(hello src/hello.cpp)
add_dependencies(hello ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(hello
  ${catkin_LIBRARIES}
)

2、自定义源文件

头文件设置和上一节相似，命名为hello1.h，配置好VS。

在 src 目录下新建文件hello1.cpp，示例内容如下:

1
2
3
4
5
6
7
#include "test_pkg/hello1.h"
#include "ros/ros.h"
namespace hello_ns{
void HelloPub::run(){
    ROS_INFO("hello,head and src ...");
}
}

在 src 目录下新建文件 use_head.cpp，示例内容如下:

1
2
3
4
5
6
7
#include "test_pkg/hello1.h"
#include "ros/ros.h"
namespace hello_ns{
void HelloPub::run(){
    ROS_INFO("hello,head and src ...");
}
}

头文件与源文件相关配置:

1
2
3
4
5
6
7
8
9
10
11
12
13
include_directories(
include
  ${catkin_INCLUDE_DIRS}
)
## 声明C++库
add_library(head
  include/test_pkg/hello1.h
  src/hello1.cpp
)
add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(head
  ${catkin_LIBRARIES}
)

可执行文件配置:

1
2
3
4
5
6
7
add_executable(use_head src/use_head.cpp)
add_dependencies(use_head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#此处需要添加之前设置的 head 库
target_link_libraries(use_head
  head
  ${catkin_LIBRARIES}
)

3、python模块导入

在同一目录下导入另外的py文件，可能会报错：模块未找到。因为rosrun的工作目录时在当前工作空间中

1
2
3
4
5
import os
import sys
path = os.path.abspath(".")
# 核心
sys.path.insert(0,path + "/src/test_pkg/scripts")

五、ROS运行管理

1、元功能包

MetaPackage是Linux的一个文件管理系统的概念。是ROS中的一个虚包，里面没有实质性的内容，但是它依赖了其他的软件包，通过这种方法可以把其他包组合起来，我们可以认为它是一本书的目录索引，告诉我们这个包集合中有哪些子包，并且该去哪里下载。

1、新建一个功能包

**2、修改package.xml **

1
2
3
4
5
<exec_depend>被集成的功能包</exec_depend>
.....
<export>
  <metapackage />
</export>

3、修改 CMakeLists.txt

1
2
3
4
cmake_minimum_required(VERSION 3.0.2)
project(demo)
find_package(catkin REQUIRED)
catkin_metapackage()

PS:CMakeLists.txt 中不可以有换行。

参考文档：http://wiki.ros.org/catkin/package.xml#Metapackages

2、ROS节点重命名

1
2
3
4
5
6
7
#1、重定向命名空间和运行名字可分开
rosrun turtlesim turtlesim_node __ns:=/xxx __name:=tn
#2、launch的重命名
<node pkg="turtlesim" type="turtlesim_node" name="t1" ns="hello"/>
#3、两种方法代码中设置
ros::init(argc,argv,"zhangsan",ros::init_options::AnonymousName);
rospy.init_node("lisi",anonymous=True)

3、ROS话题名称

在 ROS 中节点终端，不同的节点之间通信都依赖于话题，话题名称也可能出现重复的情况，这种情况下，系统虽然不会抛出异常，但是可能导致订阅的消息非预期的，从而导致节点运行异常。这种情况下需要将两个节点的话题名称由相同修改为不同。

话题种类

  • 全局(参数名称直接参考ROS系统，与节点命名空间平级)
  • 相对(参数名称参考的是节点的命名空间，与节点名称平级)
  • 私有(参数名称参考节点名称，是节点名称的子级)

在ROS中提供了一个比较好用的键盘控制功能包: ros-noetic-teleop-twist-keyboard，该功能包，可以控制机器人的运动，作用类似于乌龟的键盘控制节点。如果没有，则sudo apt install ros-noetic-teleop-twist-keyboard进行安装，然后执行: rosrun teleop_twist_keyboard teleop_twist_keyboard.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#1、通过rosrun重命名
rosrun名称重映射语法: rorun 包名 节点名 话题名:=新话题名称
#2、launch 文件设置话题重映射语法
<node pkg="xxx" type="xxx" name="xxx">
    <remap from="原话题" to="新话题" />
</node>
#3、在代码中进行编写
#C++编写
#全局，/chatter
ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter",1000);
#相对，xxx/chatter
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);
#私有，/xxx/hello/chatter
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);
#python编写
pub = rospy.Publisher("/chatter",String,queue_size=1000)
pub = rospy.Publisher("chatter",String,queue_size=1000)
pub = rospy.Publisher("~chatter",String,queue_size=1000)

4、ROS参数名设置

关于参数重名的处理，没有重映射实现，为了尽量的避免参数重名，都是使用为参数名添加前缀的方式，实现类似于话题名称，有全局、相对、和私有三种类型之分。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#命令行设置
rosrun 包名 节点名称 _参数名:=参数值
#launch文件设置
#输出 /p1
#    /t1/p1
<launch>
    <param name="p1" value="100" />
    <node pkg="turtlesim" type="turtlesim_node" name="t1">
        <param name="p2" value="100" />
    </node>
</launch>
#代码设置
#C++
ros::param::set("/set_A",100); #全局,和命名空间以及节点名称无关
ros::param::set("set_B",100); #相对,参考命名空间
ros::param::set("~set_C",100); #私有,参考命名空间与节点名称
#python
rospy.set_param("/py_A",100)  #全局,和命名空间以及节点名称无关
rospy.set_param("py_B",100)  #相对,参考命名空间
rospy.set_param("~py_C",100)  #私有,参考命名空间与节点名称

5、ROS分布式

ROS是一个分布式计算环境。一个运行中的ROS系统可以包含分布在多台计算机上多个节点。根据系统的配置方式，任何节点可能随时需要与任何其他节点进行通信。

1.准备

先要保证不同计算机处于同一网络中，最好分别设置固定IP，如果为虚拟机，需要将网络适配器改为桥接模式；

2.配置文件修改

分别修改不同计算机的 /etc/hosts 文件，在该文件中加入对方的IP地址和计算机名:

1
2
3
4
#主机端:
从机的IP    从机计算机名
#从机端:
主机的IP    主机算机名

设置完毕，可以通过 ping 命令测试网络通信是否正常。

1
2
3
4
#IP地址查看
ifconfig
#计算机名称查看
hostname
3.配置主机IP

配置主机的 IP 地址

~/.bashrc 追加

1
2
export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=主机IP
4.配置从机IP

配置从机的 IP 地址，从机可以有多台，每台都做如下设置:

~/.bashrc 追加

1
2
export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=从机IP

完毕后即可分布式通信

六、ROS常用组件

参考: http://wiki.ros.org/tf2

1、TF坐标变换

创建功能包

1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf2 tf2_ros tf2_geometry_msgs std_msgs geometry_msgs

1、坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped。前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

2、静态坐标建立

使用命令行设置静态坐标变换(推荐)

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

1、C++实现

发布方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //----设置子级坐标系
    ts.child_frame_id = "laser";
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

订阅者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }
        r.sleep();  
        ros::spinOnce();
    }
    return 0;
}
2、python实现

发布方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#! /usr/bin/env python
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_tf_pub_p")
    # 3.创建 静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    # 4.创建并组织被广播的消息
    tfs = TransformStamped()
    # --- 头信息
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # --- 子坐标系
    tfs.child_frame_id = "radar"
    # --- 坐标系相对信息
    # ------ 偏移量
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # ------ 四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    # 5.广播器发送消息
    broadcaster.sendTransform(tfs)
    # 6.spin
    rospy.spin()

订阅者

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#! /usr/bin/env python
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "radar"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    	#5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world")
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    	#6.spin
        rate.sleep()

3、动态坐标建立

1、C++

发布方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";
    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    //turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅到的数据(实现TF广播)       
    // 6.spin
    ros::spin();
    return 0;
}

订阅方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }
        r.sleep();  
        ros::spinOnce();
    }
    return 0;
}
2、python

发布方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
#! /usr/bin/env python
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
#     4.回调函数处理
def doPose(pose):
    #         4-1.创建 TF 广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #         4-2.创建 广播的数据(通过 pose 设置)
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.child_frame_id = "turtle1"
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    #         4-3.广播器发布数据
    broadcaster.sendTransform(tfs)
if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("dynamic_tf_pub_p")
    # 3.订阅 /turtle1/pose 话题消息
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)
    #     4.回调函数处理
    #         4-1.创建 TF 广播器
    #         4-2.创建 广播的数据(通过 pose 设置)
    #         4-3.广播器发布数据
    #     5.spin
    rospy.spin()

订阅方

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#! /usr/bin/env python
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "turtle1"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    	#5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world",rospy.Duration(1))
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    	#6.spin
        rate.sleep()

最后启动即可监听到动态坐标

1
2
3
4
#启动海龟节点
rosrun turtlesim turtlesim_node
#启动海龟控制节点
rosrun turtlesim turtle_teleop_key

4、多坐标点变换

发布方

为了方便，使用静态坐标变换发布

1
2
3
4
<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

订阅方

c++实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
int main(int argc, char *argv[])
{   setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_frames");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer; 
    tf2_ros::TransformListener listener(buffer);
    // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
    ros::Rate r(1);
    while (ros::ok())
    {
        try
        {
        //   解析 son1 中的点相对于 son2 的坐标
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );
            // 坐标点解析
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "son1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;
            geometry_msgs::PointStamped psAtSon2;
            psAtSon2 = buffer.transform(ps,"son2");
            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常信息:%s",e.what());
        }
        r.sleep();
        // 6.spin
        ros::spinOnce();
    }
    return 0;
}

python实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#!/usr/bin/env python
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("frames_sub_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
        # 4.调用 API 求出 son1 相对于 son2 的坐标关系
            #lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("son1 与 son2 相对关系:")
            rospy.loginfo("父级坐标系:%s",tfs.header.frame_id)
            rospy.loginfo("子级坐标系:%s",tfs.child_frame_id)
            rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",
                        tfs.transform.translation.x,
                        tfs.transform.translation.y,
                        tfs.transform.translation.z,
            )
        # 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
            point_source = PointStamped()
            point_source.header.frame_id = "son1"
            point_source.header.stamp = rospy.Time.now()
            point_source.point.x = 1
            point_source.point.y = 1
            point_source.point.z = 1
            point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))

            rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)
            rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z
            )
        except Exception as e:
            rospy.logerr("错误提示:%s",e)
        rate.sleep()
    # 6.spin    
    # rospy.spin()

5、坐标位置查看

可视化

启动后输入rviz，可进入可视化界面，按要求添加即可观察

在这里插入图片描述

命令行

1
2
3
4
#没安装进行安装
sudo apt install ros-noetic-tf2-tools
#生成当前目录的pdf文件
rosrun tf2_tools view_frames.py

6、海龟跟随实战

这里使用python编写，C++和python原理一样

服务客户端(生成新海龟)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#! /usr/bin/env python
"""  
    调用 service 服务在窗体指定位置生成一只乌龟
    流程:
        1.导包
        2.初始化 ros 节点
        3.创建服务客户端
        4.等待服务启动
        5.创建请求数据
        6.发送请求并处理响应
"""
#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("turtle_spawn_p")
    # 3.创建服务客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.创建请求数据
    req = SpawnRequest()
    req.x = 1.0
    req.y = 1.0
    req.theta = 3.14
    req.name = "turtle2"
    # 6.发送请求并处理响应
    try:
        response = client.call(req)
        rospy.loginfo("乌龟创建成功，名字是:%s",response.name)
    except Exception as e:
        rospy.loginfo("服务调用失败....")

发布方(发布两只乌龟坐标信息)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
#! /usr/bin/env python
"""  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.导包
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建订阅对象
        5.回调函数处理订阅的 pose 信息
            5-1.创建 TF 广播器
            5-2.将 pose 信息转换成 TransFormStamped
            5-3.发布
        6.spin
"""
# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    # rospy.loginfo("x = %.2f",pose.x)
    #1.创建坐标系广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #2.将 pose 信息转换成 TransFormStamped
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    #3.广播器发布 tfs
    broadcaster.sendTransform(tfs)


if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.解析传入的命名空间
    rospy.loginfo("-------------------------------%d",len(sys.argv))
    if len(sys.argv) < 2:
        rospy.loginfo("请传入参数:乌龟的命名空间")
    else:
        turtle_name = sys.argv[1]
    rospy.loginfo("///////////////////乌龟:%s",turtle_name)

    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
    #     4.创建订阅对象
    #     5.回调函数处理订阅的 pose 信息
    #         5-1.创建 TF 广播器
    #         5-2.将 pose 信息转换成 TransFormStamped
    #         5-3.发布
    #     6.spin
    rospy.spin()

订阅方(解析坐标信息并生成速度信息)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#! /usr/bin/env python
"""  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.导包
        2.初始化 ros 节点
        3.创建 TF 订阅对象
        4.处理订阅到的 TF
            4-1.查找坐标系的相对关系
            4-2.生成速度信息，然后发布
"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    # 4.处理订阅到的 TF
    rate = rospy.Rate(10)
    # 创建速度发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    while not rospy.is_shutdown():

        rate.sleep()
        try:
            #def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            # rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",
            #             trans.transform.translation.x,
            #             trans.transform.translation.y,
            #             trans.transform.translation.z
            #             )   
            # 根据转变后的坐标计算出速度和角速度信息
            twist = Twist()
            # 间距 = x^2 + y^2  然后开方
            twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
            pub.publish(twist)
        except Exception as e:
            rospy.logwarn("警告:%s",e)

运行(launch文件)

1
2
3
4
5
6
7
8
<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <node pkg="learning_tf" type="generate_new.py" name="turtle2" output="screen"/>
    <node pkg="learning_tf" type="demo04_pub.py" name="tf1_pub" args="turtle1" output="screen"/>
    <node pkg="learning_tf" type="demo04_pub.py" name="tf2_pub" args="turtle2" output="screen"/>
    <node pkg="learning_tf" type="demo04_sub.py" name="tf_sub" output="screen"/>
</launch>

2、rosbag

在ROS中关于数据的留存以及读取实现

命令行

1
2
3
4
5
6
#录制文件
rosbag record -a -O 目标文件
#查看文件
rosbag info 文件名
#回放文件
rosbag play 文件名

代码运行

1、C++实现

1.写 bag

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"bag_write");
    ros::NodeHandle nh;
    //创建bag对象
    rosbag::Bag bag;
    //打开
    bag.open("test.bag",rosbag::BagMode::Write);
    //写
    std_msgs::String msg;
    msg.data = "hello world";
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    //关闭
    bag.close();
    return 0;
}

2.读bag

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"bag_read");
    ros::NodeHandle nh;
    //创建 bag 对象
    rosbag::Bag bag;
    //打开 bag 文件
    bag.open("test.bag",rosbag::BagMode::Read);
    //读数据
    for (rosbag::MessageInstance const m : rosbag::View(bag))
    {
        std_msgs::String::ConstPtr p = m.instantiate<std_msgs::String>();
        if(p != nullptr){
            ROS_INFO("读取的数据:%s",p->data.c_str());
        }
    }
    //关闭文件流
    bag.close();
    return 0;
}
2、python实现

1.写 bag

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#! /usr/bin/env python
import rospy
import rosbag
from std_msgs.msg import String

if __name__ == "__main__":
    #初始化节点 
    rospy.init_node("w_bag_p")
    # 创建 rosbag 对象
    bag = rosbag.Bag("test.bag",'w')
    # 写数据
    s = String()
    s.data= "hahahaha"
    bag.write("chatter",s)
    bag.write("chatter",s)
    bag.write("chatter",s)
    # 关闭流
    bag.close()

2.读bag

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#! /usr/bin/env python
import rospy
import rosbag
from std_msgs.msg import String

if __name__ == "__main__":
    #初始化节点 
    rospy.init_node("w_bag_p")
    # 创建 rosbag 对象
    bag = rosbag.Bag("test.bag",'r')
    # 读数据
    bagMessage = bag.read_messages("chatter")
    for topic,msg,t in bagMessage:
        rospy.loginfo("%s,%s,%s",topic,msg,t)
    # 关闭流
    bag.close()

1、简单介绍与使用

1
2
3
4
5
6
7
8
9
10
11
12
#这里因为ubuntu20没有python命令，所以我复制了/usr/bin中的python3变成python
roslaunch turtle_tf turtle_tf_demo.launch
#这里可能会遇到bug
#TypeError: cannot use a string pattern on a bytes-like object
#sudo gedit /opt/ros/noetic/lib/tf/view_frames 修改89行
#m = r.search(str(vstr))
#文件会保存在当前目录下
rosrun turtlesim turtle_teleop_key
#坐标系命令行工具
rosrun tf tf_echo turtle1 turtle2
#可视化
rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

2、广播与监听编程实现

如何实现一个tf广播器

  • 定义TF广播器（TransformBroadcaster）
  • 创建坐标变换值；
  • 发布坐标变换（sendTransform）

如何实现一个TF监听器

  • 定义TF监听器；（TransformListener）
  • 查找坐标变换；（waitFor Transform、lookupTransform）
1
2
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf2 tf2_ros tf2_geometry_msgs std_msgs geometry_msgs

这里有点问题

3、可视化工具

rqt

rqt的启动方式有两种:

  • 方式1:rqt
  • 方式2:rosrun rqt_gui rqt_gui

在这里插入图片描述

rqt_graph：计算图可视化工具

rqt_console：输出日志信息

rqt_plot：数据绘图工具

rqt_image_view：图像渲染工具

rqt_bag：录制和重放 bag 文件

Rviz：三维可视化工具

GazeBo：三维物理仿真平台

七、机器人系统仿真

1、概述

通过计算机对实体机器人系统进行模拟的技术，在 ROS 中，仿真实现涉及的内容主要有三:对机器人建模(URDF)、创建仿真环境(Gazebo)以及感知环境(Rviz)等系统性实现。

URDF是 Unified Robot Description Format 的首字母缩写，直译为统一(标准化)机器人描述格式，可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度…该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件。

RViz 是 ROS Visualization Tool 的首字母缩写，直译为ROS的三维可视化工具。它的主要目的是以三维方式显示ROS消息，可以将 数据进行可视化表达。例如:可以显示机器人模型，可以无需编程就能表达激光测距仪（LRF）传感器中的传感 器到障碍物的距离，RealSense、Kinect或Xtion等三维距离传感器的点云数据（PCD， Point Cloud Data），从相机获取的图像值等。

Gazebo是一款3D动态模拟器，用于显示机器人模型并创建仿真环境,能够在复杂的室内和室外环境中准确有效地模拟机器人。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。

2、URDF集成Rviz基本流程

实现流程：

  1. 准备:新建功能包，导入依赖
  2. 核心:编写 urdf 文件
  3. 核心:在 launch 文件集成 URDF 与 Rviz
  4. 在 Rviz 中显示机器人模型

1.创建功能包，导入依赖

创建一个新的功能包，名称自定义，导入依赖包:urdf与xacro

在当前功能包下，再新建几个目录:

urdf: 存储 urdf 文件的目录

meshes:机器人模型渲染文件(暂不使用)

config: 配置文件

launch: 存储 launch 启动文件

2.编写 URDF 文件

新建一个子级文件夹:urdf(可选)，文件夹中添加一个demo01.urdf文件,复制如下内容:

1
2
3
4
5
6
7
8
9
<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

3.在 launch 文件中集成 URDF 与 Rviz

在launch目录下，新建一个 launch 文件，该 launch 文件需要启动 Rviz，并导入 urdf 文件，Rviz 启动后可以自动载入解析urdf文件，并显示机器人模型

1
2
3
4
5
6
<launch>
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find 包名)/urdf/urdf/demo01.urdf.urdf" />
    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

4.在 Rviz 中显示机器人模型

在这里插入图片描述

5.优化 rviz 启动

重复启动launch文件时，Rviz 之前的组件配置信息不会自动保存，需要重复执行步骤4的操作，为了方便使用，需要保存配置，在file->save config as保存在之前的config文件夹中。最后在launch文件中添加即可。

1
2
3
4
<launch>
    <param name="robot_description" textfile="$(find 包名)/urdf/urdf/urdf01_HelloWorld.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 报名)/config/rviz/show_mycar.rviz" />
</launch>

3、URDF语法学习

https://wiki.ros.org/urdf/XML

robot标签

  • name: 指定机器人模型的名称

link标签

urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性。比如: 机器人底座、轮子、激光雷达、摄像头…每一个部件都对应一个 link, 在 link 标签内，可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性

在这里插入图片描述

  • name：为连杆命名

    • visual —> 描述外观(对应的数据是可视的)
      • geometry 设置连杆的形状
        • 标签1: box(盒状)
          • 属性:size=长(x) 宽(y) 高(z)
        • 标签2: cylinder(圆柱)
          • 属性:radius=半径 length=高度
        • 标签3: sphere(球体)
          • 属性:radius=半径
        • 标签4: mesh(为连杆添加皮肤)
          • 属性: filename=资源路径(格式:package:////文件)
      • origin 设置偏移量与倾斜弧度
        • 属性1: xyz=x偏移 y便宜 z偏移
        • 属性2: rpy=x翻滚 y俯仰 z偏航 (单位是弧度)
      • metrial 设置材料属性(颜色)
        • 属性: name
        • 标签: color
          • 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
    • collision —> 连杆的碰撞属性
    • Inertial —> 连杆的惯性矩阵
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
<!--  举例，执行需要通过launch-->
<robot name="mycar">
   <link name="base_link">
       <visual>
           <!-- 形状 -->
           <geometry>
           <!-- 长方体的长宽高 -->
           <!-- <box size="0.5 0.3 0.1" /> -->
           <!-- 圆柱，半径和长度 -->
           <!-- <cylinder radius="0.5" length="0.1" /> -->
           <!-- 球体，半径-->
           <!-- <sphere radius="0.3" /> -->
           <mesh filename="package://urdf01_rviz/meshes/autolabor_mini.stl"/>
           </geometry>

        <!-- xyz坐标 rpy翻滚俯仰与偏航角度(3.14=180度 1.57=90度) -->
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <!-- 颜色: r=red g=green b=blue a=alpha -->
        <material name="black">
        	<color rgba="0.7 0.5 0 0.5" />
        </material>
        </visual>
    </link>
</robot>

joint

urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制…,比如:安装在底座上的轮子可以360度旋转，而摄像头则可能是完全固定在底座上。

在这里插入图片描述

  • name —> 为关节命名
  • type —> 关节运动形式
    • continuous: 旋转关节，可以绕单轴无限旋转
    • revolute: 旋转关节，类似于 continues,但是有旋转角度限制
    • prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
    • planer: 平面关节，允许在平面正交方向上平移或旋转
    • floating: 浮动关节，允许进行平移、旋转运动
    • fixed: 固定关节，不允许运动的特殊关节

子级标签

  • parent(必需的)

    parent link的名字是一个强制的属性：

    • link:父级连杆的名字，是这个link在机器人结构树中的名字。

  • child(必需的)

    child link的名字是一个强制的属性：

    • link:子级连杆的名字，是这个link在机器人结构树中的名字。

  • origin

    • 属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。

  • axis

    • 属性: xyz用于设置围绕哪个关节轴运动。

实例

创建demo03_joint.urdf

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
<robot name="mycar">
     <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>
    <!-- 底盘 -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="blue">
                <color rgba="0 0 1.0 0.5" />
            </material>
        </visual>
    </link>
    <!-- 摄像头 -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="red">
                <color rgba="1 0 0 0.5" />
            </material>
        </visual>
    </link>
    <joint name="link2foot" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz="0 0 0.075" rpy="0 0 0" />
    </joint>
    <!-- 关节 -->
    <joint name="camera2baselink" type="continuous">
        <parent link="base_link"/>
        <child link="camera" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz="0.2 0 0.075" rpy="0 0 0" />
        <axis xyz="0 0 1" />
    </joint>
</robot>

创建demo03_joint.launch

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<launch>
      <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo03_joint.urdf" />

    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

 <!-- 添加关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
    <!-- 添加机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <!-- 可选:用于控制关节运动的节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

URDF工具

在 ROS 中，提供了一些工具来方便 URDF 文件的编写，比如在文件所在路径运行:

1
2
3
4
5
6
#检查复杂的 urdf 文件是否存在语法问题
check_urdf xxx.urdf
#查看 urdf 模型结构，显示不同 link 的层级关系
urdf_to_graphiz xxx.urdf
#查看生成的pdf
evince xxx.pdf

工具之前，首先需要安装，安装命令:sudo apt install liburdfdom-tools

4、URDF之xacro

Xacro 是 XML Macros 的缩写，Xacro 是一种 XML 宏语言，是可编程的 XML。通过封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。

参考文档：http://wiki.ros.org/xacro

1、快速体验

在功能包下的urdf->xacro目录下新建demo01_urdf.xacro

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.0015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="base_link_length" value="0.08" />
    <xacro:property name="lidi_space" value="0.015" />
    <!-- 宏 -->
    <xacro:macro name="wheel_func" params="wheel_name flag" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>
        <!-- 3-2.joint -->
        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径
            -->
            <origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>
    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />
</robot>

最后命令行进入 xacro文件 所属目录，执行:rosrun xacro xacro xxx.xacro > xxx.urdf, 会将 xacro 文件解析为 urdf 文件

2、xacro语法学习

属性与算数运算

用于封装 URDF 中的一些字段

1
2
3
4
5
6
7
<!--属性定义-->
<xacro:property name="xxxx" value="yyyy" />
<!--属性调用-->
${属性名称}
<!--算数运算-->
${数学表达式}
${PI / 2}

宏

类似于函数实现，提高代码复用率，优化代码结构，提高安全性

1
2
3
4
5
6
7
<!--宏定义-->
<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">
    .....
    参数调用格式: ${参数名}
</xacro:macro>
<!--宏调用-->
<xacro:宏名称 参数1=xxx 参数2=xxx/>

文件包含

机器人由多部件组成，不同部件可能封装为单独的 xacro 文件，最后再将不同的文件集成，组合为完整机器人

1
2
3
4
5
6
<robot name="xxx" xmlns:xacro="http://wiki.ros.org/xacro">
      <xacro:include filename="my_base.xacro" />
      <xacro:include filename="my_camera.xacro" />
      <xacro:include filename="my_laser.xacro" />
      ....
</robot>

3、xacro模型实现

在xacro文件中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <xacro:property name="PI" value="3.141"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->

    <!-- 底盘 -->
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="${base_footprint_radius}" />
        </geometry>
      </visual>
    </link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
    </link>

    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    </joint>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>
    </xacro:macro>
    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />
    <!-- 支撑轮 -->
    <!-- 支撑轮属性 -->
    <xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->

    <!-- 支撑轮宏 -->
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        </visual>
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      </joint>
    </xacro:macro>
    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />
</robot>

launch文件，注意文件名修改

1
2
3
4
5
6
7
<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/demo05_car_base.urdf.xacro" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />
</launch>

5、Rviz控制机器人运动(arbotix)

实现流程:

  1. 安装 Arbotix
  2. 创建新功能包，准备机器人 urdf、xacro 文件
  3. 添加 Arbotix 配置文件
  4. 编写 launch 文件配置 Arbotix
  5. 启动 launch 文件并控制机器人模型运动

安装

1
sudo apt install ros-noetic-arbotix

添加 arbotix 所需配置文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
          #类型: 差速控制器
       type: diff_controller,
       #参考坐标
       base_frame_id: base_footprint, 
       #两个轮子之间的间距
       base_width: 0.2,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制
       accel_limit: 1.0 
    }
}

launch 示例代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/car.urdf.xacro" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
   <!-- < node> 调用了 arbotix_python 功能包下的 arbotix_driver 节点 -->
   <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
     <!--< rosparam> arbotix 驱动机器人运行时，需要获取机器人信息，可以通过 file 加载配置文件-->
     <rosparam file="$(find urdf01_rviz)/config/control.yml" command="load" />
     <!--< param> 在仿真环境下，需要配置 sim 为 true-->
     <param name="sim" value="true" />
    </node>
</launch>

启动launch后，命令行输入rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'机器人即可成功启动

6、URDF集成Gazebo

1、快速体验

1.创建功能包

创建新功能包，导入依赖包: urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins

2.编写URDF文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
<!-- 
    创建一个机器人模型(盒状即可)，显示在 Gazebo 中 
-->
<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="yellow">
                <color rgba="0.5 0.3 0.0 1" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <origin xyz="0 0 0" />
            <mass value="6" />
            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
        </inertial>
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Black</material>
    </gazebo>

</robot>

注意， 当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:

1.必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。

2.必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。

3.颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项。

3.启动Gazebo并显示模型

launch 文件实现:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" textfile="$(find urdf02_gazebo)/urdf/demo01_hello.urdf" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
    <!-- 
    在 Gazebo 中加载一个机器人模型，该功能由 gazebo_ros 下的 spawn_model 提供:
    -urdf 加载的是 urdf 文件
    -model mycar 模型名称是 mycar
    -param robot_description 从参数 robot_description 中载入模型
    -x 模型载入的 x 坐标
    -y 模型载入的 y 坐标
    -z 模型载入的 z 坐标
-->
</launch>

2、gazebo相关设置

1.collision

如果机器人link是标准的几何体形状，和link的 visual 属性设置一致即可。

2.inertial

惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 xacro 实现):

球体惯性矩阵

1
2
3
4
5
6
7
8
9
<!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

圆柱惯性矩阵

1
2
3
4
5
6
7
8
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

立方体惯性矩阵

1
2
3
4
5
6
7
8
<xacro:macro name="Box_inertial_matrix" params="m l w h">
      <inertial>
              <mass value="${m}" />
              <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                  iyy="${m*(w*w + l*l)/12}" iyz= "0"
                  izz="${m*(w*w + h*h)/12}" />
      </inertial>
  </xacro:macro>

需要注意的是，原则上，除了 base_footprint 外，机器人的每个刚体部分都需要设置惯性矩阵，且惯性矩阵必须经计算得出，如果随意定义刚体部分的惯性矩阵，那么可能会导致机器人在 Gazebo 中出现抖动，移动等现象。

3.颜色设置

在 gazebo 中显示 link 的颜色，必须要使用指定的标签:

1
2
3
<gazebo reference="link节点名称">
     <material>Gazebo/Blue</material>
</gazebo>

**PS：**material 标签中，设置的值区分大小写，颜色可以设置为 Red Blue Green Black …

3、仿真环境的搭建与使用

对于使用已搭建完的世界，在空世界中包含world文件即可

1
2
3
<include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find demo02_urdf_gazebo)/worlds/hello.world" />
</include>

对于环境的搭建(要占用大量电脑资源)

  • 方式1: 直接添加内置组件创建仿真环境
  • 方式2: 手动绘制仿真环境(更为灵活)

7、URDF、Rviz和Gazebo综合使用

关于URDF(Xacro)、Rviz 和 Gazebo 三者的关系。 URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息，Gazebo 用于仿真，可以模拟外界环境，以及机器人的一些传感器
参考文档： http://gazebosim.org/tutorials?tut=ros_gzplugins

八、机器人导航

http://wiki.ros.org/navigation

1、导航概述

全局地图、自身定位、路径规划、运动控制、环境感知

2、导航实现

1、准备工作

  • 安装 gmapping 包(用于构建地图):sudo apt install ros-noetic-gmapping
  • 安装地图服务包(用于保存与读取地图):sudo apt install ros-noetic-map-server
  • 安装 navigation 包(用于定位以及路径规划):sudo apt install ros-noetic-navigation

最后创建新功能包，导入依赖gmapping map_server amcl move_base

2、SLAM建图

官方文档

创建launch文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
<launch>
<param name="use_sim_time" value="true"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/><!-- 雷达话题 -->
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <!--
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav_sum)/rviz/gmapping.rviz"/>
    -->
</launch>

首先启动 Gazebo 仿真环境

再启动地图绘制的 launch 文件:

然后启动键盘键盘控制节点，用于控制机器人运动建图

1
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

最后在 rviz 中添加组件，显示栅格地图

3、地图服务

依次启动仿真环境，键盘控制节点与SLAM节点，绘制地图后启动保存地图的launch文件，即可保存地图信息

1
2
3
4
<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

对于地图的获取，启动新的launch文件，即可发布map地图信息了，之后只需要在rviz订阅/map即可

1
2
3
4
5
6
<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
</launch>

nav.yaml部分详解

1
2
3
4
5
6
7
8
9
10
11
12
#1.声明地图图片资源的路径
image: /home/shawn/catkin_demo/src/nav_demo/map/nav.pgm
#2.地图刻度尺单位是米/像素
resolution: 0.050000
#3.地图的位姿（相对于rViz中的原点的位姿）
origin: [-50.000000，-50.000000，0.0]
#4.占用阈值
occupied thresh: 0.65
#5空闲间值
free thresh: 0.196
#6.取反
negate: 0

4、定位

编写launch文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>

  <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
  <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
  <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

测试launch文件

1
2
3
4
5
6
7
8
9
10
<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find nav_demo)/launch/amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>
执行

1.先启动 Gazebo 仿真环境

2.启动键盘控制节点：

1
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；

4.在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5、路径规划

创建nav05_path.launch文件

1
2
3
4
5
6
7
8
9
<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" />
    </node>
</launch>

costmap_common_params.yaml

该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

该文件用于全局代价地图参数设置:

1
2
3
4
5
6
7
8
9
10
global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

local_costmap_params.yaml

该文件用于局部代价地图参数设置:

1
2
3
4
5
6
7
8
9
10
11
12
13
local_costmap:
  global_frame: odom #里程计坐标系
  robot_base_frame: base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

base_local_planner_params

基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

最后文件集成

运行仿真环境、运行launch文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
       <!-- 关节以及机器人状态发布节点 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz" />
</launch>

6、导航与SLAM建图

1.编写launch文件

当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点，示例内容如下:

1
2
3
4
5
6
7
8
<launch>
    <!-- 启动SLAM节点 -->
    <include file="$(find nav_demo)/launch/nav01_slam.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
    <!-- 运行rviz -->
    <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz" /> -->
</launch>

2.测试

1.首先运行gazebo仿真环境；

2.然后执行launch文件；

3.在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；

4.最后可以使用 map_server 保存地图。

九、学习资料

ROS: https://www.ros.org
ROS Wiki: http://wiki.ros.org
ROSCon 2012~2019：https://roscon.ros.org
ROS Robots: https://robots.ros.org
Ubuntu Wiki: https://wiki.ubuntu.org.cn
古月居：http://www.guyuehome.com
古月居泡泡：https://www.guyuehome.com/Bubble
古月学院：https://class.guyuehome.com
如何学习ROS：
https://mp.weixin.qq.com/s/uYvGuiG-TlOalWUynR2Nzg
一起从零手写URDF模型：
https://class.guyuehome.com/detail/p5eleea4fe1e5c_lgm126Xn/6
如何从Solidworks导出URDF模型
https://class.guyuehome.com/detail/p5e32dce7906e0_6TqS7BwX/6
如何在Gazebo中实现移动机器人仿真：
https://class.guyuehome.com/detail/p_5eb2366befe4aE4rbNmXt/6
Movelt可视化配置及仿真指南
https://class.guyuehome.com/detail/p_5e71966b3fdfd_g4DpRGg9/6

赏

谢谢你请我吃糖果

支付宝
-------------本文结束^-^感谢您的阅读-------------
本文作者： Shawn
发布时间： 2022-05-09
最后更新： 2023-03-17
本文标题： ROS Noetic入门完整版
本文链接： https://blog.shawncoding.top/posts/83cf3815.html
版权声明： 本作品采用 CC BY-NC-SA 4.0 许可协议进行许可。转载请注明出处！
知识共享许可协议

扫一扫，分享到微信

微信分享二维码

tag:

    缺失模块。
    1、请确保node版本大于6.2
    2、在博客根目录（注意不是yilia根目录）执行以下命令：
    npm i hexo-generator-json-content --save

    3、在根目录_config.yml里添加配置： 

      jsonContent:
    meta: false
    pages: false
    posts:
      title: true
      date: true
      path: true
      text: false
      raw: false
      content: false
      slug: false
      updated: false
      comments: false
      link: false
      permalink: false
      excerpt: false
      categories: false
      tags: true

<br/><br/><br/>心怀天下<br/>只认真生活<br/>自信微笑面对未来