代码模板

c++ 模板

1
2
3
4
5
6
7
8
9
10
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"");
    ros::NodeHandle nh;

    return 0;
}

python 模板

1
2
3
4
5
6
7
#! /usr/bin/env python

import rospy

if __name__ == "__main__":
	rospy.init_node("")

ROS环境搭建与初认识

ROS安装

具体细节看教程 1.2 ROS安装

采用虚拟机安装 ubuntu，再安装 ROS

虚拟机软件：virtualbox(免费) 官网下载软件安装包以及拓展包

ubuntu版本：ubuntu-20.04.6 镜像文件下载地址

在下载过程中可能需要修改软件与更新中的下载自(以后安装库的时候经常会碰上)

在虚拟机中如果出现右键没有反应的情况，按住shift再点

ROS版本：Noetic

1
sudo apt install ros-noetic-desktop-full

配置环境变量，方便在任意终端中使用 ROS：

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

安装构建依赖：

1
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

测试ROS安装环境：

  1. 启动三个命令行(ctrl + alt + T)
  2. 命令行1键入:roscore
  3. 命令行2键入:rosrun turtlesim turtlesim_node(此时会弹出图形化界面)
  4. 命令行3键入:rosrun turtlesim turtle_teleop_key(可以控制小乌龟运动)

以下命令行内容中[ ]包围的为替换内容

补充ubuntu创建右键快捷键方法：

  1. 在用户主目录里找到“模板“文件夹
  2. 右键在终端打开，sudo gedit 文本文件
  3. 保存，可在任何文件夹下创建文本文件了

ROS集成开发环境

在刚刚的过程会发现ubuntu频繁需要密码，打开terminal，输入seahorse，点击左上角➕号，新建一个空钥匙环，建立完成后设置为默认，打开一些软件不再提示输入密码了

终端

在 ROS 中，需要频繁的使用到终端，且可能需要同时开启多个窗口，推荐使用Terminator

安装：

1
sudo apt install terminator

常用快捷键：

1
2
3
4
5
6
Alt+Up                          //移动到上面的终端
Alt+Down                        //移动到下面的终端
Alt+Left                        //移动到左边的终端
Alt+Right                       //移动到右边的终端
Ctrl+Shift+O                    //水平分割终端
Ctrl+Shift+E                    //垂直分割终端

VScode

下载：vscode 下载 选择适用于linux系统的.deb文件

安装：

  • 方式1：双击安装即可(或右击选择安装) [可能会出现没反应或闪退行为]
  • 方式2：sudo dpkg -i xxxx.deb 复制具体的下载文件名称

vscode 下载插件：c++，CMake，python，ROS

快捷键配置：向上向下复制行；向上向下移动

vscode 使用_基本配置：

  1. 创建 ROS 工作空间

    1
    2
    3
    mkdir -p [工作空间名称]/src(必须得有 src)
    cd [工作空间名称]
    catkin_make

  2. 启动 vscode

    1
    code .

  3. vscode 中编译 ros

    快捷键 ctrl + shift + B 调用编译，选择catkin_make:build右边的小齿轮配置

    修改.vscode/tasks.json 文件

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    {
        "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"
            }
        ]
    }

  4. 创建 ROS 功能包

    选定 src 右击 —> create catkin package

    设置包名(helloworld) 添加依赖(roscpp rospy std_msgs)

  5. C++ 实现

    在功能包的 src 下新建 cpp 文件

    1
    2
    3
    4
    5
    6
    7
    8
    9
    #include "ros/ros.h"
    int main(int argc, char *argv[])
    {
        // 解决中文乱码
        setlocale(LC_ALL,"");
        ros::init(argc,argv,"hello_c");
        ROS_INFO("hello vscode");
        return 0;
    }

    如果没有代码提示

    修改 .vscode/c_cpp_properties.json，设置 “cppStandard”: “c++17”

    则ctrl+shift+空格提示函数格式

  6. python 实现

    在 功能包 下新建 scripts 文件夹，添加 python 文件

    1
    2
    3
    4
    5
    #! /usr/bin/env python
    import rospy
    if __name__ == "__main__":
        rospy.init_node("hello_p")
        rospy.loginfo("hello vscode! it's python")

    在scripts文件夹下打开终端并添加可执行权限

    1
    chmod +x *.py

  7. 配置 CMakeLists.txt

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    add_executable([自定义名称]
      src/helloworld_c.cpp
    )
    target_link_libraries([自定义名称]
      ${catkin_LIBRARIES}
    )

    catkin_install_python(PROGRAMS scripts/[文件名].py
      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
    )

  8. 编译执行

    编译：ctrl + shift + B

    执行：在 VScode 中添加终端

    1
    roscore
    1
    source ./devel/setup.bash

    c++：rosrun [包名] [自定义名称]

    python：rosrun [包名] [文件名称]

    如果不编译直接执行 python 文件，会抛出异常

    解决方案：

    1. 第一行解释器声明，可以使用绝对路径定位到 python3 的安装路径 #! /usr/bin/python3(不建议)
    2. 创建一个链接符号到 python 命令：sudo ln -s /usr/bin/python3 /usr/bin/python(建议)

launch文件

解决需要启动多个节点的效率问题

实现：

  1. 选定功能包右击 —> 添加 launch 文件夹

  2. 选定 launch 文件夹右击 —> 添加 launch 文件

  3. 编辑 launch 文件内容

    以小乌龟示例程序启动为例：

    1
    2
    3
    4
    5
    <launch>
        <!-- 乌龟GUI -->
        <node pkg="turtlesim" type="turtlesim_node" name="turtle_GUI" />
        <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_key" />
    </launch>
    • node —> 包含的某个节点
    • pkg —–> 功能包
    • type —-> 被运行的节点文件
    • name –> 为节点命名
    • output-> 设置日志的输出目标

  4. 运行 launch 文件

    1
    roslaunch [功能包名] [launch文件名]

  5. 运行结果：一次性启动了多个节点

ROS架构

文件系统

ROS_file 
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
WorkSpace --- 自定义的工作空间

    |--- build:编译空间，用于存放CMake和catkin的缓存信息、配置信息和其他中间文件。

    |--- devel:开发空间，用于存放编译后生成的目标文件，包括头文件、动态&静态链接库、可执行文件等。

    |--- src: 源码

        |-- package：功能包(ROS基本单元)包含多个节点、库与配置文件，包名所有字母小写，只能由字母、数字与下划线组成

            |-- CMakeLists.txt 配置编译规则，比如源文件、依赖项、目标文件

            |-- package.xml 包信息，比如:包名、版本、作者、依赖项...(以前版本是 manifest.xml)

            |-- scripts 存储python文件

            |-- src 存储C++源文件

            |-- include 头文件

            |-- msg 消息通信格式文件

            |-- srv 服务通信格式文件

            |-- action 动作格式文件

            |-- launch 可一次性运行多个节点 

            |-- config 配置信息

        |-- CMakeLists.txt: 编译的基本配置

文件系统相关命令

  1. 增

    catkin_create_pkg 自定义包名 依赖包 === 创建新的ROS功能包

    sudo apt install xxx === 安装 ROS功能包

  2. 删

    sudo apt purge xxx === 删除某个功能包

  3. 查

    rospack list === 列出所有功能包

    rospack find 包名 === 查找某个功能包是否存在，如果存在返回安装路径

    roscd 包名 === 进入某个功能包

    rosls 包名 === 列出某个包下的文件

    apt search xxx === 搜索某个功能包

  4. rosed 包名 文件名 === 修改功能包文件

    需要安装 vim

    比如:rosed turtlesim Color.msg

  5. 执行

    roscore === 是 ROS 的系统先决条件节点和程序的集合， 必须运行 roscore 才能使 ROS 节点进行通信

    roscore 将启动:

    • ros master
    • ros 参数服务器
    • rosout 日志节点

    rosrun 包名 可执行文件名 === 运行指定的ROS节点

    比如:rosrun turtlesim turtlesim_node

    roslaunch 包名 launch文件名 === 执行某个包下的 launch 文件

计算图

rqt_graph能够创建一个显示当前系统运行情况的动态图形

计算图可以以点对点的网络形式表现数据交互过程

运行程序后，启动新终端，键入:

1
rqt_graph
1
rosrun rqt_graph rqt_graph

可以看到网络拓扑图，显示不同节点之间的关系

ROS通信机制

话题通信

话题通信是ROS中使用频率最高的一种通信模式，基于发布订阅模式，即一个节点发布消息，另一个节点订阅该消息

以激光雷达信息的采集处理为例，在 ROS 中有一个节点需要时时的发布当前雷达采集到的数据，导航模块中也有节点会订阅并解析雷达数据

以此类推，像雷达、摄像头、GPS…. 等等一些传感器数据的采集都使用了话题通信

话题通信适用于不断更新、少逻辑处理的数据传输场景

image-20250519215104543_compressed (1)
  • 在工作空间下功能包中，基本每一个可执行文件都会初始化一个节点(node)，并唯一命名
  • 节点之间通过话题(topic)进行通信
  • 话题类型就是发布的消息(msg)
  • 传输的内容需要关注消息类型

普通文本

需求: 实现基本的话题通信，一方发布数据，一方接收数据

c++实现

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注：

  1. 发布方
  2. 接收方
  3. 数据

发布方：

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
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

/*  
    发布方实现：
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据
*/
int main(int argc, char *argv[])
{   
    // 设置编码格式
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"talker");
    // 3.创建节点句柄
    ros::NodeHandle nh; //该类封装了 ROS 中的一些常用功能
    // 4.创建发布者对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，早接受到的销毁
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);
    // 5.编写发布逻辑并发布数据
    // 创建发布消息
    std_msgs::String msg;
    // 分布频率
    ros::Rate rate (10);
    // 设置编号
    int count = 0;
    // 编写循环
    while (ros::ok())
    {
        // 实现字符串拼接
        std::stringstream ss;
        ss << "hello ---"<< count;
        msg.data = ss.str();
        pub.publish(msg);
        //  添加日志
        ROS_INFO("发布的数据: %s",ss.str().c_str());
        count++;
        //根据前面制定的发送频率自动休眠
        rate.sleep();
        ros::spinOnce(); 
    }
    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
#include "ros/ros.h"
#include "std_msgs/String.h"

/*  
    订阅方实现：
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.创建节点句柄
        4.创建订阅者对象
        5.处理订阅数据
        6.设置循环调用回调函数

*/

void doMsg(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("我听见: %s",msg->data.c_str());
}

int main(int argc, char *argv[])
{
    // 设置编码格式
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"listener");
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("chatter",10,doMsg);
    // 5.处理订阅数据
    ros::spin(); // 循环读取接收的数据，并调用回调函数处理
   
    return 0;
}

ConstPtr 是 boost::shared_ptr（智能指针），即 msg 是一个指针

必须用 -> 访问其成员的 data 字段

配置CmakeList.txt

1
2
3
4
5
6
7
8
9
add_executable(demo01_pub src/demo01_pub.cpp)
add_executable(demo02_sub src/demo02_sub.cpp)
target_link_libraries(demo01_pub
  ${catkin_LIBRARIES}
)

target_link_libraries(demo02_sub
  ${catkin_LIBRARIES}
)

vscode 中的 main 函数 声明 int main(int argc, char const *argv[]){}，默认生成 argv 被 const 修饰，需要去除该修饰符

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
#! /usr/bin/env python
""" 
实现流程:
    1.导包 
    2.初始化 ROS 节点:命名(唯一)
    3.实例化 发布者 对象
    4.组织被发布的数据，并编写逻辑发布数据
"""

import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("talker_p")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("chatter_p",String,queue_size= 10 )
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()
    #指定发布频率
    rate = rospy.Rate(1)
    #设置计数器
    count = 0
    #循环发布数据
    while not rospy.is_shutdown():
        count+=1
        msg.data = "hello---"+str(count)
        #发布数据
        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写入的数据：%s",msg.data)

订阅方：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
#! /usr/bin/env python
""" 
    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 订阅者 对象
        4.处理订阅的消息(回调函数)
        5.设置循环调用回调函数
 """

import rospy
from std_msgs.msg import String

def doMsg(msg):
    rospy.loginfo("I heard:%s",msg.data)

if __name__ == "__main__":
    # 2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("listener_p")
    # 3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter_p",String,doMsg,queue_size=10)
    # 4.处理订阅的消息(回调函数)  
    # 5.设置循环调用回调函数   
    rospy.spin()

添加可执行权限

终端下进入 scripts 执行：chmod +x *.py

配置 CMakeLists.txt：

1
2
3
4
5
catkin_install_python(PROGRAMS
  scripts/demo01_pub_p.py
  scripts/demo02_sub_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

自定义msg

msgs只是简单的文本文件，每行具有字段类型和字段名称

如果需要自定义消息类型，则需要自定义msg

流程：

  1. 按照固定格式创建 msg 文件
  2. 编辑配置文件
  3. 编译生成可以被 Python 或 C++ 调用的中间文件

创建自定义消息

  1. 定义msg文件

    功能包下新建 msg 目录，添加文件 Person.msg

    1
    2
    3
    string name
    uint16 age
    float64 height

  2. 编辑配置文件

    package.xml中添加编译依赖与执行依赖

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

    CMakeLists.txt编辑 msg 相关配置

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    find_package(catkin REQUIRED COMPONENTS
      roscpp
      rospy
      std_msgs
      message_generation
    )
    # 需要加入 message_generation,必须有 std_msgs
    ...
    # 配置 msg 源文件
    add_message_files(
      FILES
      Person.msg
    )
    ...
    # 执行时依赖
    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES demo02_talker_listener
      CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
    #  DEPENDS system_lib
    )

    编译依赖find_package内的包

    find_package依赖catkin_package内的包

    如果catkin_package内的包不对，可能编译成功但运行失败

  3. 编译

    编译后查看

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

    Python 需要调用的中间文件(…/工作空间/devel/lib/python3/msg/xxx.py)

c++实现

需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性

发布方：

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
#include "ros/ros.h"
#include "plumbing_pub_sub/Person.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("发布方：");

    ros::init(argc,argv,"talker_person");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<plumbing_pub_sub::Person>("chatter_person",1000);
    plumbing_pub_sub::Person p;
    p.name = "孙悟空";
    p.age = 2000;
    p.height = 1.45;
    
    ros::Rate r(1);
    while (ros::ok())
    {
        pub.publish(p);
        p.age += 1;
        ROS_INFO("我叫:%s,今年%d岁,高%.2f米", p.name.c_str(), p.age, p.height);

        r.sleep();
        ros::spinOnce();
    }
    return 0;
}

订阅方：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#include "ros/ros.h"
#include "plumbing_pub_sub/Person.h"

void doPerson(const plumbing_pub_sub::Person::ConstPtr& person)
{
    ROS_INFO("订阅的人信息:%s,%d,%.2f",person->name.c_str(),person->age,person->height);
} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("订阅方：");

    ros::init(argc,argv,"listener_person");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("chatter_person",1000,doPerson);
    ros::spin();
    return 0;
}

配置CMakeList：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
add_executable(person_talker src/person_talker.cpp)
add_executable(person_listener src/person_listener.cpp)

// 额外配置，避免编译报错
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(person_talker ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(person_listener ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(person_talker
  ${catkin_LIBRARIES}
)
target_link_libraries(person_listener
  ${catkin_LIBRARIES}
)

add_dependencies如果考虑简单，可以仅将注释的内容

1
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

中${PROJECT_NAME}_node处改为节点名称，其它保留原来格式也是可以的

python实现

将前面生成的 python 文件路径配置进 settings.json

1
2
3
4
"python.autoComplete.extraPaths": [
    "/opt/ros/noetic/lib/python3/dist-packages",
    "/home/ros/demo03_ws/devel/lib/python3/dist-packages" //添加的
],

发布方：

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

if __name__ == "__main__":
    rospy.init_node("talker_person_p")
    pub = rospy.Publisher("chatter_person",Person,queue_size=1000)
    p = Person()
    p.name = "葫芦娃"
    p.age = 18
    p.height =0.75

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        pub.publish(p)
        rate.sleep()
        rospy.loginfo("姓名:%s, 年龄:%d, 身高:%.2f",p.name,p.age,p.height)

订阅方：

1
2
3
4
5
6
7
8
9
10
11
#! /usr/bin/env python
import rospy
from plumbing_pub_sub.msg import  Person

def doPerson(p):
    rospy.loginfo("订阅信息：%s,%d,%.2f",p.name,p.age,p.height)

if __name__ == "__main__":
    rospy.init_node("listener_person_p")
    sub = rospy.Subscriber("chatter_person",Person,doPerson,queue_size=1000)
    rospy.spin()

终端下进入 scripts 执行:chmod +x *.py

配置 CMakeLists.txt：

1
2
3
4
5
6
7
catkin_install_python(PROGRAMS
  scripts/talker_p.py
  scripts/listener_p.py
  scripts/person_talker.py
  scripts/person_listener.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

服务通信

服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制

即一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A

服务通信更适用于对时时性有要求、具有一定逻辑处理的应用场景

Snipaste_2025-05-22_16-29-54_compressed

案例：

实现两个数字的求和，客户端节点，运行会向服务器发送两个数字，服务器端节点接收两个数字求和并将结果响应回客户端

自定义srv

srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似:

  1. 按照固定格式创建srv文件
  2. 编辑配置文件
  3. 编译生成中间文件

流程：

  1. 定义srv文件

    功能包下新建 srv 目录，添加 xxx.srv 文件，请求和响应数据类型之间用---分开

    1
    2
    3
    4
    5
    6
    # 客户端请求时发送的两个数字
    int32 num1
    int32 num2
    ---
    # 服务器响应发送的数据
    int32 sum

  2. 编译配置文件

    package.xml中添加编译依赖与执行依赖

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

    同话题通信自定义

    CMakeLists.txt编辑 srv 相关配置

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    find_package(catkin REQUIRED COMPONENTS
      roscpp
      rospy
      std_msgs
      message_generation
    )
    # 需要加入 message_generation,必须有 std_msgs

    add_service_files(
      FILES
      AddInts.srv
    )

    generate_messages(
      DEPENDENCIES
      std_msgs
    )

    官网示例没有在 catkin_package 中配置 message_runtime，经测试配置也可以

  3. 编译，查看包方式同之前

需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性

  "includePath": [
    "/opt/ros/noetic/include/**",
    "/usr/include/**",
    "/home/ros/demo03_ws/devel/include/**" //增加的
  ],

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
/*
    服务器实现:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建 服务 对象
        5.回调函数处理请求并产生响应
        6.由于请求有多个，需要调用 ros::spin()
*/

#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"

bool doNums(plumbing_server_client::AddInts::Request &req,
            plumbing_server_client::AddInts::Response &resp){
    int num1 = req.num1;
    int num2 = req.num2;
    ROS_INFO("服务器接收到的数据为: num1 = %d,num2 = %d",num1,num2);
    int sum = num1 + num2 ;
    resp.sum = sum;
    ROS_INFO("处理结果: sum = %d",sum);
    return true;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("服务器启动");
    ros::init(argc,argv,"AddInts_Server");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("AddInts",doNums);
    ros::spin();
}

服务端的回调函数的参数是 demo03_server_client::AddInts::Request& req，没有ConstPtr， req 是请求对象的引用，不是指针

引用 (req) 的行为和对象实例一致，直接用 . 访问成员，因为服务是“一问一答”模式，不需要共享所有权，直接修改传入的请求和响应对象即可

话题通信中将接收到的消息封装为 boost::shared_ptr（智能指针）传递给回调函数，目的是为了高效管理内存（避免拷贝）

客户端：

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
/*
    服务器实现:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建 客户端 对象
        5.请求服务，接收响应
    实现参数的动态提交：
        1. 格式 rosrun ..... (参数)
        2. 节点执行时，需要获取命令中的参数，并组织进 request
    问题：
        如果先启动客户端，会请求异常
    需求：
        先启动客户端，不直接抛出异常，挂起，等待服务器启动再请求
    解决：
        ROS中内置函数，可以让客户端启动后挂起等待
*/

#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 优化实现，获取命令中的参数
    // 如果不传递任何ROS参数，argc 的值为 1（只有程序名本身）
    if(argc != 3)
    {
        ROS_INFO("提交的参数个数不对");
        return 1;
    }
    ros::init(argc,argv,"AddInts_Client");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("AddInts");
    // 5.请求服务，接收响应
    plumbing_server_client::AddInts ai;
    // 组织请求
    ai.request.num1 = atoi(argv[1]);
    ai.request.num2 = atoi(argv[2]);
    // 处理响应
    // 函数1，等到服务启动后才继续
    // client.waitForExistence();
    // 函数2，需要传入具体的服务话题
    ros::service::waitForService("AddInts"); 
    bool flag = client.call(ai);
    if(flag)
    {
        ROS_INFO("响应成功");
        ROS_INFO("响应结果 = %d",ai.response.sum);
    } else {
        ROS_INFO("响应失败");
    }

}

配置CMakeLists

1
2
3
4
5
6
7
8
9
10
11
12
add_executable(AddInts_Server src/AddInts_Server.cpp)
add_executable(AddInts_Client src/AddInts_Client.cpp)

add_dependencies(AddInts_Server ${PROJECT_NAME}_gencpp)
add_dependencies(AddInts_Client ${PROJECT_NAME}_gencpp)

target_link_libraries(AddInts_Server
  ${catkin_LIBRARIES}
)
target_link_libraries(AddInts_Client
  ${catkin_LIBRARIES}
)

python

服务端：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#! /usr/bin/env python
import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse

def doReq(req):
    num1 = req.num1
    num2 = req.num2
    sum = num1 + num2
    resp = AddIntsResponse()
    resp.sum = sum
    rospy.loginfo("服务器解析结果:%d",sum)
    return resp

if __name__ == "__main__":
    rospy.init_node("AddInts_Server_p")
    server = rospy.Service("AddInts",AddInts,doReq)
    rospy.loginfo("服务器启动")
    rospy.spin()

客户端：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#! /usr/bin/env python
import rospy
from plumbing_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
import sys


if __name__ == "__main__":
    #优化实现
    if len(sys.argv) != 3:
        rospy.logerr("请正确提交参数")
        sys.exit(1)

    rospy.init_node("AddInts_Client_p")
    client = rospy.ServiceProxy("AddInts",AddInts)
    client.wait_for_service()
    req = AddIntsRequest()
    req.num1 = int(sys.argv[1])
    req.num2 = int(sys.argv[2])

    resp = client.call(req)
    rospy.loginfo("响应结果:%d",resp.sum)

配置CMakeLists

1
2
3
4
5
catkin_install_python(PROGRAMS
  scripts/demo01_server_p.py
  scripts/demo02_client_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

参数服务器

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

案例：实现参数增删改查操作

ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数

03ROS通信机制03_参数服务器

注意：参数服务器不是为高性能而设计的，因此最好用于存储静态的非二进制的简单数据

c++

  • 参数服务器新增(修改)参数

实现参数服务器数据的增删改查，可以通过两套 API 实现:

1
2
3
4
5
6
/*   
	ros::NodeHandle
        setParam("键","值")
    ros::param
        set("键","值")
*/        
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
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"set_param_c");
    std::vector<std::string> stus;
    stus.push_back("zhangsan");
    stus.push_back("lisi");

    std::map<std::string,std::string> friends;
    friends["guo"] = "huang";
    friends["yuang"] = "xiao";
    //NodeHandle--------------------------------------------------------
    ros::NodeHandle nh;
    nh.setParam("nh_int",10); //整型
    nh.setParam("nh_double",3.14); //浮点型
    nh.setParam("nh_bool",true); //bool
    nh.setParam("nh_string","hello NodeHandle"); //字符串
    nh.setParam("nh_vector",stus); // vector
    nh.setParam("nh_map",friends); // map
    //param--------------------------------------------------------
    ros::param::set("param_int",20);
    ros::param::set("param_double",3.14);
    ros::param::set("param_string","Hello Param");
    ros::param::set("param_bool",false);
    ros::param::set("param_vector",stus);

    return 0;
}
  • 参数服务器获取参数

在 roscpp 中提供了两套 API 实现参数操作

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
/* 
	ros::NodeHandle

        param(键,默认值) 
            存在，返回对应结果，否则返回默认值
				int res1 = nh.param("nh_int",100); 

        getParam(键,存储结果的变量)
            存在,返回 true,且将值赋值给参数2
            如果键不存在，那么返回值为 false，且不为参数2赋值
				nh.getParam("nh_int",nh_int_value);
			
        getParamCached(键,存储结果的变量)--提高变量获取效率
            存在,返回 true,且将值赋值给参数2
            如果键不存在，那么返回值为 false，且不为参数2赋值
			    nh.getParamCached("nh_int",nh_int_value);

        getParamNames(std::vector<std::string>)
            获取所有的键,并存储在参数 vector 中 
                std::vector<std::string> param_names;
                nh.getParamNames(param_names);
                for (auto &&name : param_names)
                {
                    ROS_INFO("名称解析name = %s",name.c_str());   
                }
        
        hasParam(键)
        是否包含某个键，存在返回 true，否则返回 false
			ROS_INFO("存在 nh_int 吗? %d",nh.hasParam("nh_int"));

        searchParam(参数1，参数2)
            搜索键，参数1是被搜索的键，参数2存储搜索结果的变量
                nh.searchParam("nh_int",key);
                ROS_INFO("搜索键:%s",key.c_str());
                -> 搜索键:/nh_int
                
    ros::param ----- 与 NodeHandle 类似
*/
  • 参数服务器删除参数
1
2
3
4
5
6
7
8
9
/* 
    ros::NodeHandle
        deleteParam("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false

    ros::param
        del("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false
*/

python

  • 参数服务器新增(修改)参数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#! /usr/bin/env python

import rospy

if __name__ == "__main__":
    rospy.init_node("set_update_paramter_p")

    # 设置各种类型参数
    rospy.set_param("p_int",10)
    rospy.set_param("p_double",3.14)
    rospy.set_param("p_bool",True)
    rospy.set_param("p_string","hello python")
    rospy.set_param("p_list",["hello","haha","xixi"])
    rospy.set_param("p_dict",{"name":"hulu","age":8})

    # 修改
    rospy.set_param("p_int",100)
  • 参数服务器获取参数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
"""
	rospy.init_node("get_param_p")

    参数服务器操作之查询_Python实现:    
        get_param(键,默认值)
            当键存在时，返回对应的值，如果不存在返回默认值
            	int_value = rospy.get_param("p_int",10000)
            	
        get_param_cached(键,默认值)--提高变量获取效率
        		int_cached = rospy.get_param_cached("p_int")
        		
        get_param_names--获取所有的键
            names = rospy.get_param_names()
            for name in names:
                rospy.loginfo("name = %s",name)
                
        has_param
        	是否包含某个键，存在返回 true，否则返回 false
        		    flag = rospy.has_param("p_int")

        search_param--寻找某个键，并返回完整的路径
        	key = rospy.search_param("p_int")
    		rospy.loginfo("搜索的键 = %s",key)
"""
  • 参数服务器删除参数
1
2
3
4
5
6
7
8
9
10
 """
    rospy.delete_param("键")
    键存在时，可以删除成功，键不存在时，会抛出异常
    
    rospy.init_node("delete_param_p")
    try:
        rospy.delete_param("p_int")
    except Exception as e:
        rospy.loginfo("删除失败")
"""

常用命令

和之前介绍的文件系统操作命令比较，文件操作命令是静态的，操作的是磁盘上的文件，而上述命令是动态的，在ROS程序启动后，可以动态的获取运行中的节点或参数的相关信息

ROS/CommandLineTools - ROS Wiki

rosnode - ROS Wiki 用于获取节点信息的命令

  • rosnode list 列出所有的活动节点
  • rosnode ping [node_name] 测试到节点的连接状态
  • rosnode info [node_name] 打印节点信息
  • rosnode machine [machine_name] 列出指定设备上的节点
  • rosnode kill [node_name] 杀死某个节点
  • rosnode cleanup 清除无用节点，比如小乌龟示例

rostopic - ROS Wiki 用于显示有关ROS话题的调试信息，包括发布者，订阅者，发布频率和ROS消息

  • rostopic list 显示所有活动状态下的话题

  • rostpic echo [topic-name] 获取指定话题当前发布的消息

  • rostopic pub 直接调用命令向订阅者发布消息 [这类指令都需要先source 一下]

    1
    2
    rostopic pub -r [频率] /话题名称 消息类型 消息内容
    rostopic pub -r 10 chatter_person plumbing_pub_sub/Person [按两次Tab补齐]

  • rostopic info [topic-name] 获取当前话题的相关信息

  • rostopic type [topic-name] 打印话题类型

  • rostopic hz [topic-name] 显示话题的发布频率

rosmsg - ROS Wiki 用于显示有关 ROS消息类型的工具

  • rosmsg list 列出所有消息

  • rosmsg list | grep -i [消息种类] 显示消息的具体路径

  • rosmsg show [消息种类] 显示消息描述

  • rosmsg info [消息种类] 作用与 rosmsg show 一样

rosservice - ROS Wiki 用于列出和查询ROS Services的工具

  • rosservice list 列出所有活动的服务
  • rosservice call [service-name] 使用提供的参数调用服务
  • rosservice info [service-name] 打印有关服务的信息
  • rosservice type [service-name] 打印服务类型

rossrv是用于显示有关ROS服务类型的信息的命令行工具，与 rosmsg 使用语法高度一致

rosparam - ROS Wiki 用于使用YAML编码文件在参数服务器上获取和设置ROS参数

  • rosparam list 列出所有参数

  • rosparam set 设置参数 [参数名] [参数值]

  • rosparam get 获取参数

  • rosparam delete 删除参数

  • rosparam dump 将参数写出到外部文件

    1
    rosparam dump xxx.yaml

  • rosparam load [yaml文件] 从外部文件加载参数

    1
    rosparam load xxx.yaml

通信机制比较

三种通信机制中

参数服务器是一种数据共享机制，可以在不同的节点之间共享数据

话题通信与服务通信是在不同的节点之间传递数据的

话题通信：Publisher→Subscriber [Topic] [msg]

服务通信：Client→Server [Service] [srv]

二者的实现流程相似，都是两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输

Topic(话题) Service(服务)
通信模式 发布/订阅 请求/响应
同步性 异步(无序) 同步(有序)
底层协议 ROSTCP/ROSUDP ROSTCP/ROSUDP
缓冲区 有(queue_size 队列) 无
时时性 弱 强
节点关系 多对多 一对多(一个 Server)
通信数据 msg srv
使用场景 连续高频的数据发布与接收：雷达、里程计 偶尔调用或执行某一项特定功能：拍照、语音识别

ROS通信机制进阶

roscpp rospy

常用API

建议参考官方API文档或参考源码

在运行程序时rosrun 后接着写 _[参数名] = [参数值] 可以给节点

初始化函数

ros::init(argc, argv, “name”, [options])

  1. argc — 封装实参的个数(n+1)

  2. argv — 封装参数的数组

  3. name — 为节点命名(唯一性)

  4. options — 节点启动选项 ros::init_options::AnonymousName

    给前面的节点命名后增加一个随机数，使得同一个节点能够重复启动

    否则重名情况下，第二次启动时第一次会自动停止

返回值: void

rospy.init_node(name, argv=None, anonymous=False)

  1. name — 节点名称
  2. argv — 封装节点调用时传递的参数
  3. anonymous — 取值为 true 时，为节点名称后缀随机编号

话题与服务

在 roscpp 中，话题和服务的相关对象一般由 ros::NodeHandle nh 创建，python中不用专门创建NodeHandle

发布对象

ros::Publisher pub = nh.advertise< type >(“topic”, queue_size, [latch])

  1. type — 消息类型

  2. topic — 话题名称

  3. queue_size — 队列长度

  4. latch — 设置为ture时，该话题发布的最后一条消息将被保存，并且当有订阅者连接时会将该消息发送给订阅者

    以静态地图发送为例(短时间不变的数据)

    方案1：可以使用固定频率发送地图数据，但是效率低

    方案2：可以将地图发布对象的latch设置为true，并且发布方只发送一次数据，每当订阅者连接时，将地图数据发送给订阅者(只发送一次)，这样提高了数据的发送效率

pub = rospy.Publisher(“topic”, type, queue_size, [lathc])

订阅对象

ros::Subscriber sub = nh.subscribe(“topic”, queue_size, callback);

void callback(const std_msgs::String::ConstPtr &msg)

sub = rospy.Subscriber(“topic”, type, callback, queue_size)

def callback(msg):

服务对象

ros::ServiceServer server = nh.advertiseService(“service”, callback);

bool callback(srv type::Request &req, srv type::Response &resp)

server = rospy.Service(“service”, srv type, callback)

def callback(req):

return resp

客户端对象

ros::ServiceClient client = nh.serviceClient< srv type >(“service”);

等待服务函数1 ros::service::waitForService(“service”)

等待服务函数2 client.waitForExistence();

请求写入：srv type req; req.request.input1 = atoi(argv[1]); (类推写完输入)

client = rospy.ServiceProxy(“service”, srv type)

等待服务函数 client.wait_for_service()

请求写入：req = srv typeRequets() req.input1 = sys.argv[1] (类推写完输入)

回调函数

spinOnce() 处理一轮回调，一般用于循环体内

spin() 进入循环去处理回调

不同点：在ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行

python 只有 rospy.spin()

时间

时刻

获取时刻，或是设置指定时刻

ros::Time::now()

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
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_time");
    ros::NodeHandle nh;
    // 获取当前时刻：now 被调用执行的那一刻
    // 参考系：1970年01月01日 00:00:00
    ros::Time current_time = ros::Time::now();

    // 获取距离 1970年01月01日 00:00:00 的秒数
    ROS_INFO("当前时刻:%.2f",current_time.toSec()); // 浮点型
    ROS_INFO("当前时刻:%d",current_time.sec);  // 整型    

    // 转化为年月日时分秒格式
    std::time_t raw_time = current_time.toSec(); // 转换为秒
    std::tm* time_info = std::localtime(&raw_time); // 转换为本地时间
    // 如果需要UTC时间使用 std::gmtime
    char buffer[80];
    std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", time_info); // 格式化时间
    ROS_INFO("当前时间: %s",buffer);

    // 设置指定时刻
    ros::Time t1(100,100000000); // 参数1:秒数  参数2:纳秒
    ROS_INFO("时刻:%.2f",t1.toSec()); 
    ros::Time t2(100.1); // 传入浮点型
    ROS_INFO("时刻:%.2f",t2.toSec()); 

    return 0;
}

命名增加时间戳方法：

1
2
3
4
5
std::time_t raw_time = ros::Time::now().toSec(); // 转换为秒
std::tm* time_info = std::localtime(&raw_time); // 转换为本地时间
// 如果需要UTC时间使用 std::gmtime
char buffer[80];
std::strftime(buffer, sizeof(buffer), "%Y%m%d_%H%M%S", time_info); // 格式化时间

后续直接+buffer即可

rospy.Time.now()

1
2
3
4
5
6
7
8
9
10
11
current_time = rospy.Time.now()
rospy.loginfo("当前时刻:%.2f",right_now.to_sec()) # 浮点型
rospy.loginfo("当前时刻:%.2f",right_now.to_nsec()) # 整型

# 设置指定时刻
time1 = rospy.Time(100,100000000)  # 参数1:秒数  参数2:纳秒
time2 = rospy.Time(100.1)
time3 = rospy.Time.from_sec(100.1)
rospy.loginfo("设置时刻1:%.2f",time1.to_sec())
rospy.loginfo("设置时刻2:%.2f",time2.to_sec())
rospy.loginfo("设置时刻2:%.2f",time3.to_sec())

持续时间

设置一个时间区间(间隔)

ros::Duration du()

1
2
3
4
ROS_INFO("开始休眠:%.2f",ros::Time::now().toSec());
ros::Duration du(4.5);//持续4.5秒,double类型，以秒为单位
du.sleep();
ROS_INFO("结束休眠:%.2f",ros::Time::now().toSec());

du = rospy.Duration()

1
2
3
4
rospy.loginfo("持续时间测试开始.....")
du = rospy.Duration(3.3)
rospy.sleep(du) #休眠函数
rospy.loginfo("持续时间测试结束.....")

时刻运算

1
2
3
4
5
ros::Time begin = ros::Time::now();
ros::Duration du1(5);     // 模拟运行时间
ros::Time stop = begin + du1;
ROS_INFO("开始时刻:%.2f",begin.toSec());
ROS_INFO("结束时刻:%.2f",stop.toSec());

time 与 duration 可以+/-，duration 之间也可以+/-

但time 之间只可以 - ，不可以 + ，返回的是 ros::Duration 类型

python与c++相同

设置运行频率

1
2
3
4
5
ros::Rate rate(1);//指定频率
   while (true)
   {
       rate.sleep(); // 要放在循环里
   }
1
2
3
4
rate = rospy.Rate(1)
while not rospy.is_shutdown():
    rate.sleep() #休眠，要放在循环里
    rospy.loginfo("+++++++++++++++")

定时器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void cb(const ros::TimerEvent &event){
    ROS_INFO("----");
    ROS_INFO("调用时刻:%.2f",event.current_real.toSec());
}
/*
    Timer createTimer(ros::Duration(period), // 时间间隔
        const TimerCallback& callback,      // 回调函数
        bool oneshot = false,               // 是否一次性
        bool autostart = true ）            // 是否自动启动，设置为false时需要手动启动 time.start()
    定时器启动需要ros::spin() 因为有回调函数
*/

    ros::Timer timer = nh.createTimer(ros::Duration(1),cb);
    // ros::Timer timer = nh.createTimer(ros::Duration(1),cb,false,false);
    // timer.start(); // 手动启动
    ros::spin();
1
2
3
4
5
6
7
8
9
10
11
12
def cb(event):
    rospy.loginfo("+++++++++++")
    rospy.loginfo("当前时刻:%.2f",event.current_real.to_sec())
"""
	rospy.Timer(rospy.Duration(period), // 时间间隔
				callback,				// 回调函数
				oneshot=False,			// 是否一次性
                reset=False)			
"""
    rospy.Timer(rospy.Duration(1),cb)
	# rospy.Timer(rospy.Duration(1),cb,True) # 只执行一次
	rospy.spin()

其它函数

在发布实现时，一般会循环发布消息，循环的判断条件一般由节点状态来控制

C++中可以通过 ros::ok() 来判断节点状态是否正常，而 python 中则通过 rospy.is_shutdown() 来实现判断，导致节点退出的原因主要有如下几种：

  • 节点接收到了关闭信息，比如常用的 ctrl + c(z) 快捷键就是关闭节点的信号
  • 同名节点启动，导致现有节点退出
  • 程序中的其他部分调用了节点关闭相关的API(C++中是ros::shutdown()，python中是rospy.signal_shutdown())

日志函数：

1
2
3
4
5
ROS_DEBUG("hello,DEBUG"); //不会输出
ROS_INFO("hello,INFO"); //默认白色字体
ROS_WARN("Hello,WARN"); //默认黄色字体
ROS_ERROR("hello,ERROR");//默认红色字体
ROS_FATAL("hello,FATAL");//默认红色字体
1
2
3
4
5
rospy.logdebug("hello,debug")  #不会输出
rospy.loginfo("hello,info")  #默认白色字体
rospy.logwarn("hello,warn")  #默认黄色字体
rospy.logerr("hello,error")  #默认红色字体
rospy.logfatal("hello,fatal") #默认红色字体

头文件与源文件

核心内容在于CMakeLists.txt文件的配置

自定义头文件调用

流程:

  1. 编写头文件；
  2. 编写可执行文件(同时也是源文件)；
  3. 编辑配置文件并执行。

头文件

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#ifndef _HELLO_H
#define _HELLO_H
/* 
    声明namespace
        |-- class
            |- run
*/

namespace hello_ns{

class MyHello{

public:
    void run();
};
    
}

#endif

为了后续包含头文件时不抛出异常，配置 .vscode 下 _cpp_properties.json 的 includepath属性（和之前配置服务srv文件生成的头文件过程相似）

1
2
3
4
"includePath": [
		...
        "/home/ros/demo03_ws/src/plumbing_head/include/**"
      ],

源文件

src目录下新建 hello.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#include "ros/ros.h"
#include "plumbing_head/hello.h"

namespace hello_ns{

void MyHello::run(){
    ROS_INFO("run函数执行....");
}    

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_head");
    hello_ns::MyHello myhello;
    myhello.run();
    return 0;
}

配置文件

与之前配置多一步

1
2
3
4
5
6
7
8
9
10
11
12
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}
)

自定义源文件调用

流程:

  1. 编写头文件；
  2. 编写源文件；
  3. 编写可执行文件；
  4. 编辑配置文件并执行。

可执行文件

src目录下新建 use_head.cpp

1
2
3
4
5
6
7
8
9
10
#include "ros/ros.h"
#include "plumbing_head/hello.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_head_src");
    hello_ns::MyHello my;
    my.run();
    return 0;

配置文件

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
include_directories(
include // 这里解除注释
  ${catkin_INCLUDE_DIRS}
)

## 声明C++库
## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/plumbing_pub_sub.cpp
# )
add_library(head_src
  include/${PROJECT_NAME}/hello.h
  src/hello.cpp
)

add_dependencies(head_src ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(head_src
  ${catkin_LIBRARIES}
)

可执行文件配置:

1
2
3
4
5
6
7
8
9
add_executable(use_hello src/use_hello.cpp)

add_dependencies(use_hello ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

#此处需要添加之前设置的库
target_link_libraries(use_hello
  head_src // 库声明
  ${catkin_LIBRARIES}
)

Python模块导入

实现:

  1. 新建两个Python文件，使用 import 实现导入关系；
  2. 添加可执行权限、编辑配置文件并执行UseA

tool.py文件

1
2
#! /usr/bin/env python
num = 1000

use_tool.py文件

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 python

import os
import sys
import rospy

# 设置临时环境变量
# sys.path.insert(0,"/home/ros/demo03_ws/src/plumbing_pub_sub/scripts")
# 路径动态，保证移植可执行
path = os.path.abspath(".")
sys.path.insert(0,path + "/src/plumbing_head/scripts")

import tool

if __name__ == "__main__":
    rospy.init_node("tool")
    # 异常：ModuleNotFoundError:No module named'tools'
    """ 
        原因: rosrun 执行时，参考路径是工作空间的路径，在工作空间下无法查找依赖的模块 
        解决：可以声明python的环境变量,当依赖某个模块时,先去指定环境变量中查找依赖
    """
    rospy.loginfo("num=%d",tool.num)

ROS运行管理

元功能包

完成ROS中一个系统性的功能，可能涉及到多个功能包，逐一安装功能包的效率低下，在ROS中，提供了一种方式可以将不同的功能包打包成一个功能包，当安装某个功能模块时，直接调用打包后的功能包即可，该包又称之为元功能包

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

例如：sudo apt install ros-noetic-desktop-full 命令安装ros时就使用了元功能包

实现

新建一个功能包，不需要添加依赖，修改package.xml

1
2
3
4
5
6
7
8
9
<buildtool_depend>catkin</buildtool_depend>
<!-- 添加依赖的功能包名 -->
<exec_depend>plumbing_pub_sub</exec_depend>
<exec_depend>plumbing_server_client</exec_depend>
<exec_depend>plumbing_param_server</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
  <metapackage />
</export>

修改 CMakeLists.txt

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

不能出现换行以及注释

catkin/package.xml - ROS Wiki

launch文件

在功能包下添加 launch目录, 目录下新建 xxxx.launch 文件，编辑 launch 文件

1
2
3
4
<launch>
    <node pkg="turtlesim" type="turtlesim_node"     name="myTurtle" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />
</launch>

调用 launch 文件

1
roslaunch 包名 xxx.launch

roslaunch 命令执行launch文件时，首先会判断是否启动了 roscore，如果启动了，则不再启动，否则，会自动调用 roscore

launch标签

<launch>标签是所有 launch 文件的根标签，充当其他标签的容器

deprecated 告知用户当前 launch 文件已经弃用

1
<launch deprecated="此文件可能过时！">

node标签

<node>标签用于指定 ROS 节点，是最常见的标签，但 roslaunch 命令不能保证按照 node 的声明顺序来启动节点(节点的启动是多进程的)

  • pkg=”包名”

    节点所属的功能包

  • type=”nodeType”

    节点类型，即与之相同名称的可执行文件，python文件需要带上.py，cpp文件不需要，类似CMakeList配置

  • name=”nodeName”

    节点名称，在 ROS 网络拓扑中节点的名称

  • machine=”机器名”

    在指定机器上启动节点

  • respawn=”true | false” (可选)

    如果节点退出，是否自动重启

  • respawn_delay=” N” (可选)

    如果 respawn 为 true，那么延迟 N 秒后启动节点

  • required=”true | false” (可选)

    如果为 true，那么该节点退出，将杀死整个 roslaunch，不能和respawn一起用，会报错

  • ns=”xxx” (可选)

    在指定命名空间 xxx 中启动节点 ，主要用来避免重名

    node list -> /命名空间/name

  • output=”log | screen” (可选)

    默认log，但一般改成screen

1
<node pkg="turtlesim" type="turtlesim_node" name="myTurtle" output="screen"/>

include标签

用于将另一个 xml 格式的 launch 文件导入到当前文件

file=”$(find 包名)/xxx/xxx.launch”

1
2
3
4
5
<launch>
    <!-- 包含 -->
    <include file = "$(find launch01_basic)/launch/start_turtle.launch"/>
    <!-- 其他节点 -->
</launch>

用于复用

remap标签

用于话题重命名

1
2
3
4
5
<node pkg="turtlesim" type="turtlesim_node"  name="myTurtle" output="screen">
    <remap from="/turtle1/cmd_vel" to = "/cmd_vel" />
</node>
<!-- 在launch中的语法是每个标签结束后才标斜杠，没结束还有子标签的话就不标 -->

from = “原始话题名称” to = “目标话题名称”

此时原来的控制失效，因为turtle_teleop_key 是以/turtle1/cmd_vel 发布

1
<node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />

这里需要下载一个控制包

1
sudo apt-get install ros-noetic-teleop-twist-keyboard

param标签

<param>标签主要用于在参数服务器上设置参数

在<node>标签中时，相当于私有参数

在<node>标签外时，相当于全局参数

  • name=”[命名空间]/参数名”

    参数名称，可以包含命名空间

  • value=”xxx” (可选)

    定义参数值，如果此处省略，必须指定外部文件作为参数源

  • type=”str | int | double | bool | yaml” (可选)

    指定参数类型，如果未指定，roslaunch 会尝试确定参数类型，规则如下:

    • 如果包含 ‘.’ 的数字解析未浮点型，否则为整型
    • “true” 和 “false” 是 bool 值(不区分大小写)
    • 其他是字符串
1
2
3
4
5
<param name="param_A" type = "int" value ="100" />
<node pkg="turtlesim" type="turtlesim_node" name="myTurtle" output="screen">
    <remap from="/turtle1/cmd_vel" to = "/cmd_vel" />
    <param name="param_B" type = "double" value ="3.14" />
</node>

rosparam标签

<rosparam>标签可以从 YAML 文件导入参数，或将参数导出到 YAML 文件，也可以用来删除参数

  • command=”load | dump | delete” (可选，默认 load)
  • file=”$(find 功能包)/xxx/yyy….”
  • param=”参数名称”
  • ns=”命名空间” (可选)
1
2
<!-- 导入参数 -->
<rosparam command = "load" file = "$(find launch01_basic)/launch/param.yaml"/>

由于launch文件也不是按顺序进行，导出虽然写在launch文件较后的位置也可能无法正常导出，所以最好分两个launch文件，在完成后再进行导出

1
2
3
4
<launch>
    <!-- 导出参数 -->
    <rosparam command = "dump" file = "$(find launch01_basic)/launch/param_out.yaml"/>
</launch>

删除操作：

1
<rosparam command = "delete" param = "bg_B" />

group标签

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

  • clear_params=”true | false” (可选)

    启动前，是否删除组名称空间的所有参数(慎用….此功能危险)

这个时候即使在node中name属性相同也不会报错，因为group使得前面加上前缀，不会产生相同的节点名称

1
2
3
4
5
6
7
8
9
10
<launch >
    <group ns = "first">
        <node pkg="turtlesim" type="turtlesim_node" name="myTurtle" output="screen" />
        <node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />
    </group> 
    <group ns = "second">
        <node pkg="turtlesim" type="turtlesim_node" name="myTurtle" output="screen" />
        <node pkg="turtlesim" type="turtle_teleop_key"  name="myTurtleContro" output="screen" />
    </group> 
</launch>

arg标签

<arg>标签是用于动态传参，类似于函数的参数

  • name=”参数名称”

  • default=”默认值” (可选)

  • value=”数值” (可选)

    不可以与 default 并存

  • doc=”描述”

命令行调用传参

1
roslaunch hello.launch xxx:=值

工作空间覆盖

所谓工作空间覆盖，是指不同工作空间中，存在重名的功能包的情形

比如：自定义工作空间A存在功能包 turtlesim，自定义工作空间B也存在功能包 turtlesim，当然系统内置空间也存在turtlesim，如果调用turtlesim包，会调用哪个工作空间中的呢

  1. 新建工作空间A与工作空间B，两个工作空间中都创建功能包: turtlesim

    1
    2
    3
    4
    5
    6
    7
    8
    #include "ros/ros.h"

    int main(int argc, char *argv[])
    {
        ros::init(argc,argv,"hello_ws1");
        ROS_INFO("demo01_ws");
        return 0;
    }

  2. 为了在任何工作空间下调用demo01_ws和demo02_ws，需要在根目录下的.bashrc文件中进行修改

    1
    2
    3
    4
    source /opt/ros/noetic/setup.bash
    # 新增
    source /home/ros/demo01_ws/devel/setup.bash
    source /home/ros/demo02_ws/devel/setup.bash

  3. 刷新环境变量

    1
    source .bashrc

    运行

    1
    rosrun turtlesim [TAB]

    会发现直接跳出hello_ws2

    是因为在.bashrc中后刷新的会覆盖前面的

  4. 查看ROS环境环境变量

    1
    echo $ROS_PACKAGE_PATH

    ROS_PACKAGE_PATH 中的值，和 .bashrc 的配置顺序相反—>后配置的优先级更高

功能包重名时，会按照 ROS_PACKAGE_PATH 查找，配置在前的会优先执行

隐患

比如当前工作空间B优先级更高，意味着当程序调用 turtlesim 时，不会调用工作空间A也不会调用系统内置的 turtlesim，如果工作空间A在实现时有其他功能包依赖于自身的 turtlesim，而按照ROS工作空间覆盖的涉及原则，那么实际执行时将会调用工作空间B的turtlesim，从而导致执行异常，出现安全隐患

BUG 说明:

在 .bashrc 文件中 source 多个工作空间后，可能出现在 ROS PACKAGE PATH 中只包含两个工作空间，可以删除自定义工作空间的 build 与 devel 目录，重新 catkin_make，然后重新载入 .bashrc 文件，问题解决

目前没有特别的解决方案

节点名称重名

ros::init(argc,argv,””) rospy.init_node(“”) 定义节点名称

在ROS的网络拓扑中，是不可以出现重名的节点的，也不可以启动重名节点或者同一个节点多次

在ROS中给出的解决策略是使用命名空间或名称重映射

命名空间就是为名称添加前缀，名称重映射是为名称起别名

rosrun实现

rosrun设置命名空间

语法: rosrun 包名 节点名 __ns:=新名称

1
2
3
rosrun turtlesim turtlesim_node __ns:=t1
rosrun turtlesim turtlesim_node __ns:=t2
# 注意，这里=后面不可以有空格

rosnode list 

1
2
/t1/turtlesim
/t2/turtlesim

rosrun名称重映射

语法: rosrun 包名 节点名 __name:=新名称

1
2
rosrun turtlesim  turtlesim_node __name:=t1
rosrun turtlesim  turtlesim_node __name:=t2

设置命名空间同时名称重映射

1
rosrun turtlesim turtlesim_node __ns:=w1 __name:=t1

使用环境变量也可以设置命名空间，启动节点前在终端键入如下命令

1
2
export ROS_NAMESPACE=sw1
rosrun turtlesim turtlesim_node

launch实现

1
2
3
4
5
6
7
8
9
<launch >
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim" output="screen"/>
    <!-- 名称重映射 -->
    <node pkg="turtlesim" type="turtlesim_node" name="t1" output="screen"/>
    <!-- 命名空间 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim" output="screen" ns="p1" />
    <!-- 命名空间 + 称重映射 -->
    <node pkg="turtlesim" type="turtlesim_node" name="t2" output="screen" ns="p2" />
</launch>

在 node 标签中，name 属性是必须的，ns 可选

rosnode list查看节点信息

1
2
3
4
5
/p1/turtlesim
/p2/t2
/rosout
/t1
/turtlesim

编码实现

c++

重映射：(设置别名)

1
ros::init(argc,argv,"name",ros::init_options::AnonymousName)；

在名称后面添加时间戳

命名空间：

1
2
3
std::map<std::string, std::string> map;
map["__ns"] = "name";
ros::init(map,"");

python

重映射：(设置别名)

1
rospy.init_node("name",anonymous=True)

话题名称重名

rosrun实现

语法: rorun 包名 节点名 话题名:=新话题名称

实现teleop_twist_keyboard与乌龟显示节点通信方案

方案1：将 teleop_twist_keyboard 节点的话题设置为/turtle1/cmd_vel

1
rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/turtle1/cmd_vel

这个时候rosrun turtlesim turtle_teleop_key 也能正常控制，相当于两个发布

方案2：将乌龟显示节点的话题设置为 /cmd_vel

1
rosrun turtlesim turtlesim_node /turtle1/cmd_vel:=/cmd_vel

launch实现

在之前launch文件标签那里讲过，这里就给个代码

语法<remap from="原话题" to="新话题" />

1
2
3
4
5
6
<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="t1" />
    <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="key">
        <remap from="/cmd_vel" to="/turtle1/cmd_vel" />
    </node>
</launch>
1
2
3
4
5
6
<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="t1">
        <remap from="/turtle1/cmd_vel" to="/cmd_vel" />
    </node>
    <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="key" />
</launch>

编码实现

话题的名称与节点的命名空间、节点的名称是有一定关系的，话题名称大致可以分为三种类型:

  • 全局(话题参考ROS系统，与节点命名空间平级)
  • 相对(话题参考的是节点的命名空间，与节点名称平级)
  • 私有(话题参考节点名称，是节点名称的子级)
Snipaste_2025-05-30_21-27-59

c++：

全局名称：以/开头，和节点名称无关

1
ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter",1000);

相对名称：非/开头，参考命名空间(与节点名称平级)来确定话题名称

1
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);

__ns:=设置命名空间后，则话题位于 /name/chatter

私有名称：以~开头的名称

需要在节点handle创建时候做出些改变

1
2
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);

但如果话题以/开头则话题仍为全局(全局话题优先级更高)

1
2
ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter",1000);

python：

全局名称：以/开头，和节点名称无关

1
pub = rospy.Publisher("/chatter",String,queue_size=1000)

相对名称：非/开头，参考命名空间(与节点名称平级)来确定话题名称

1
pub = rospy.Publisher("chatter",String,queue_size=1000)

私有名称：以~开头的名称

1
pub = rospy.Publisher("~chatter",String,queue_size=1000)

参数名称重名

在ROS中节点名称话题名称可能出现重名的情况，同理参数名称也可能重名

当参数名称重名时，那么就会产生覆盖

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

rosrun实现

语法：rosrun 包名 节点名称 _参数名:=参数值

1
rosrun turtlesim turtlesim_node _A:=100

rosparam list 查看节点信息

1
/turtlesim/A

这种方法创建出来的参数为私有模式

launch实现

在launch文件那部分细讲过了，不再赘述

1
2
3
4
5
6
<launch>
    <param name="radius" value="0.2" />
    <node pkg="turtlesim" type="turtlesim_node" name="t1" ns="xxx">
        <param name="radius" value="0.08" />
    </node>
</launch>

编码实现

c++

使用 ros::param 或者 ros::NodeHandle 来设置参数

具体内容查看 - ROS通信机制 -> 参数服务器

ros::param::set，参数1传入参数名称，参数2传入参数值

1
2
3
ros::param::set("/set_A",100); //全局,和命名空间以及节点名称无关
ros::param::set("set_B",100); //相对,参考命名空间
ros::param::set("~set_C",100); //私有,参考命名空间与节点名称

ros::NodeHandle，首先需要创建 NodeHandle 对象，然后调用该对象的 setParam 函数，该函数参数1为参数名，参数2为要设置的参数值

1
2
3
4
5
6
7
ros::NodeHandle nh;
nh.setParam("/nh_A",100); //全局,和命名空间以及节点名称无关

nh.setParam("nh_B",100); //相对,参考命名空间

ros::NodeHandle nh_private("~");
nh_private.setParam("nh_C",100);//私有,参考命名空间与节点名称

python

