2.1 ROS概述
2.1.1 ROS
基础概念
节点(node
)间的通信是ROS
的核心。节点是一种使用ROS
的中间件进行通信的ROS
程序。一个节点可以独立于其他节点启动,多个节点也可以以任何顺序启动。多个节点可以运行在同一台计算机,或者分布于一个计算机网络中。一个节点只有当其在与其他节点通信并最终与传感器和执行器连接时,它才是有用的。
发布器节点通过通知roscore
一个话题名来初始化该话题(类ros::Publisher
)。由于所有节点都需要与roscore
通信,所以roscore
必须在ROS
所有节点启动前就已经开始运行。
订阅器节点也可以与roscore
通信(类ros::Subscriber
)。
一个节点既可以是订阅器,也可以是发布器。
2.1.2 编写ROS
节点
(1)创建ROS
程序包
在终端运行以下命令:
roscd # 进入默认工作区~/ros_ws
cd src # 进入子目录 /src
catkin_create_pkg my_minimal_nodes roscpp std_msgs # 创建并且进入新目录 ~/ros_ws/src/my_minimal_nodes
完成后,新目录 ~/ros_ws/src/my_minimal_nodes
下会出现package.xml
、CMakeLists.txt
以及子目录src
和include
。
catkin_create_pkg
命令列举了两个依赖项:roscpp
和std_msgs
。roscpp
依赖项表明将要使用C++
编译器创建ROS
代码,当然包括兼容C++
的接口(ros::Publisher
、ros::Subscriber
等)。std_msgs
依赖项则表明将要依赖在ROS
中预定义的数据类型格式,如std_msgs::Float64
。
消息message
:即package.xml
文件。其内容如下:
<?xml version="1.0"?>
<package format="2">
<name>my_minimal_nodes</name>
<version>0.0.0</version>
<description>The my_minimal_nodes package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="haiden@todo.todo">haiden</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/my_minimal_nodes</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
package.xml
文件需要注意的点如下:
(1)行 <name>my_minimal_nodes</name> 不能修改,它包含了目录包文件名
(2)里面的作者名字和电子邮箱可以修改
(3)以下行说明了对roscpp和std_msgs的依赖
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
子目录src
用于存放C++
代码文件。
(2)编写一个ROS
发布器
在目录 ~/ros_ws/src/my_minimal_nodes/src
下利用以下命令在终端创建minimal_publisher.cpp
文件:
touch minimal_publisher.cpp # 生成ROS发布器程序
利用编辑器打开,并且写入以下发布器代码:
// 最小发布器程序:minimal_publisher.cpp
#include <ros/ros.h> // ROS库的头文件
#include <std_msgs/Float64.h> // 描述对象类型stg::Float64的头文件
int main(int argc, char **argv) { // 主函数带参,提供了使用命令行选项的机会
ros::init(argc, argv, "minimal_publisher"); // 节点名,每个节点都不一样,用于被ROS系统监视
ros::NodeHandle n; // 建立节点间的网络通信
ros::Publisher my_publisher_object = n.advertise<std_msgs::Float64>("topic1", 1); // 实例化发布器对象
//"topic1" 表明ROS系统收到通知后,该节点将要在topic1话题上发布stg::Float64类型的消息
// 1 表示缓冲区的大小
std_msgs::Float64 input_float; //创建一个stg::Float64类型对象input_float
// 该对象拥有名为data的成员
input_float.data = 0.0; // 为对象的data成员初始化
// 保持循环,直到ROS系统终止(可以通过关闭roscore终止ROS系统)
while (ros::ok())
{
input_float.data = input_float.data + 0.001; // 循环中,input_float.data的值每次自增0.001
my_publisher_object.publish(input_float); // 发布器对象将消息发布到topic1话题上
}
}
(3)编译ROS
节点
编译ROS
节点可以在指定目录下执行终端命令catkin_make
。即在终端打开ROS
工作区(~/ros_ws
),输入catkin_make
。
(a)在编译前,我们需要让catkin_make
知道新的代码minimal_publisher.cpp
的存在。方法是编辑运行catkin_create_pkg
时生成的CMakeList.txt
文件,做以下修改:
## Declare a cpp executable
add_executable(my_minimal_publisher src/minimal_publisher.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(my_minimal_publisher ${catkin_LIBRARIES} )
(b)在终端打开ROS
工作区目录(~/ros_ws
)输入:
catkin_make # 编译
(4)运行ROS
节点
打开一个终端,输入命令,保持这一终端运行:
roscore
打开另外一个终端,输入命令:
rosrun my_minimal_nodes my_minimal_publisher
(5)检查运行中的最小发布器节点
在(4)运行后,终端并不能显示节点的运行状态。如果需要检查节点是否在运行,我们可以采用以下方法:
(a)显示所有话题。
rostopic list # 在终端运行rostopic list可以显示所有话题
显示结果为:
/rosout
/rosout_agg
/topic1
(b)显示话题的发布数据频率
rostopic hz topic1 # 在终端运行rostopic hz topic1可以显示话题的发布数据频率
显示结果为:(可以看出最小发布器的频率约为14kHz)
subscribed to [/topic1]
average rate: 13622.946
min: 0.000s max: 0.009s std dev: 0.00045s window: 13456
average rate: 13783.123
min: 0.000s max: 0.009s std dev: 0.00044s window: 27456
average rate: 13921.255
min: 0.000s max: 0.009s std dev: 0.00043s window: 41723
(c)显示话题消耗的可用通信带宽
rostopic bw topic1 # 在终端运行rostopic bw topic1可以显示话题消耗的可用通信带宽
显示结果为:
subscribed to [/topic1]
average: 86.90KB/s
mean: 0.01KB min: 0.01KB max: 0.01KB window: 100
average: 98.50KB/s
mean: 0.01KB min: 0.01KB max: 0.01KB window: 100
(d)显示话题发布数据的数据类型
rostopic info topic1 # 在终端运行rostopic info topic1可以显示话题发布数据的数据类型
显示结果为:
Type: std_msgs/Float64
Publishers:
* /minimal_publisher (http://haiden-virtual-machine:38811/)
Subscribers: None
(e)显示发布器在话题上发布的所有数据
rostopic echo topic1 # 在终端运行rostopic echo topic1可以显示发布器在话题上发布的所有数据
显示结果为:(可以看出是以0.001增长的)
data: 9260031.83066
---
data: 9260031.83166
---
data: 9260031.83266
---
data: 9260031.83366
---
data: 9260031.83466
---
(f)显示roscore
系统运行的所有节点
rosnode list # 在终端运行rosnode list可以显示roscore系统运行的所有节点
显示结果为:(/rosout是通用进程,用于节点在终端上显示文本,由roscore默认启动)
/minimal_publisher
/rosout
(6)规划节点时间
在上面我们可以看到最小发布器的更新频率为14kHz
,理想的更新频率应该是1kHz
。因此优化发布器得到sleep_minimal_publisher.cpp
,代码如下:
// 更新频率为1Hz的最小发布器:sleep_minimal_publisher.cpp
#include <ros/ros.h> // ROS库的头文件
#include <std_msgs/Float64.h> // 描述对象类型stg::Float64的头文件
int main(int argc, char **argv) { // 主函数带参,提供了使用命令行选项的机会
ros::init(argc, argv, "minimal_publisher"); // 节点名,每个节点都不一样,用于被ROS系统监视
ros::NodeHandle n; // 建立节点间的网络通信
ros::Publisher my_publisher_object = n.advertise<std_msgs::Float64>("topic1", 1); // 实例化发布器对象
// "topic1" 表明ROS系统收到通知后,该节点将要在topic1话题上发布stg::Float64类型的消息
// 1 表示缓冲区的大小
ros::Rate naptime(1.0) // 设置更新频率为1Hz
std_msgs::Float64 input_float; // 创建一个stg::Float64类型对象input_float
// 该对象拥有名为data的成员
input_float.data = 0.0; // 为对象的data成员初始化
// 保持循环,直到ROS系统终止(可以通过关闭roscore终止ROS系统)
while (ros::ok())
{
input_float.data = input_float.data + 0.001; // 循环中,input_float.data的值每次自增0.001
my_publisher_object.publish(input_float); // 发布器对象将消息发布到topic1话题上
naptime.sleep() // 延迟更新
}
}
(7)编写一个ROS
订阅器
在目录 ~/ros_ws/src/my_minimal_nodes/src
下利用以下命令在终端创建minimal_subscriber.cpp
文件:
touch minimal_subscriber.cpp # 生成ROS订阅器程序
利用编辑器打开,并且写入以下订阅器代码:
// 最小订阅器:minimal_subscriber.cpp
#include<ros/ros.h>
#include<std_msgs/Float64.h>
void myCallback(const std_msgs::Float64& message_holder) // callback回调函数
{
// 当关联话题有新数据可用时,该订阅器会被唤醒
ROS_INFO("received value is: %f",message_holder.data); // 打印函数,功能和printf和cout类似,但是ROS_INFO()使用消息发布,
// 避免了因为显示驱动而放慢时间要求严格的代码,适用于日志记录和监控
} // 回调函数执行后,它会返回休眠状态,等待被话题新数据唤醒
int main(int argc, char **argv)
{
ros::init(argc,argv,"minimal_subscriber"); // 订阅器节点名
ros::NodeHandle n; // 建立节点间的网络通信
ros::Subscriber my_subscriber_object= n.subscribe("topic1",1,myCallback);
// Subscriber实例化
// 参数1:话题名,表明该订阅器监听my_minimal_publisher发布器在话题topic1上发布的数据
// 参数2:队列大小,如果回调函数无法跟上发布数据的频率,数据需要排队
// 参数3:回调函数名,用于接收topic1话题上的数据
ros::spin(); // spin()命令可以让主程序挂起,而回调函数可以持续工作,直到主程序运行结束
// 主程序运行结束的条件是roscore终止,即该ROS系统终止
return 0;
}
(8)编译和运行最小订阅器
编译前,需要在CMakeLists.txt
文件中做出增加:
add_executable(my_minimal_subscriber src/minimal_subscriber.cpp)
target_link_libraries(my_minimal_subscriber ${catkin_LIBRARIES} )
终端在ros_ws
目录下进行编译:
catkin_make
运行ROS
系统:
roscore # 运行ROS系统
打开另外一个终端,运行一个发布器:
rosrun my_minimal_nodes sleep_minimal_publisher # 运行发布器
打开另外一个终端,运行订阅器:
rosrun my_minimal_nodes my_minimal_subscriber # 运行订阅器
显示结果如下:
[ INFO] [1658200918.666544871]: received value is: 0.002000
[ INFO] [1658200919.665577116]: received value is: 0.003000
[ INFO] [1658200920.666047623]: received value is: 0.004000
[ INFO] [1658200921.665597160]: received value is: 0.005000
[ INFO] [1658200922.665693545]: received value is: 0.006000
[ INFO] [1658200923.665601282]: received value is: 0.007000
[ INFO] [1658200924.665771934]: received value is: 0.008000
[ INFO] [1658200925.666214094]: received value is: 0.009000
[ INFO] [1658200926.666306562]: received value is: 0.010000
[ INFO] [1658200927.666320329]: received value is: 0.011000
为了观察运行情况,打开另外一个终端,输入:
rosnode list
显示结果如下:(/rosout
是默认运行的)
/minimal_publisher
/minimal_subscriber
/rosout
为了观察系统运行的图像显示,打开另外一个终端,输入:
rqt_graph
显示结果如下:
2.1.3 更多ROS
工具
(1)catkin_simple
简化CMakeLists.txt
在目录 ~/ros_ws/src/my_minimal_nodes/src
下建立一个子目录learning_ros_external_packages
,将cs_create_pkg.py
放入该子目录下,打开终端:运行以下命令,在~/ros_ws/src
目录下新建一个my_minimal_nodes2
的程序包:
cs_create_pkg my_minimal_nodes2 roscpp std_msgs
可以打开.bashrc
文件,将上述命令添加进去。
简化后的CMakeLists.txt
文件如下:
cmake_minimum_required(VERSION 2.8.3)
project(my_minimal_nodes2)
find_package(catkin_simple REQUIRED)
#uncomment next line to use OpenCV library
#find_package(OpenCV REQUIRED)
#uncomment the next 2 lines to use the point-cloud library
#find_package(PCL 1.7 REQUIRED)
#include_directories(${PCL_INCLUDE_DIRS})
#uncomment the following 4 lines to use the Eigen library
#find_package(cmake_modules REQUIRED)
#find_package(Eigen3 REQUIRED)
#include_directories(${EIGEN3_INCLUDE_DIR})
#add_definitions(${EIGEN_DEFINITIONS})
catkin_simple()
# example boost usage
# find_package(Boost REQUIRED COMPONENTS system thread)
# C++0x support - not quite the same as final C++11!
# use carefully; can interfere with point-cloud library
# SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
# Libraries: uncomment the following and edit arguments to create a new library
# cs_add_library(my_lib src/my_lib.cpp)
# Executables: uncomment the following and edit arguments to compile new nodes
# may add more of these lines for more nodes from the same package
# cs_add_executable(example src/example.cpp)
#the following is required, if desire to link a node in this package with a library created in this same package
# edit the arguments to reference the named node and named library within this package
# target_link_library(example my_lib)
cs_install()
cs_export()
添加编译文件,进行编译即可。
(2)自动启动多个节点
在程序包my_minimal_nodes
中,我们可以创建一个子目录launch
并在此目录创建一个包含如下行的minimal_nodes.launch
文件:
<launch>
<node name= "publisher" pkg= "my_minimal_nodes" type= "sleepy_minimal_publisher" />
<node name= "subscriber" pkg= "my_minimal_nodes" type= "my_minimal_subscriber" />
</launch>
上述代码使用了XML
语法,使用关键词node
提示ROS
系统启动一个ROS
节点(经过catkin_make
编译后的可执行程序)。同时指定三个键值对:(a)pkg
:程序包名;(b)type
:可执行文件名;(c)name
:启动时可由ROS
识别的节点名(相当于在原先已经编译好的可执行文件的基础上多了一次重命名机会)。
在终端输入以下命令启动文件:
roslaunch my_minimal_nodes minimal_nodes.launch
终端显示结果如下:
... logging to /home/haiden/.ros/log/db233dd2-07f0-11ed-a28e-000c29be9f61/roslaunch-haiden-virtual-machine-89250.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://haiden-virtual-machine:34693/
SUMMARY
========
PARAMETERS
* /rosdistro: kinetic
* /rosversion: 1.12.17
NODES
/
publisher (my_minimal_nodes/sleep_minimal_publisher)
subscriber (my_minimal_nodes/my_minimal_subscriber)
auto-starting new master
process[master]: started with pid [89260]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to db233dd2-07f0-11ed-a28e-000c29be9f61
process[rosout-1]: started with pid [89273]
started core service [/rosout]
process[publisher-2]: started with pid [89277]
process[subscriber-3]: started with pid [89292]
(3)在ROS
控制台观察输出
终端输入rqt_console
可以利用ROS
控制台监视ROS
消息:
rqt_console # 终端输入rqt_console以监视ROS消息
控制台显示结果如下:
(4)使用rosbag
记录并且回放数据
记录数据:在程序包my_minimal_nodes
目录下创建一个子目录bagfiles
用来存放bag
文件,保持ROS系统在运行中(可以打开控制台观察),在该目录下打开终端并输入:
rosbag record topic1
rosbag
执行后,在该终端按下Ctrl+C
终止终端运行,在bagfiles
目录下将出现保存一个.bag
文件。
回放数据:重启ROS
系统,在另外一个终端只运行订阅器,不运行发布器,即在终端输入:
rosrun my_minimal_nodes my_minimal_subscriber
此时该终端是暂停的(打开控制台可以发现,并没有显示任何消息),因为topic1
话题并没有被激活。在bagfiles
打开一个新的终端,输入以下命令:
rosbag play filename.bag # filename是bag文件的文件名,运行时需要更改
此时可以从控制台观察到有消息出现,该消息是记录下来的数据,也就是说此时的rosbag
相对于发布器。这意味着测试时可以将记录的数据作为发布器的数据。
2.1.4 仿真器和控制器
在~/ros_ws/src/my_minimal_nodes
目录下编写最小仿真器代码如下:
// 最小仿真器:minimal_simulator.cpp
// 仿真 F = ma:通过加速度的积分来更新速度。
// 力的输入由另外一个节点发布到话题force_cmd上。得到速度后将速度发布到velocity话题上
#include<ros/ros.h>
#include<std_msgs/Float64.h>
std_msgs::Float64 g_velocity;
std_msgs::Float64 g_force;
void myCallback(const std_msgs::Float64& message_holder) { // 回调函数
ROS_INFO("received force value is: %f", message_holder.data);
g_force.data = message_holder.data; // 将新数据更新给全局变量g_force
}
int main(int argc, char **argv) { // 主函数同时拥有发布器和订阅器,相当于仿真器链条中的一个节点
ros::init(argc, argv, "minimal_simulator"); //节点名
ros::NodeHandle nh; // 建立节点间的网络通信
// 订阅器:订阅话题force_cmd,获取力数据(有回调函数,即会监视话题上的数据)
ros::Subscriber my_subscriber_object = nh.subscribe("force_cmd", 1, myCallback);
// 发布器:实例化发布器,初始化话题velocity,之后将速度发布到该话题上
ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("velocity", 1);
double mass = 1.0; // 质量
double dt = 0.01; // 数据传输时间间隔:10ms
double sample_rate = 1.0 / dt; // 采样频率
ros::Rate naptime(sample_rate);
g_velocity.data = 0.0; // 速度初始化为0,通过加速度积分更新
g_force.data = 0.0; // 力初始化为0,通过回调函数更新
while (ros::ok()) {
g_velocity.data = g_velocity.data + (g_force.data / mass) * dt; // 欧拉加速度积分
my_publisher_object.publish(g_velocity); // 发布器发布速度数据
ROS_INFO("velocity = %f", g_velocity.data); // 输出显示
ros::spinOnce();
naptime.sleep();
// 在控制器中,力数据的发布频率为10Hz,而仿真器速度更新频率为100Hz,也就是说仿真器将利用g_force保存的数据更新10次速度数据后才会接收到新的力数据。
}
return 0;
}
在~/ros_ws/src/my_minimal_nodes
目录下编写最小控制器代码如下:
// 最小控制器:minimal_controller.cpp
// 控制器订阅两个话题(velocity和vel_cmd),发布数据到话题force_cmd
#include<ros/ros.h>
#include<std_msgs/Float64.h>
std_msgs::Float64 g_velocity;
std_msgs::Float64 g_vel_cmd;
std_msgs::Float64 g_force;
void myCallbackVelocity(const std_msgs::Float64& message_holder) {
ROS_INFO("received velocity value is: %f", message_holder.data);
g_velocity.data = message_holder.data; // 将新数据更新给g_velocity
}
void myCallbackVelCmd(const std_msgs::Float64& message_holder) {
ROS_INFO("received velocity command value is: %f", message_holder.data);
g_vel_cmd.data = message_holder.data; // 将新数据更新给g_vel_cmd
}
int main(int argc, char **argv) {
ros::init(argc, argv, "minimal_controller"); // 节点名
ros::NodeHandle nh; // 建立节点间的网络通信
// 订阅器1:订阅话题velocity(带有回调函数)
ros::Subscriber my_subscriber_object1 = nh.subscribe("velocity", 1, myCallbackVelocity);
// 订阅器2:订阅话题vel_cmd(带有回调函数)
ros::Subscriber my_subscriber_object2 = nh.subscribe("vel_cmd", 1, myCallbackVelCmd);
// 发布器:实例化发布器,初始化话题force_cmd,并且将数据发布到该话题上
ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("force_cmd", 1);
double Kv = 1.0; // 速度反馈增益
double dt_controller = 0.1; // 控制器控制时间间隔
double sample_rate = 1.0 / dt_controller; // 控制器控制更新频率
ros::Rate naptime(sample_rate);
g_velocity.data = 0.0; // 初始化速度
g_force.data = 0.0; // 初始化力大小
g_vel_cmd.data = 0.0; // 初始化速度命令
double vel_err = 0.0; // 速度误差
while (ros::ok()) {
vel_err = g_vel_cmd.data - g_velocity.data; // 计算速度期望值于实际值的误差
g_force.data = Kv*vel_err; // 根据速度反馈增益设计控制力大小
my_publisher_object.publish(g_force); // 将控制器的控制力大小发布到话题force_cmd上
ROS_INFO("force command = %f", g_force.data);
ros::spinOnce(); // 允许回调函数在100ms内传入数据
naptime.sleep(); // 控制控制器更新频率
}
return 0; // ROS系统终止,即终端命令roscore终止
}
更新CMakeList.txt
,并使用catkin_make
编译。
打开一个终端运行ROS
系统:
roscore
打开另外一个终端,运行仿真:
rosrun my_minimal_nodes my_minimal_simulator
打开另外一个终端,运行控制器:
rosrun my_minimal_nodes my_minimal_controller
打开另外一个终端,运行以下命令,以使得结果可视化:
rqt_plot vel_cmd/data,velocity/data,force_cmd/data
打开另外一个终端,输入速度控制命令:
rostopic pub -r 10 vel_cmd std_msgs/Float64 1.0
结果显示如下图: