ROS1入门

ROS

参考官方教程进行学习ROS/Tutorials，先从ROS1开始学习而不是ROS2，是因为很多项目还是基于ROS1的，而且ROS1/2很多地方并不兼容，例如ROS2就没有catkin编译了，而且很多包名称都不相同，因此还是从ROS1开始学习吧！

什么是ROS

从官网上的ROS/Introduction中可以看出，ROS是一个用于管理机器人控制的操作系统（既可以从底层控制每一个电机，也可以结合上层信息，通过雷达、深度相机进行决策），ROS运行时类似一个系统，可以开启多个不同的进程，他们称之为节点。

这个系统中的通讯包含不同类型：同步的services（服务），异步的topics（话题），以及用于数据存储的Parameter Server（参数服务器）

ROS所支持的语言有Python, C++, Lisp，下面开始用Docker安装ROS1吧。

安装

在ROS/Installation上可以看到当前ROS1最长维护的版本为ROS Noetic，推荐Ubuntu20.04（高版本装不上😭），但是我们不能为了装个ROS去装这个版本的系统，因此需要用到Docker，还可以方便的使用不同版本的ROS🤗。

Docker安装与常用命令可以参考我这篇博文，安装完成Docker后，可以直接从docker hub上pull我准备好的ROS1环境（下载大小为1.33 GB，支持Nvidia 11.8驱动，zsh, tmux, git等工具）

docker pull wtyyy/ros:ros1-noetic-cuda11.8.0-ubuntu20.04  # 记得开代理或镜像
xhost +local:root  # 用于可视化
CATKIN_WORKSPACE=/home/yy/Coding/learn/catkin  # 路径设置为本机的catkin代码保存路径

# 启动!
# 很多nvidia或gpu相关的指令都是启动nvidia渲染X11用的, 如果没有nvidia显卡则无需这些指令（加了也没坏处）
docker run -it \
	--name ${USER}_learn_ros \
	--gpus all \
	-e NVIDIA_DRIVER_CAPABILITIES=all \
	-e "__NV_PRIME_RENDER_OFFLOAD=1" \
	-e "__GLX_VENDOR_LIBRARY_NAME=nvidia" \
	--net=host \
	--privileged \
	-e "DISPLAY" -e "QT_X11_NO_MITSHM=1" \
	-v "/dev:/dev" \
	-v "/tmp/.X11-unix:/tmp/.X11-unix" \
	-v "$CATKIN_WORKSPACE:/catkin" \
	wtyyy/ros:ros1-noetic-cuda11.8.0-ubuntu20.04 zsh

（当然也可以自己跟着官方教程ROS/Installation Ubuntu自己动手安装）

1 初级教程

1.1 配置ROS环境

加载ROS环境参数，通过source /opt/ros/noetic/setup.sh启动ROS相关的环境变量，将ROS软件加入到路径中。

在Docker镜像中我已经将source /opt/ros/noetic/setup.zsh加入到了~/.zshrc中，即默认就会加载ROS配置

创建工作空间，ROS的工作环境如下所示，通过mkdir -p ~/catkin/src即可在用户目录下创建

在Docker镜像中使用，我将本地的$CATKIN_WORKSPACE路径挂在到了/catkin下，也就是创建/catkin/src文件夹即可

然后在/catkin文件夹在执行catkin_make（相当于cmake -B build && cd build && make），并会自动生成devel文件夹，在该文件夹下会有setup.sh文件，通过source该文件可以将当前工作空间设置在环境的最顶层。

通过查看环境变量ROS_PACKAGE_PATH以确定当前工作路径已经被包含：

echo $ROS_PACKAGE_PATH
> /catkin/src:/opt/ros/noetic/share

1.2 ROS文件系统

这节主要介绍ROS中的软件包如何安装以及查找软件包的相应位置等操作。

包路径查找指令

  1. rospack find <pkg_name>: 输出pkg_name的路径。例如rospack find roscpp
  2. roscd <pkg_name[/subdir]>: 类似cd命令，直接cd到pkg对应的文件夹下，还支持进入其自文件夹。例如roscd roscpp/cmake
  3. roscd log: 在运行过ROS程序后，可以通过该命令进入到日志文件夹下。
  4. rosls <pkg_name[/subdir]>: 类似ls命令，相当于执行ls $(rospack find pkg_name)[/subdir]。例如rosls roscpp/cmake

1.3 ROS包文件结构

一个caktin软件包包含至少两个文件

package/
  CMakeLists.txt  # CMake文件代码编译指令
  package.xml  # 所用到的相关包

多个软件包的文件格式如下

catkin/
  src/
    CMakeLists.txt  # 最上层的CMake文件（自动生成）
    package1/
      CMakeLists.txt  # package1的CMake文件
      package.xml  # package1的清单文件(manifest)
      srv/  # 存储定义的service数据格式 *.srv
      msg/  # 存储定义的message数据格式 *.msg
      scripts/  # C++/Python脚本
      launch/  # roslaunch并发执行配置文件 *.launch
    package2/
      CMakeLists.txt  # package2的CMake文件
      package.xml  # package2的清单文件(manifest)
    ...

创建空项目

可以通过catkin_create_pkg <pkg_name> [dep1] [dep2] ...指令创建一个新的空项目，例如

cd /catkin/src
catkin_create_pkg tutorials std_msgs rospy roscpp

这样就会创建一个软件包，包含上述的CMakeLists.txt和package.xml文件，以及src/和include/目录。

于是我们就可以对其进行编译，然后将软件包加入到工作空间中：

cd /catkin
catkin_make
source ./devel/setup.sh

查看包依赖关系

  1. rospack depends1 <pkg_name>: 查看包的第一级依赖，在包对应的package.xml文件中可以找到返回的依赖文件。例如rospack depends1 tutorials就可以看到std_msgs rospy roscpp三个包。
  2. rospack depends <pkg_name>: 递归地查找依赖包。

查看package.xml文件

参考官方的package.xml/Format 2文档，xml文件类似于网页文件，变量定义都称为tag，格式例如<name tag传入参数>tag定义</name>，当前所用的版本为Format2至少所需的tag内容如下:

  1. <name>: 软件包名称
  2. <version>: 软件版本号，必须是3个用.分隔的整数
  3. <description>: 对本项目的描述内容
  4. <maintainer>: 对项目维护者信息介绍
  5. <license>: 本项目使用的许可证

还有<url>, <author>可选信息可以加，参考用catkin_create_pkg创建的package.xml模板文件

声明完上述5个信息后，我们需要加入构建包所需的依赖包声明，声明函数都是类似*depend格式:

  • <depend>: 最方便的导入包方式，包含下面build, export, exec三个命令
  • <build_depend>: 找到包的路径，类似cmake中的find_package(...)
  • <build_export_depend>: 加入包的头文件, 类似cmake中的target_include_directories(...)
  • <exec_depend>: 加入包的动态链接库, 类似cmake中的target_link_libraries(...)
  • <test_depend>: 指定仅用于单元测试的包, 这些包不应该在上面export,exec中出现
  • <buildtool_depend>: 一般必须加的tag，制定编译所需的工具，这里一般就是catkin
  • <doc_depend>: 指定用于生成文档的包

上面创建tutorials/package.xml最终简化版本的文件如下

<?xml version="1.0"?>
<package format="2">
  <name>tutorials</name>
  <version>0.0.0</version>
  <description>The tutorials package</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <depend>roscpp</depend>
  <depend>rospy</depend>
  <depend>std_msgs</depend>
</package>

还可以在最后加<export>用于将多个包整合编译成一个元包（meta-package）

1.4 构建ROS软件包

这一节主要理解catkin_make对项目代码的编译原理，其实它就是对cmake指令的包装，以下两种命令等价:

# First
cd /catkin
catkin_make
# Second
cd /catkin/src
catkin_init_workspace  # 这会对src/下的所有项目创建一个父级的CMakeLists.txt
mkdir ../build && cd ../build
# 创建build配置, 安装文件位于../instsall下(如果安装), catkin工作空间配置文件保存在../devel下(运行既创建)
cmake ../src -DCMAKE_INSTALL_PREFIX=../install -DCATKIN_DEVEL_PREFIX=../devel
make  # 编译
# (可选) 等价于 catkin_make install
make install  # (可选)安装, 这样就会在/catkin/install路径下安装本软件包

当然此处也可以不安装在工作站位置../install，可以直接装到全局的ros包位置，也可以用catkin_make一步完成:

catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install

1.5 ROS Node

首先安装ros-tutorials软件包（package）apt install ros-noetic-ros-tutorials（如果安装的不是桌面完整版则需安装）

ROS正常可以被描述成一个图(Graph)， 包含如下这些概念：

  1. Nodes（节点）：一个可执行程序，该程序可以通过向Topic交互数据，从而与其他节点通信；
  2. Messages（消息）：ROS的数据格式，当node通过subscribing（订阅，接受消息）或者publishing（发布，发送消息）从Topic交互信息时使用的格式，同步；
  3. Topics（话题）：node之间可以通过subscribing从topic接收消息，publish向topic发送消息，异步；
  4. Master（主节点）：ROS的主服务，例如能帮助node能够互相找到；
  5. rosout：相当于ROS中的标准输出stdout/stderr
  6. roscore：包含master+rosout+parameter server（参数服务器，后文介绍）

ROS Graph

下面我们来测试下ROS工作流程：

  1. 终端执行roscore，这是执行所有ROS程序前需要的命令（最好在tmux的一个新建window中运行，后台挂起，在其他window中运行node）
  2. 新建一个终端，可以尝试rosnode命令来查看各种node相关的信息，例如rosnode list可以列出当前所有节点（只有/rosout在运行哦）
  3. 开启一个新的node，通过rosrun <pkg_name> <node_name>来启动一个node（ROS程序），例如rosrun turtlesim turtlesim_node（启动小乌龟渲染节点）
  4. 如果想重复开同一个程序，直接运行会因为重名而把之前的node冲掉，因此我们要再设置一个新名字，在最后加上重定义参数__name:=[新名字]，例如rosrun turtlesim turtlesim_node __name=my_turtle，就可以开两个乌龟窗口了🐢🐢
  5. 测试node连接性是否正常，通过rosnode ping <node_name>来和node ping下是否联通

1.6 ROS Topic

我们继续保持上面的turtlesim_node开启，再开启一个rosrun turtlesim turtle_teleop_key，这样就可以用方向键上下左右控制小乌龟运动了。

rqt可视化节点关系

通过安装rqt可以查看节点之间的关联性：

apt-get install ros-noetic-rqt
apt-get install ros-noetic-rqt-common-plugins

rosrun rqt_graph rqt_graph  # 可视化节点关系图
rqt_graph  # 或者直接运行也可以启动


每个圆圈就是一个节点，中间连线表示消息的message传输方向，连线上的名称为topic，在这里就只有一个topic: /turtle1/cmd_vel，/teleop_turtle向其publish，/yy_turtle从其subscrib

rostopic指令

通过rostopic相关函数可以获取topic的信息：

  1. rostopic list：显示节点信息，可选-v显示详细信息
  2. rostopic echo </topic_name>：获取topic中的消息
  3. rostopic type </topic_name>：获取topic中信息的类型（由publisher决定），publisher和subscriber需要支持该类型消息处理
  4. rostopic pub [args] </topic_name> <data_type> -- <data>：向topic发送格式为data_type的消息data，可以通过args设置发送频率（默认只发送一次消息，就卡着了）
  5. rostopic hz </topic_name>：获取topic的信息接受频率

我们可以通过rostopic echo /turtle1/cmd_vel，获取消息，再回到控制小乌龟的终端，移动小乌龟，就可以看到发送的消息是什么了，rostopic type /turtle1/cmd_vel来看看消息是什么类型的：geometry_msgs/Twist

通过rosmsg show geometry_msgs/Twist可以看到这类消息的详细格式要求，或者一行搞定rostopic type /turtle1/cmd_vel | rosmsg show，返回的数据格式如下：

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

看到消息要求后，我们就可以通过rostopic pub向小乌龟发送消息了：

rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 0.8]'
  • -r 1 表示以1hz频率向topic发送消息
  • topic名字为/turtle1/cmd_vel, 消息type为geometry_msgs/Twist
  • -- 表示对前面指令和后面消息的分隔符（如果消息里面都是用''或者""包裹其实没影响，不包裹且有负数出现才必须要这个）
  • '[2.0, 0.0, 0.0]' '[0.0, 0.0, 0.8]' 对发送数据的描述，命令行版本的YAML，参考

我们分别开两个终端发送这两个数据：

rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist '[2, 0, 0]' '[0, 0, 2]'
rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist '[3, 0, 0]' '[0, 0, -2]'

可以画出下图的效果了

通过rostopic hz /turtle1/color_sensor来确定你的节点以多少hz发送画面渲染消息（我是125hz）

通过rostopic echo /turtle1/pose可以查看这个topice下的数据有哪些，看到有如下这些信息

x: 3.218510150909424
y: 7.931597709655762
theta: 2.8436872959136963
linear_velocity: 2.0
angular_velocity: 0.800000011920929

于是可以通过rosrun rqt_plot rqt_plot实时绘制这些topic中相应数值的曲线图，打开界面后在左上角分别输入以下三个，用右侧加号加入图表

/turtle1/pose/x
/turtle1/pose/y
/turtle1/pose/theta

如果发现绘制速度过快，是因为x轴范围太小导致，可以通过上方倒数第二个按钮，修改X-Axis, Left, Right的差值更大（修改其中一个即可，自动更新时会保持差值一致的），绘制效果如下图所示

配置X轴范围 绘制曲线效果
1 2

1.7 ROS Service

ROS中service（服务）是节点中的另一种通讯方式，service是同步的通讯机制（RPC模式，发送request请求立马获得一个response响应），而topic是异步的通讯机制（一个发送数据，另一个可以选择性接受数据）

rosservice包含以下这些操作：

rosservice list  # 显示当前的service, 可选-n选项, 显示是由哪个node创建的service
rosservice info </srv_name>   # 显示当前srv的具体信息, 包含type, args, uri(链接), node
rosservice call </srv_name> -- <msg>  # 向srv发送message, message格式需要和rosservice args </srv_name>
rosservice find </srv_msg> | rossrv show  # 根据service message查找对应的node

这里有两个message:

  • topic发送的：rosmsg show <topic_msg>获取参数数据，直接查询topic并获取args：rostopic type </topic_name> | rosmsg show
  • service发送的：rossrv show <srv_msg>获取参数数据，直接查询service并获取args：rosservice type </srv_name> | rossrv show

测试效果

rosservice list可以直接看到当前turtlesim相关的服务，例如：

/clear  # 清除轨迹
/kill  # 杀死乌龟
/reset  # 重置乌龟
/spawn  # 下蛋, 初始化一个新的乌龟
...

例如我们想创建一个新乌龟：首先确定新建乌龟需要什么参数？rosservice info /spawn可以看到

Node: /turtlesim  # 所属节点
URI: rosrpc://yy-ASUS-TUF-Gaming-A15-FA507XV:41699  # 通讯的uri地址
Type: turtlesim/Spawn  # 通讯message类型
Args: x y theta name  # 通讯数据格式: 初始乌龟位置, 角度, 乌龟名字

新加一个乌龟: rosservice call /spawn 5 5 3 "turtle2"，查看当前node有哪些：

rostopic list | grep turtle
> /turtle1/cmd_vel
> /turtle1/color_sensor
> /turtle1/pose
> /turtle2/cmd_vel
> /turtle2/color_sensor
> /turtle2/pose

这样就可以同时控制两只龟龟了

rostopic pub -r 1 /turtle1/cmd_vel geometry_msgs/Twist '[3, 0, 0]' '[0, 0, 2]'
rostopic pub -r 1 /turtle2/cmd_vel geometry_msgs/Twist '[3, 0, 0]' '[0, 0, -2]'

rosparam（参数服务器）

参考官方介绍，这个可以看作一个全局变量存储器，可以用yaml格式存储：整型（integer）、浮点（float）、布尔（boolean）、字典（dictionaries）和列表（list）等数据类型（咋感觉就是Python的数据类型😂）

常用的命令如下：

  • rosparam set </param_name> -- <data>：设置参数，向param_name赋予新的yaml类型的data
  • rosparam get </param_name>：获取param_name参数
  • rosparam load <file_name.yaml> [namespace]：从文件file_name.yaml中加载参数到namespace关键字下
  • rosparam dump <file_name.yaml> [namespace]：向文件file_name.yaml中存储namespace关键字下的参数
  • rosparam delete </param_name>：删除参数
  • rosparam list：列出参数名