1
2
3
rospy.set_param("/py_A",100)  #全局,和命名空间以及节点名称无关
rospy.set_param("py_B",100)  #相对,参考命名空间
rospy.set_param("~py_C",100)  #私有,参考命名空间与节点名称

分布式通信

ROS是一个分布式计算环境，一个运行中的ROS系统可以包含分布在多台计算机上多个节点

ROS对网络配置有某些要求：

  • 所有端口上的所有机器之间必须有完整的双向连接
  • 每台计算机必须通过所有其他计算机都可以解析的名称来公告自己

实现

  1. 准备

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

    Snipaste_2025-05-31_13-04-06

    修改有线连接的IPv4

    下载包 

    1
    sudo apt install net-tools
    1
    ifconfig

    查看网络信息

    1
    2
    3
    4
    5
    6
    # ros-VirtualBox (ROS1)
    inet 10.129.163.127 # ip地址
    netmask 255.255.248.0 # 子网掩码
    # ROS2
    inet 10.129.164.127 # ip地址
    netmask 255.255.248.0 # 子网掩码

    网关和地址一样，但是最后一组数只留1

  2. 配置文件修改

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

    1
    2
    cd /etc
    sudo gedit hosts

    可以通过hostname查看计算机名

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

    1
    2
    ping 10.129.164.127 #地址 
    ping ROS2 #计算机名

  3. 配置主机

    ~/.bashrc 追加

    1
    2
    3
    4
    5
    # export ROS_MASTER_URI=http://主机IP:11311
    # export ROS_HOSTNAME=主机IP

    export ROS_MASTER_URI=http://10.129.163.127:11311
    export ROS_HOSTNAME=10.129.163.127

  4. 配置从机

    从机可以有多台，每台都做设置，~/.bashrc 追加

    1
    2
    3
    4
    5
    # export ROS_MASTER_URI=http://主机IP:11311
    # export ROS_HOSTNAME=从机IP

    export ROS_MASTER_URI=http://10.129.163.127:11311
    export ROS_HOSTNAME=10.129.164.127

    source .bashrc 刷新环境变量

测试

  1. 主机启动 roscore(必须)

  2. 主机启动订阅节点，从机启动发布节点，测试通信是否正常

  3. 反向测试，主机启动发布节点，从机启动订阅节点，测试通信是否正常

ROS常用组件

章主要介绍ROS中内置的如下组件:

  • TF坐标变换，实现不同类型的坐标系之间的转换；
  • rosbag 用于录制ROS节点的执行过程并可以重放该过程；
  • rqt 工具箱，集成了多款图形化的调试工具

案例演示: 小乌龟跟随实现

1
2
3
roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch
roslaunch turtle_tf2 turtle_tf2_demo.launch 
# 如果报错 sudo ln -s /usr/bin/python3 /usr/bin/python

TF坐标变换

机器人系统上有多个传感器，如激光雷达、摄像头等，有的是可以感知机器人周边的物体方位(坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物

但不可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息

ROS 中直接封装了相关的模块: 坐标变换(TransForm Frame，TF)

tf2常用功能包有：

  • tf2_geometry_msgs：将ROS消息转换成tf2消息

  • tf2：封装了坐标变换的常用消息

    • 欧拉角转四元数：tf2/LinearMath/Quaternion.h (python：tf.transformations)

  • tf2_ros：为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API(python比较简洁，直接import tf2_ros就行)，一般创建类对象

    • 静态变换广播器：tf2_ros/static_transform_broadcaster.h
    • 转换关系订阅器：tf2_ros/transform_listener.h
    • 缓存数据：tf2_ros/buffer.h

可以借助于rviz显示坐标系关系

  • 新建窗口输入命令rviz
  • 在启动的 rviz 中设置Fixed Frame 为 base_link;
  • 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系

坐标msg消息

在坐标转换实现中常用的 msg：

geometry_msgs/TransformStamped 传输坐标系相关位置信息

geometry_msgs/PointStamped 传输坐标系内坐标点的信息

c++: 调用PointStamped需要调用tf2_geometry_msgs/tf2_geometry_msgs.h

python: 需要from tf2_geometry_msgs import PointStamped调用

命令行键入:rosmsg info geometry_msgs/TransformStamped

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w
# 四元数用于表示坐标的相对姿态

命令行键入:rosmsg info geometry_msgs/PointStamped

1
2
3
4
5
6
7
8
std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                 #|-- x y z 坐标
  float64 y
  float64 z

静态坐标变换

静态坐标变换，是指两个坐标系之间的相对位置是固定的

需求：

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

添加依赖

1
roscpp rospy std_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs

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
#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,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh; // 非必需
    // 创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;
    // 组织发布消息 
    geometry_msgs::TransformStamped tfs;
    // 设置头信息
    tfs.header.stamp = ros::Time::now(); // 时间戳
    tfs.header.frame_id = "base_link"; // 原点坐标系
    // 部件坐标系
    tfs.child_frame_id = "laser"; 
    // 部件相对原点的位移
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    // 设置四元数
    tf2::Quaternion qtn; 
    // 向该对象设置欧拉角,可以将欧拉角转化为四元数
    qtn.setRPY(0,0,0); // 翻滚roll,俯仰pitch,偏航yaw,值为弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();  
    // 发布数据
    pub.sendTransform(tfs);
    ros::spin();
    return 0;
}
欧拉角1

欧拉角：翻滚roll,俯仰pitch,偏航yaw，对应绕xyz轴旋转多少度

转化为四元数是为了避免出现万向节死锁，当俯仰角达到90°时（机头垂直向下），此时横滚（X轴）和偏航（Z轴）重合为同一旋转轴，无法区分左右倾斜和水平转向

四元数：由1个实部 + 3个虚部组成，这个转化过程计算太复杂了，会用就行

订阅方：

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
#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"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;
    // 创建TF订阅节点
    // 创建一个Buffer 缓存
    tf2_ros::Buffer buffer;
    // 创建监听对象(可以将订阅数据放入buffer)
    tf2_ros::TransformListener listener(buffer);
    // 正常雷达不断扫描坐标
    ros::Rate r(1);
    while (ros::ok())
    {
        // 组织一个部件坐标系
        geometry_msgs::PointStamped point_laser;
        // 部件坐标系名称 与发布方的child_frame_id 相同
        point_laser.header.frame_id = "laser"; 
        point_laser.header.stamp = ros::Time::now();
        // 检测物体信息(部件坐标系位置)
        point_laser.point.x = 2.0;
        point_laser.point.y = 3.0;
        point_laser.point.z = 5.0;
        // 添加休眠,避免开始的时候没有找到发布的坐标系相对关系,导致异常
        // ros::Duration(2); 但是用try的方法更好
        // 转换坐标点(相对于原点坐标系)
        // 新建一个坐标点，用于接收转换结果 
        // 使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败
        try
        {
            // 核心代码,将部件坐标系坐标点转化为原点坐标系坐标点
            geometry_msgs::PointStamped point_base;
            /*
                调用的是buffer的转换函数transform
                参数1是被转换的坐标点
                参数2是目标坐标系
                返回输出的坐标点

                PS:调用时需要加头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
             */
            point_base = buffer.transform(point_laser,"base_link"); 
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                point_base.point.x,
                point_base.point.y,
                point_base.point.z,
                point_base.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            ROS_INFO("程序异常.....");
        }
        r.sleep();
        ros::spinOnce();
    }
    return 0;
}

tos::NodeHandle nh 注册节点到ROS Master，提供访问ROS功能（如话题、服务、参数）的接口，如如果删除 ros::NodeHandle nh，TransformListener 的订阅失败，buffer 收不到TF数据

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
#! /usr/bin/env python

import rospy
import tf.transformations
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    rospy.init_node("static_pub_p")
    pub = tf2_ros.StaticTransformBroadcaster()
    # 组织发布的数据
    tfs = TransformStamped()
    # 设置头信息
    tfs.header.stamp = rospy.Time.now()  # 时间戳信息
    tfs.header.frame_id = "base_link"  # 原点坐标系id
    # 设置部件坐标系id
    tfs.child_frame_id = "laser"
    # 部件相对原点的位移
    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]
    # 广播发送
    pub.sendTransform(tfs)
    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
#! /usr/bin/env python

import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":
    rospy.init_node("static_sub_p")
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 组织一个部件坐标系 
        point_laser = PointStamped()
        # 部件坐标系名称
        point_laser.header.frame_id = "laser"
        # 坐标点数据
        point_laser.header.stamp = rospy.Time.now()
        point_laser.point.x = 2.0
        point_laser.point.y = 3.0
        point_laser.point.z = 5.0
        try:
            # 组织一个转换参考系
            point_base = PointStamped()
            # 坐标转换
            point_base = buffer.transform(point_laser,"base_link")
            rospy.loginfo("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                point_base.point.x,
                point_base.point.y,
                point_base.point.z,
                point_base.header.frame_id)
        except Exception as e:
            rospy.logerr("异常:%s",e)
        rate.sleep()

python 这里不写spin是因为python没有spinOnce，无法跳过循环

当坐标系之间的相对位置固定时，那么所需参数也是固定的，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下：

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

其实父系就是代码中的原点坐标系，子级就是代码中的部件坐标系

1
rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /base_link /laser

建议使用该种方式直接实现静态坐标系相对信息发布

动态坐标变换

需求：

启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布

依赖同静态

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
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h" 
#include "geometry_msgs/TransformStamped.h" 
#include "tf2/LinearMath/Quaternion.h" 
/* 
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准 备
        话题：/turtle1/pose
        消息：/turtlesim/Pose
*/

