ROS 学习记录（三）——ROS 通信机制实践
文章目录
话题通信
话题发布
在安装完 ROS 的测试样例中，系统提供了一个 turtlesim_node 节点，用于显示乌龟移动路径。我们要尝试的就是向这个节点发布话题，让它显示我们自定义的移动轨迹。
首先运行 ros 和 turtlesim_node 节点：
1roscore &
2rosrun turtlesim turtlesim_node
查看当前可用话题：
1rostopic list
输出：
1/rosout
2/rosout_agg
3/turtle1/cmd_vel
4/turtle1/color_sensor
5/turtle1/pose
可以看到，turtlesim_node 节点提供了三个话题，而我们要发布的轨迹信息就发布在 /turtle1/cmd_vel 话题上。
用下面的命令查看 /turtle1/cmd_vel 话题的类型：
1rostopic type /turtle1/cmd_vel
输出：
1geometry_msgs/Twist
可以看到，/turtle1/cmd_vel 话题的类型是 geometry_msgs/Twist。再查看一下这个类型的定义：
1rosmsg show geometry_msgs/Twist
输出：
1geometry_msgs/Vector3 linear
2 float64 x
3 float64 y
4 float64 z
5geometry_msgs/Vector3 angular
6 float64 x
7 float64 y
8 float64 z
可以看到，/turtle1/cmd_vel 话题的消息类型是 geometry_msgs/Twist，其中包含两个成员变量 linear 和 angular，分别表示线速度和角速度。我们希望乌龟做匀速圆周运动，因此设定其线速度为 1，角速度为 0.5，即 linear.x = 1, angular.z = 0.5 并发布到 /turtle1/cmd_vel 话题。随后开始编写代码。
C++ 实现：
1#include "ros/ros.h"
2#include "geometry_msgs/Twist.h"
3
4int main(int argc, char **argv)
5{
6 setlocale(LC_ALL, "");
7 ros::init(argc, argv, "topic_publish_cpp");
8 ros::NodeHandle n;
9
10 ros::Publisher publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
11 geometry_msgs::Twist twist;
12 twist.linear.x = 1.0;
13 twist.linear.y = 0.0;
14 twist.linear.z = 0.0;
15 twist.angular.x = 0.0;
16 twist.angular.y = 0.0;
17 twist.angular.z = 0.5;
18
19 ros::Rate rate(10);
20 while (ros::ok())
21 {
22 publisher.publish(twist);
23 ros::spinOnce();
24 rate.sleep();
25 }
26 return 0;
27}
Python 实现：
1import rospy
2from geometry_msgs.msg import Twist
3
4
5def main():
6 rospy.init_node("topic_publish_py")
7 publisher = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=1000)
8
9 twist = Twist()
10 twist.linear.x = 1.0
11 twist.linear.y = 0.0
12 twist.linear.z = 0.0
13 twist.angular.x = 0.0
14 twist.angular.y = 0.0
15 twist.angular.z = 0.5
16
17 rate = rospy.Rate(10)
18 while not rospy.is_shutdown():
19 publisher.publish(twist)
20 rate.sleep()
21
22
23if __name__ == "__main__":
24 main()
修改 launch 文件：
1<launch>
2 <node pkg="turtle" type="topic_publish_cpp_node" name="topic_publish_cpp_node" />
3 <node pkg="turtle" type="topic_publish_py.py" name="topic_publish_py_node" />
4
5 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" required="true" />
6</launch>
required 属性表示这个节点必须运行，如果该节点关闭则通过当前 launch 文件启动的节点也会关闭。
运行：
1roslaunch turtle topic_publish.launch
随后就可以看见小乌龟做匀速圆周运动了。
话题订阅
刚才在执行 rostopic list 时，有一个话题为 /turtle1/pose，这个话题是 turtlesim_node 节点发布的，用于显示当前乌龟的位置信息。我们可以订阅这个话题，并在控制台打印出来。
查看 /turtle1/pose 话题的类型：
1rostopic type /turtle1/pose
输出：
1turtlesim/Pose
可以看到，/turtle1/pose 话题的类型是 turtlesim/Pose，这个类型定义在 turtlesim_node 包中。我们可以查看这个类型的定义：
1rosmsg show turtlesim/Pose
输出：
1float32 x
2float32 y
3float32 theta
4float32 linear_velocity
5float32 angular_velocity
这些参数分别代表了当前乌龟的 x 坐标、y 坐标、朝向角、线速度和角速度。我们可以编写代码订阅这个话题并将信息打印出来。
C++ 实现：
1#include "ros/ros.h"
2#include "turtlesim/Pose.h"
3#include <cstdlib>
4
5void callback(const turtlesim::Pose::ConstPtr &msg)
6{
7 printf("\033[H");
8 printf("Turtle Position Information: \nx=%.3f \ny=%.3f \ntheta=%.3f \nlinear_velocity=%.3f \nangular_velocity=%.3f \n",
9 msg->x,
10 msg->y,
11 msg->theta,
12 msg->linear_velocity,
13 msg->angular_velocity);
14}
15
16int main(int argc, char **argv)
17{
18 setlocale(LC_ALL, "");
19 ros::init(argc, argv, "topic_subscribe_cpp");
20 ros::NodeHandle n;
21 std::system("clear");
22 ros::Subscriber subscriber = n.subscribe<turtlesim::Pose>("/turtle1/pose", 1000, callback);
23 ros::spin();
24 return 0;
25}
Python 实现：
1import os
2import rospy
3from turtlesim.msg import Pose
4
5
6def callback(msg):
7 print("\033[H")
8 print(
9 "Turtle Position Information: \nx={:.3f} \ny={:.3f} \ntheta={:.3f} \nlinear_velocity={:.3f} \nangular_velocity={:.3f} \n".format(
10 msg.x, msg.y, msg.theta, msg.linear_velocity, msg.angular_velocity
11 )
12 )
13
14
15def main():
16 rospy.init_node("topic_subscribe_py")
17 os.system("clear")
18 subscriber = rospy.Subscriber("/turtle1/pose", Pose, callback)
19 rospy.spin()
20
21
22if __name__ == "__main__":
23 main()
代码中的 ANSI 控制字符 \033[H 用于清屏，且避免使用 clear 命令时终端频闪。
用 launch 文件启动多个节点固然方便，但是 roslaunch 命令启动的节点全部输出都在当前终端里，而我们这里需要用到多个终端来观察效果，所以需要开多个终端分别运行节点：
1roscore &
2rosrun turtlesim turtlesim_node
3rosrun turtle topic_subscribe_cpp_node
4rosrun turtlesim turtle_teleop_key
turtle_teleop_key 节点用于控制小乌龟的运动，我们可以用方向键控制小乌龟的运动方向。
或者直接用 topic_publish_py.py 节点发布轨迹信息：
1roscore &
2rosrun turtlesim turtlesim_node
3rosrun turtle topic_subscribe_py.py
4rosrun turtle topic_publish_py.py
服务通信
查看 turtlesim_node 节点提供的服务：
1rosservice list
输出：
1/clear
2/kill
3/reset
4/rosout/get_loggers
5/rosout/set_logger_level
6/spawn
7/turtle1/set_pen
8/turtle1/teleport_absolute
9/turtle1/teleport_relative
10/turtlesim/get_loggers
11/turtlesim/set_logger_level
其中 kill 和 spawn 分别用于移除小乌龟和生成小乌龟。查看这些服务的详细信息：
1jackgdn@ubuntu:~$ rosservice type /kill
2turtlesim/Kill
3jackgdn@ubuntu:~$ rossrv show turtlesim/Kill
4string name
5---
6
7jackgdn@ubuntu:~$ rosservice type /spawn
8turtlesim/Spawn
9jackgdn@ubuntu:~$ rossrv show turtlesim/Spawn
10float32 x
11float32 y
12float32 theta
13string name
14---
15string name
根据这些信息编写脚本，实现自定义位置生成乌龟，以及移除乌龟：
C++ 实现：
1#include "ros/ros.h"
2#include "turtlesim/Spawn.h"
3#include "turtlesim/Kill.h"
4#include <iostream>
5#include <vector>
6
7int main(int argc, char **argv)
8{
9 setlocale(LC_ALL, "");
10 ros::init(argc, argv, "service_cpp");
11 ros::NodeHandle n;
12 ros::ServiceClient client_spawn = n.serviceClient<turtlesim::Spawn>("/spawn");
13 ros::ServiceClient client_kill = n.serviceClient<turtlesim::Kill>("/kill");
14 client_spawn.waitForExistence();
15 client_kill.waitForExistence();
16
17 std::vector<std::string> turtle_names;
18 float x, y, theta;
19 std::string name;
20 while (std::cin >> x >> y >> theta >> name) {
21 turtlesim::Spawn spawn;
22 spawn.request.x = x;
23 spawn.request.y = y;
24 spawn.request.theta = theta;
25 spawn.request.name = name;
26
27 if (client_spawn.call(spawn)) {
28 ROS_INFO("Turtle Spawned");
29 turtle_names.push_back(name);
30 }
31 else
32 {
33 ROS_ERROR("Failed to Spawn Turtle");
34 }
35 }
36
37 for (auto name : turtle_names) {
38 turtlesim::Kill kill;
39 kill.request.name = name;
40 if (client_kill.call(kill)) {
41 ROS_INFO("Turtle Removed!");
42 }
43 else
44 {
45 ROS_ERROR("Failed to Remove Turtle");
46 }
47 }
48
49 return 0;
50}
Python 实现：
1#!/usr/bin/python3
2
3import rospy
4from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse, Kill, KillRequest , KillResponse
5
6
7def main():
8 rospy.init_node("service_py")
9 client_spawn = rospy.ServiceProxy("/spawn", Spawn)
10 client_kill = rospy.ServiceProxy("/kill", Kill)
11 client_spawn.wait_for_service()
12 client_kill.wait_for_service()
13
14 turtle_name = list()
15 while True:
16 try:
17 try:
18 x, y, theta, name = input().strip().split()
19 spawn_request = SpawnRequest()
20 spawn_request.x = float(x)
21 spawn_request.y = float(y)
22 spawn_request.theta = float(theta)
23 spawn_request.name = name
24 except ValueError:
25 rospy.logerr("Invalid Input")
26 continue
27
28 try:
29 client_spawn.call(spawn_request)
30 rospy.loginfo("Turtle Spawned")
31 turtle_name.append(name)
32 except rospy.ServiceException:
33 rospy.logerr("Failed to Spawn Turtle")
34
35 except EOFError:
36 break
37
38 for name in turtle_name:
39 kill_request = KillRequest()
40 kill_request.name = name
41 try:
42 client_kill.call(kill_request)
43 rospy.loginfo("Turtle Removed")
44 except rospy.ServiceException:
45 rospy.logerr("Failed to Remove Turtle")
46
47
48if __name__ == "__main__":
49 main()
配置文件的编写就不再赘述。
参数服务器通信
首先查看 turtlesim_node 节点提供的参数：
1rosparam list
输出：
1/rosdistro
2/roslaunch/uris/host_ubuntu__46433
3/rosversion
4/run_id
5/turtlesim/background_b
6/turtlesim/background_g
7/turtlesim/background_r
可以看到，/turtlesim/background_b, /turtlesim/background_g, /turtlesim/background_r 三个参数用于控制背景颜色，通过修改这些参数可以改变背景颜色。/turtlesim 表示这些参数的命名空间。
查看这些参数的类型：
1rosparam get /turtlesim
输出：
1background_b: 255
2background_g: 86
3background_r: 69
可以看到，这些参数的类型都是 int。编写脚本修改参数。
C++ 实现：
1#include "ros/ros.h"
2#include <iostream>
3
4int main(int argc, char **argv)
5{
6 setlocale(LC_ALL, "");
7 ros::init(argc, argv, "param_cpp");
8
9 ros::NodeHandle n("turtlesim_node"); // 指定命名空间
10 n.setParam("background_r", 113);
11 n.setParam("background_g", 113);
12 n.setParam("background_b", 113);
13
14 return 0;
15}
Python 实现：
1import rospy
2
3
4def main():
5 rospy.init_node("param_py")
6 rospy.set_param("background_r", 113)
7 rospy.set_param("background_g", 113)
8 rospy.set_param("background_b", 113)
9
10
11if __name__ == "__main__":
12 main()
用于 launch 文件无法决定节点的加载顺序，因此我们需要在 turtlesim_node 节点启动前修改参数。
1roscore &
2rosrun turtle param_cpp_node
3rosrun turtlesim turtlesim_node
随后可以看到背景颜色已经变成灰色。
由于 Python 脚本中无法指定命名空间，因此需要在 rospy.set_param 函数中指定参数的完整路径（我显然没有这样做），或者在启动节点时传递 __ns: 参数。
1roscore &
2rosrun turtle param_py.py __ns:="turtlesim"
3rosrun turtlesim turtlesim_node
这样可以起到同样的效果。