例如：

  • 我们可以设置新的参数rosparam set /hi -- "[1,2,{'a':3, '3': 0.14},1.2]"，真是类似python的定义，字典的关键字必须是字符串
  • rosparam list可以查看当期已有的参数
  • rosparam get /hi获取参数中的信息（以yaml格式输出出来）
  • rosparam dump test.yaml /turtlesim保存当前的/turtlesim相关参数到test.yaml中
  • rosparam load test.json /turtlesim读取当前test.yaml中参数到/turtlesim
  • rosparam set /turtlesim/background_r 150修改当前乌龟的背景色中的红色设成150
  • rosservice call /reset重置下小乌龟环境，看到小乌龟背景板变色了！

1.8 日志DEBUG和roslaunch

日志DEBUG

安装rqt相关依赖包:

apt install ros-noetic-rqt ros-noetic-rqt-common-plugins

先启动日志记录器rosrun rqt_console rqt_console，日志筛选器rosrun rqt_logger_level rqt_logger_level，这样就可以实时截取日志消息了。

我们启动一个小乌龟node：rosrun turtlesim turtlesim_node，向其中添加一个小乌龟rosservice call /spawn 1 5 0 ""，在rqt_console上就可以看到显示的Info消息了。

我们再让小乌龟去撞墙：rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 "[1,0,0]" "[0,0,0]"，等到小乌龟撞到墙时候，就可以从rqt_console中看到很多Warn消息了。

我们再看到刚才打开的rqt_logger_level，这个可以对node message按照日志等级进行筛选，如果我们将Nodes选为/turtlesim，Loggers选为ros.turtlesim，Levels选为Debug，我们就可以在rqt_console里面开到实时的乌龟位置了，日志的优先级从高到低分别为：

Fatal （致命）
Error （错误）
Warn  （警告）
Info  （信息）
Debug （调试）

当将level设置为某一个优先级时，高于其优先级的logger就会被输出出来。

roslaunch启动两个同步小乌龟

通过写*.launch文件我们可以对相同程序启动多个的node（通过不同namespace区分它们），还是回到上次我们创建的tutorials项目中去roscd tutorials，如果把他删了，或者忘记了source那么重新创建一下吧，参考 - 创建空项目。

roscd tutorials
mkdir launch && cd launch
vim turtlemimic.launch  # 或者用vscode打开

把下面这段代码贴进去，分别是通过不同namespace启动相同程序rosrun turtlesim turtlesim_node两次（所有的param, topic, node名称前面，都会先加上turtlesim1或turtlesim2的命名）

而下面的rosrun turtlesim mimic就是将turtlesim1收到的消息转发给turtlesim2

<!-- launch tag开始 -->
<launch>

  <!-- 创建第一个小乌龟窗口, 通过对所有变量前加上命名空间"turtlesim1"
       和后面一个小乌龟窗口进行区分 -->
  <group ns="turtlesim1">
    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
  </group>

  <group ns="turtlesim2">
    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
  </group>

  <!-- 从turtlesim软件包中启动其二个名为mimic的程序,
       通过这个程序转发turtlesim1的消息到turtlesim2中去 -->
  <node pkg="turtlesim" name="mimic" type="mimic">
    <remap from="input" to="turtlesim1/turtle1"/>
    <remap from="output" to="turtlesim2/turtle1"/>
  </node>

</launch>

保存文件，执行roslaunch tutorials turtlemimic.launch就可以看到启动的两个乌龟窗口了，再对turtlesim1发送指令就可以同时控制两个乌龟了rostopic pub /turtlesim1/turtle1/cmd_vel geometry_msgs/Twist -r 1 '[2,0,0]' '[0,0,4]'

一个问题就是为什么这里再对turtlesim2发送消息每一步走的距离就很短？

终端输入rqt直接打开窗口，在上面选择Plugins > Introspection > Node Graph就可以打开一个节点图（当然直接输入rqt_graph也可以开），选择Nodes/Topics (active)就可以看到下图的效果：

1.9 msg和srv介绍

  • msg（就是发送到topic的通讯文件）：文本文件，用多个变量组成的数据格式来描述一个消息
  • src（包含service通讯信息的文件）：描述一个service传输的数据，由request和response两个部分组成，分别为接受与发送的数据格式