void doPose(const turtlesim::Pose::ConstPtr& pose){
    // 获取位姿信息，转换成坐标系相对关系(核心),并发布
    // 创建发布对象
    // tf2_ros::TransformBroadcaster pub; 如果这么写每次处理都要创建一个新的发布对象
    static tf2_ros::TransformBroadcaster pub; // 静态发布对象
    // 处理发布数据
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = "turtle1";
    // 从乌龟位姿信息获取坐标
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0;
    // 坐标系四元数
    // pose 信息中只有偏航yaw -> theta,二维没有翻滚roll,俯仰pitch
    // 欧拉角为 (0,0,theta)
    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();
    // 发布
    pub.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    // 创建订阅对象
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
    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
45
46
47
48
49
50
51
52
53
54
55
#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"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_sub");
    ros::NodeHandle nh;
    // 创建TF订阅节点 -> 订阅相对关系
    // 创建一个Buffer 缓存
    tf2_ros::Buffer buffer;
    // 创建监听对象(可以将订阅数据放入buffer)
    tf2_ros::TransformListener listener(buffer);
    ros::Rate r(1);
    // 组织一个部件坐标系
    geometry_msgs::PointStamped point_turtle;
    point_turtle.header.frame_id = "turtle1";
    while (ros::ok())
    {
        // 使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败
        point_turtle.header.stamp = ros::Time(0.0);
        point_turtle.point.x = 1;
        point_turtle.point.y = 1;
        point_turtle.point.z = 0;
        try
        {
            // 核心代码,将部件坐标系坐标点转化为原点坐标系坐标点
            geometry_msgs::PointStamped point_world;
            /*
                调用的是buffer的转换函数transform
                参数1是被转换的坐标点
                参数2是目标坐标系
                返回输出的坐标点

                PS:调用时需要加头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
             */
            point_world = buffer.transform(point_turtle,"world"); 
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                point_world.point.x,
                point_world.point.y,
                point_world.point.z,
                point_world.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {
            ROS_INFO("程序异常.....");
        }
        r.sleep();
        ros::spinOnce();
    }
    return 0;
}

注意这里ros::Time不可以用now 了，因为动态使得部件位置一直在发生变化，buffer接收有延迟，使得部件相对位置和检测物体位置的时间戳匹配不上，这个时候就会报错，设置ros::time(0.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
#! /usr/bin/env python

import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations
"""  
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准 备
        话题：/turtle1/pose
        消息：/turtlesim/Pose
"""
def doPose(pose):
	# 1. 创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    # 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

    # 四元数,从欧拉角转换
    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]

    # 3. 发布
    pub.sendTransform(tfs)
    pass

if __name__ == "__main__":
    rospy.init_node("dynamic_pub_p")
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
    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
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped


if __name__ == "__main__":
    rospy.init_node("dynamic_sub_p")
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rate = rospy.Rate(1)
    point_turtle = PointStamped()
    point_turtle.header.frame_id = "turtle1"
    while not rospy.is_shutdown():
        point_turtle.header.stamp = rospy.Time()
        point_turtle.point.x = 1
        point_turtle.point.y = 1 
        point_turtle.point.z = 1
        try:
            point_world = buffer.transform(point_turtle,"world")
            rospy.loginfo("转换后的坐标值:(%.2f,%.2f,%.2f)",
                point_world.point.x,
                point_world.point.y,
                point_world.point.z,)
        except Exception as e:
            rospy.loginfo("异常:%s",e)
        rate.sleep()

多坐标系变换

需求：

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

发布方：

1
2
3
4
<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 1 0 0 0 0 /world /son1" output="screen"/>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="-5 -1 0 0 0 0 /world /son2" output="screen"/>
</launch>

使用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
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#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"
#include "geometry_msgs/TransformStamped.h"
/* 
    订阅方实现：
        1. 计算son1与son2的相对关系
        2. 计算son1中的某个点在son2中的坐标值
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    ros::Rate rate(10);
    // 创建坐标点
    geometry_msgs::PointStamped psAtSon1;
    psAtSon1.header.frame_id = "son1";
    
    while (ros::ok()){

        psAtSon1.header.stamp = ros::Time::now();
        psAtSon1.point.x = 1.0;
        psAtSon1.point.y = 2.0;
        psAtSon1.point.z = 3.0;
        try
        {
            geometry_msgs::PointStamped psAtSon2;
            // 计算son1 和son2 的坐标关系
            // lookupTransform:参数1父级坐标系,参数2子坐标系,参数3ros::Time(0) 取间隔最小的两个时间戳进行计算
            geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1 相对 son2 的信息: 父级:%s,子级:%s, 偏移量:(%.2f,%.2f,%.2f)",
                    son1Toson2.header.frame_id.c_str(),
                    son1Toson2.child_frame_id.c_str(),
                    son1Toson2.transform.translation.x,
                    son1Toson2.transform.translation.y,
                    son1Toson2.transform.translation.z
                );

            // 计算son1中某个点在son2中的坐标值
            psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO("坐标点在Son2中的值:(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z);
            
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
    }

    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
45
46
47
48
49
50
51
#!/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()

坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱

安装指令

1
sudo apt install ros-noetic-tf2-tools

启动坐标系广播程序之后，运行如下命令:

1
rosrun tf2_tools view_frames.py

会产生类似于下面的日志信息:

1
2
[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

查看当前目录会生成一个 frames.pdf 文件

可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf

rosbag

机器人传感器获取到的信息，有时我们可能需要时时处理，有时可能只是采集数据，事后分析

机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式

  • 方式1：控制机器人运动，将机器人传感器感知到的数据时时处理，生成地图信息
  • 方式2：控制机器人运动，将机器人传感器感知到的数据留存，事后再重新读取数据，生成地图信息

两种方式比较，显然方式2使用上更为灵活方便

在ROS中关于数据的留存以及读取实现，提供了专门的工具: rosbag

rosbag是用于录制和回放 ROS 主题的一个工具集

本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息

通过命令行

创建目录保存录制的文件 比如根目录下创建bsgs文件夹

开始录制（-a指的是全部）

1
rosbag record -a -O [bags/turtle.bag]

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件

此时文件命名为turtle_[时间戳]

查看文件

1
rosbag info [bags/turtle TAB]

回放文件

1
rosbag play [bags/turtle TAB]
1
rosrun rosbag play [bags/turtle TAB]

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动

通过编码

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
#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"bag_write");
    ros::NodeHandle nh;
    // 创建rosbag对象
    rosbag::Bag bag;
    // 打开文件流
    bag.open("hello.bag",rosbag::BagMode::Write);
    // 写数据
    std_msgs::String msg;
    msg.data = "hello xxx";
    /* 
        参数1:话题
        参数2:时间戳
        参数3:消息
    */
    bag.write("/chatter",ros::Time::now(),msg);
    // 关闭文件流(养成习惯,打开写完就写关)
    bag.close();
    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
#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("hello.bag",rosbag::BagMode::Read);
    // 读数据
    // 可以先获取消息的集合，再迭代取出消息的字段
    // forrange 增强for循环
    for (auto &&m : rosbag::View(bag)) // rosbag::View 解析信息
    {
        // 解析, 对应的函数类型可以看函数返回值
        std::string topic =  m.getTopic();
        ros::Time time = m.getTime();
        std_msgs::StringPtr p = m.instantiate<std_msgs::String>();
        ROS_INFO("解析的数据为:话题:%s,时间戳:%.2f,消息:%s",
                topic.c_str(),
                time.toSec(),
                p->data.c_str());
    }
    // 关闭文件流
    bag.close();
    return 0;
}

python

写：

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("write_bag_p")
	# 创建bag对象并打开数据流
	bag = rosbag.Bag("hello_p.bag",'w') # 注意这里是'w'，不是双引号
	# 写数据
	msg = String()
	msg.data = "hello bag"
	bag.write("/chatter",msg)
    # 关闭数据流
	bag.close()

读：

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

if __name__ == "__main__":
	rospy.init_node("read_bag_p")
	# 创建bag对象并打开数据流
	bag = rosbag.Bag("hello_p.bag",'r')
	# 读数据
	msgs = bag.read_messages(topics="/chatter")
	for topic,msg,time in msgs: # 将返回的topic, message, timestamp赋到topic,msg,time
		rospy.loginfo("话题:%s,消息:%s,时间:%s",topic,msg,time)
	bag.close()

rqt工具箱

ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt

rqt 工具箱组成有三大部分

  • rqt——核心实现，开发人员无需关注
  • rqt_common_plugins——rqt 中常用的工具套件
  • rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)

基本使用

一般只要安装的是desktop-full版本就会自带工具箱

如果需要安装可以以如下方式安装

1
2
sudo apt-get install ros-noetic-rqt
sudo apt-get install ros-noetic-rqt-common-plugins

rqt的启动方式有两种：

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

启动 rqt 之后，可以通过 plugins 添加所需的插件

常用插件

  • rqt_graph：plugins/introspection/Node Graph or rqt_graph

圆框是节点，方框是话题，箭头是数据的订阅发布方向

  • rqt_console：plugins/Logging/Console or rqt_console

ROS 中用于显示和过滤日志的图形化插件

编写 Node 节点输出各个级别的日志信息

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"log_demo");
    ros::NodeHandle nh;

    ros::Rate r(0.3);
    while (ros::ok())
    {
        ROS_DEBUG("Debug message d");
        ROS_INFO("Info message oooo");
        ROS_WARN("Warn message wwww");
        ROS_ERROR("Erroe message EEEE");
        ROS_FATAL("Fatal message FFFF");
        r.sleep();
    }
    return 0;
}
01_rqt_console插件
  • rqt_plot：plugins/Visualization/plot or rqt_plot

图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据

  • rqt_bag：plugins/Logging/Bag or rqt_bag

录制和重放 bag 文件的图形化插件

14rqt_bag_录制 15rqt_bag_回放