一般的项目中，我们将msg文件放在msg/文件夹下，srv文件放在srv/文件夹下。

msg

就是简单的文本文件，每行由类型 名称组成，类型包含：

  • int8, int16, int32, int64, uint[8|16|32|64]
  • float32, float64
  • string
  • time, duration
  • 其他的msg文件（可嵌套）
  • 变长数组, 固定长度数组

还有一个特殊的类型Header，通常我们会在msg定义的第一行写上，他会被自动解析为std_msgs/msg/Header.msg中的内容：

uint32 seq
time stamp
string frame_id

例子，我们编辑之前tutorials的项目，创建/catkin/src/tutorials/msg/test.msg如下：

Header header
string s
float32[2] abc
int32 i

source /catkin/devel/setup.sh执行rosmsg show tutorials/test就可以看到我们写的msg格式如下：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string s
float32[2] abc
int32 i

想要在代码中使用到这个test.msg数据格式，需要在编译时支持转化，修改如下文件：

  • 修改package.xml：解开以下两行的注释（分别用于生成消息和运行时接收消息） 
    <build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
  • 修改CMakeLists.xml（src/tutorials下的）：
    1. find_package(...)中加入message_generation
    2. catkini_package(...)中找到CATKIN_DEPENDS后加入message_runtime（这个是包依赖关系，如果这个包被其他包调用了，那么会自动导入message_runtime包）
    3. 找到add_message_files(...)，将其改为
    add_message_files(
  FILES
  test.msg  # 你的msg文件名
)
    1. 找到generate_messages(...)解开注释，如下
    generate_messages(
  DEPENDENCIES
  std_msgs
)

OK，让我们重新编译一下cd /catkin && catkin_make，编译完成后就可以找到msg转码文件了：

  • C++：/catkin/devel/include/tutorials/test.h
  • Python：/catkin/devel/lib/python3/dist-packages/tutorials/msg/_test.py
    这样我们的后续项目代码就可以解包和发包了

srv

我们创建文件夹roscd tutorials && mkdir srv，直接从另一个包里面复制现有的srv：

roscd tutorials/src/
roscp rospy_tutorials AddTwoInts.srv test_srv.srv

cat test_srv.srv
> int64 a  # 发送的数据格式
> int64 b
> ---
> int64 sum  # 接受的数据格式

现在可以用rossrv show tutorials/test_srv.srv来看看是否识别到了我们的service文件，可以看到输出和test_srv.srv文件内容一致。

下面类似msg的流程，让代码支持test_srv.srv：

  • 修改package.xml：解开以下两行的注释（和msg相同） 
    <build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
  • 修改CMakeLists.xml（src/tutorials下的）：
    1. find_package(...)中加入message_generation（和msg相同）
    2. 找到add_service_files(...)，将其改为
    add_service_files(
  FILES
  test_srv.srv  # 你的srv文件名，注意不要和*.msg重名!!!
)
    1. 找到generate_messages(...)解开注释，如下
    generate_messages(
  DEPENDENCIES
  std_msgs
)

OK，类似地让我们重新编译一下cd /catkin && catkin_make，编译完成后就可以找到srv转码文件了：

  • C++：/catkin/devel/include/tutorials/[test_srv.h, test_srvRequest.h, test_srvResponse.h]
  • Python：/catkin/devel/lib/python3/dist-packages/tutorials/srv/_test_srv.py
    这样我们的后续项目代码就可以使用srv接受和发送消息了

1.10 ROS Python脚本

运行Python script方法

在/catkin/src/tutorials/scripts/下面创建我们的代码tmp.py，导入import rospy来和ros进行交互

import rospy
# 这两个没有自定义就可以删掉
from tutorials.msg import test  # 这个是上文中自定义的message
from tutorials.srv import test_srv  # 这个是上文中自定义的serve

def play():
  topics = rospy.get_published_topics()  # 显示当前可用的topics
  print(topics)

if __name__ == '__main__':
  play()

如果有自定义的/srv或者/msg下定义的数据格式，就需要按照上文中msg和srv介绍中介绍的编译方法，修改package.xml, CMakeLists.txt文件，并再修改CMakeLists.txt中的

# 加这个就是可以编译后让rosrun找到python脚本
# 如果想要直接测试代码也可以不加，完成项目时候还是全加上吧
catkin_install_python(PROGRAMS
  scripts/easy_play_turtle.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

最后用catkin_make编译即可。

  1. 直接运行代码：
    1. 如果没有自定义的依赖包，直接在终端运行就行了
    2. 如果要用到当前包定义的数据类型，先source /catkin/devel/setup.sh一下，添加路径，就可以直接运行了
  2. 使用rosrun运行，例如上面的代码叫easy_play_turtle.py，直接运行rosrun tutorials easy_play_turtle.py即可。

测试msg和srv

我们需要在/catkin/src/tutorials/scripts/中创建如下三个代码：

使用方法：先启动talker.py，再启动topic_listener.py接受消息，或者启动add_two_ints_client.py 3 5后面两个数字为要进行加和的数据。

他们作用分别为：

  1. talker.py：向一个topic发送消息，并且一个用于做加法的service，这两个函数Thread同时运行
  2. topic_listener.py：从topic中接受消息，并打印出来
  3. add_two_ints_client.py：可以通过命令行输入的方式，向做加法的service中发送加法请求，并接收消息

使用到rospy.log*()的代码都加上了，这一行，不然他默认的time时钟就是一个时间戳，没有任何可读性😵‍💫

os.environ['ROSCONSOLE_FORMAT'] = '[${severity}] [${time:%Y-%m-%d %H:%M:%S}]: ${message}'

下面分别分析上述三个代码块：

talker.py

node初始化
  1. rospy.init_node('node_talker', anonymous=True)：如果当前Python进程想加入ROS中就要先创建一个属于自己的node，这里节点名字叫node_talker（anonymous会在你的节点后面加上时间戳，节点最好就别重名，否则之前重名的节点就被kill了）
topic publish
  1. pub = rospy.Publisher('my_topic', test, queue_size=10)：向topic publish消息
    • 'my_topic'：我们向这个名字的topic发送消息
    • test：定义发送的消息格式（我们在msg/test.msg中定义的），当my_topictopic还没有创建时，它会被设置为test类型，否则，就会检查当前的类型是否和my_topic已有的类型相同，否则报错
    • queue_size=10：设置topic处理的消息最大缓存长度，注意，这个处理是将数据从网络中读取到内存中所用的速度，通常不会成为瓶颈（也就是发送频率不会高于内存写入频率），因此这个值写成100,1000都可以，不写可能不是很安全
  2. rate = rospy.Rate(10)：和rate.sleep()结合使用，表示以10hz的频率进行休息，保证消息发送的频率
  3. pub.publish：假设pub对应当前topic的message类型为test，其包含两个变量int32 a和int32 b，那么我们可以从from *.msg import test将这个数据类型读入进来，这里有三种不同的publish写法：
    • pub.publish(test(a=10,b=20))：直接实例化消息
    • pub.publish(10, 20)：传入序列解包（不包含message的嵌套，不能递归解包），等价于pub.publish(*args) = pub.publish(test(*args))
    • pub.publish(a=10, b=20)：传入字典解包，等价于pub.publish(**kwargs) = pub.publish(test(**kwargs))

    本质上，都是先实例化后再发送

日志处理
  1. rospy.loginfo(...)：会将日志信息通过/rosouttopic输出(参考)，还有rospy.logwarn(...), logerror(...), ...

    代码通过/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/rosout.py中的_rosout函数实现

service初始化 (response)
  1. rospy.Service('my_service', test_srv, add)：创建一个名为my_service的service，使用的数据格式为test_srv，add是对receive的数据进行处理的函数（得到response返回给request）

topic_listener.py

topic subscribe
  1. rospy.Subscriber('my_topic', test, callback)：和rospy.Service类似
    • 'my_topic'：接收topic的名称
    • test：topic的数据类型
    • callback：处理接收到消息的函数
  2. rospy.spin()：类似cv2.wait()会一直进行等待，不过这个是等到强制关闭这个进程

add_two_ints_client.py

service request
  1. rospy.wait_for_service('my_service')：等待名为my_service的service被创建

    类似地，等待topic的函数为rospy.wait_for_message(topic_name)

  2. req = rospy.ServiceProxy('my_service', AddTwoInts)：创建request请求函数
    • service名称为'my_service'
    • srv数据类型为AddTwoInts
    • 返回的结果就是一个可直接调用函数req，使用方法就是类似pub.publish的方法，将参数直接实例化或者将实例化的参数以序列或者字典形式输入进去，例如req(x, y) <=> req(AddTwoIntsRequest(x, y))，调用返回的数据类型为AddTwoIntsResponse，也就是srv类型后面加了个Response

PID控制小乌龟绘制图形

接下来，这里我们直接开始写Python代码来用PID控制小乌龟的线速度linear.x和角速度angular.z，首先启动我们的小乌龟节点：rosrun turtlesim turtlesim_node，然后创建文件.../tutorials/scripts/play_turtle.py

catkin_make编译完成后分别执行

rosrun tutorials play_turtle.py --fig-id 0 --reset --name turtle1  # 终端1
rosrun tutorials play_turtle.py --fig-id 1 --no-reset --name turtle2  # 终端2

或者我们可以在launch/文件夹下写一个draw_double_love.launch启动文件，然后一键启动roslaunch tutorials draw_double_love.launch：

代码中需要注意的地方：

  1. PID系数调整，可以尝试下不同的PID系数组合，可能会崩溃哦
  2. 角误差的计算，通过做差得到ang_error后需要用(δ+π)%(2π)−π(\delta+\pi)\%(2\pi) - \pi这个变换来将超过[−π,π][-\pi,\pi]的角度等价变换到该范围内（举例：当αtarget=0.8π,αnow=−0.8π\alpha_{target}=0.8\pi,\alpha_{now}=-0.8\pi，则δ=αtarget−αnow=1.6π\delta=\alpha_{target}-\alpha_{now}=1.6\pi，但是这两个角差距很小，只需要转−0.4π-0.4\pi即可，这就是这个变化的作用，如果转1.6π1.6\pi可能导致PID计算崩溃哦）
  3. 可以自己尝试下不同的控制频率--hz 10，默认是10，更低的hz可能导致pid控制的出错哦（抖动非常厉害），而更高的hz就看不出来什么区别了
  4. 执行*.launch文件时，会将ROS所需的CLA(Command-line argument)，例如__name:=和__log:=传给Python，因此就需要忽略这些参数，对于tyro可以在解析时候加入return_unknown_args=True来忽略，使用argparse时候可以通过parser.parse_known_args()忽略多余参数
  5. *.launch中执行的node输出的info日志不会显示出来，需要加上output="screen"才会显示
绘制过程 结果
double_love result

1.11 ROS Bag录制topic

录制Bag

mkdir ~/bagfiles && cd ~/bagfiles，使用rosbag record -a录制开启到关闭这段时间内的所有topic中message，例如，启动如下两个node:

rosrun turtlesim turtlesim_node
rosbag record -a  # 开始录制
rosrun tutorials play_turtle.py  # 启动绘制程序
# 等待绘制完毕后，ctrl+c关闭录制

录制完毕后可以看到当前文件夹下创建了一个*.bag文件，rosbag info *.bag可以查看包的信息，例如总共录制时长、每个topic中的消息数目，重放包信息如下：

rosbag play *.bag  # 重放包中每个message
rosbag play -r 2 *.bag  # 以两倍速重放

在rosservice call /reset后，执行2次1倍速重放+1次2倍速重放，可以看到如下图的效果：

如果我们只想录制部分topic，可以如下指定:

# -O 制定输出的文件名为draw_love
# 最后的变量均为录制的topic对象
rosbag record -O draw_love /turtle1/cmd_vel

执行rosbag play draw_love.bag应该和上面录制全部message的回放效果相同。

保存为yaml

bag中还保存了消息发送的频率，时间等信息，如果我们只想看yaml数据信息，可以直接通过rostopic echo <topic_name> | tee <filename>.yaml保存到文件中，例如

rostopic echo /turtle1/cmd_vel | tee cmd_vel.yaml  # 终端1
rosbag play 2024-12-16-14-11-27.bag -i  # 终端2, -i表示immediate, 立刻将所有msg全部输出出来

查看cmd_vel.yaml文件就可以看到每个msg的具体信息了。

Robotics > ROS
ROS1入门
https://wty-yy.github.io/posts/19333/
作者
wty
发布于
2024年12月9日
许可协议