[系统运维]ROS 创建msg和srv 编写发布者和订阅者节点 编写服务端和客户端节点(python版本)

rosed

利用它可以直接通过软件包名编辑包中的文件，而无需键入完整路径。

$ rosed roscpp Logger.msg

编辑roscpp软件包中的Logger.msg文件。退出vim，按下键盘上的Esc，然后分别按下:q!

Tap补全，在不知道准确文件名的情况下，你也可以轻松地查看和编辑包中的所有文件。

$ rosed roscpp <tab><tab>

jym@ubuntu:~$ rosed roscpp 
Empty.srv                   roscpp.cmake
genmsg_cpp.py               roscppConfig.cmake
gensrv_cpp.py               roscppConfig-version.cmake
GetLoggers.srv              roscpp-msg-extras.cmake
Logger.msg                  roscpp-msg-paths.cmake
msg_gen.py                  SetLoggerLevel.srv
package.xml                 

msg和srv

msg（消息）：msg文件就是文本文件，用于描述ROS消息的字段。它们用于为不同编程语言编写的消息生成源代码。

srv（服务）：一个srv文件描述一个服务。它由两部分组成：请求（request）和响应（response）。

创建msg

在之前创建的软件包里定义一个新的消息。直接使用roscd beginner_tutorials，会报错。显示：

roscd: No such package/stack 'beginner_tutorials'

因为roscd只能切换到那些路径已经包含在ROS_PACKAGE_PATH环境变量中的软件包。

工作空间构建完成后，要将这个工作空间添加到ROS环境中，需要source一下生成的配置文件。

然后用roscd beginner_tutorials就没问题了。

jym@ubuntu:~$ roscd beginner_tutorials
roscd: No such package/stack 'beginner_tutorials'
jym@ubuntu:~$ . ~/catkin_ws/devel/setup.bash
jym@ubuntu:~$ roscd beginner_tutorials
jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ 

然后输入：

$ mkdir msg
$ echo "int64 num" > msg/Num.msg

那么就在msg文件夹里面生成了Num.msg文件。

然后打开package.xml，确保有以下两行代码，没有的话加上。

确保msg文件能被转换为C++、Python和其他语言的源代码。

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

在构建时，只需要message_generation，而在运行时，只需要message_runtime。

输入下面指令可以直接打开CMakeLists.txt文件

$  beginner_tutorials CMakeLists.txt

按esc后再按i，可以进入编辑。

1.在CMakeLists.txt文件中，为已经存在里面的find_package调用添加message_generation依赖项

添加message_generation后的find_package如下所示。

find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
   message_generation
)

2.还要确保导出消息的运行时依赖关系：

找到catkin_package添加CATKIN_DEPENDS message_runtime

catkin_package(
  ...
  CATKIN_DEPENDS message_runtime ...
  ...)

3.找到如下代码块：

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

删除#符号来取消注释，然后将Message*.msg替换为你的.msg文件名，就像下边这样：

add_message_files(
  FILES
  Num.msg
)

手动添加.msg文件后，我们要确保CMake知道何时需要重新配置项目。

4.现在必须确保generate_messages()函数被调用：

取消下面几行的注释：添加任意你的消息用到的包含.msg文件的软件包（本例中为std_msgs）

# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )

以上是创建消息的所有步骤。

使用rosmsg

通过rosmsg show命令看看ROS能否识别创建的消息。

输入：rosmsg show beginner_tutorials/Num

可看到：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ rosmsg show beginner_tutorials/Num
int64 num

上面的例子中，消息类型包含两部分：

• beginner_tutorials – 定义消息的软件包
• Num – 消息的名称Num

如果不记得msg在哪个包中，也可以省略包名称。$ rosmsg show Num

可看到：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ rosmsg show Num
[beginner_tutorials/Num]:
int64 num

创建srv

可从另一个包复制现有的srv定义，而不是手动创建新的srv。

roscp是一个实用的命令行工具，用于将文件从一个包复制到另一个包。

从rospy_tutorials包中复制一个服务：

$ roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv

剩下的和创建msg类似：

1.打开package.xml，确保有以下两行代码：

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

2.在CMakeLists.txt文件中，为已经存在里面的find_package调用添加message_generation依赖项。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

3.在package.xml中修改服务字段。

删除#符号来取消注释，然后将Service*.srv替换为你的.srv文件名，就像下边这样：

add_service_files(
  FILES
  AddTwoInts.srv
)

以上就是创建服务的所有步骤。

使用rossrv

通过rossrv show命令看看ROS能否识别创建的服务。

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ rossrv show beginner_tutorials/AddTwoInts
int64 a
int64 b
---
int64 sum

也可以在不指定包名的情况下找到这样的服务。

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ rossrv show AddTwoInts
[rospy_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

[beginner_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

第一个是刚刚在beginner_tutorials包中创建的，第二个是之前rospy_tutorials包中已经存在的。

重新make一下软件包

现在我们已经创建了一些新消息，所以需要重新make一下软件包：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ roscd beginner_tutorials
jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ cd ../..
jym@ubuntu:~/catkin_ws$ catkin_make

jym@ubuntu:~/catkin_ws$ cd -
/home/jym/catkin_ws/src/beginner_tutorials
jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ 

msg目录中的任何.msg文件都将生成所有支持语言的代码。

C++消息的头文件将生成在~/catkin_ws/devel/include/beginner_tutorials/。Python脚本将创建在~/catkin_ws/devel/lib/python3/dist-packages/beginner_tutorials/msg。而Lisp文件则出现在~/catkin_ws/devel/share/common-lisp/ros/beginner_tutorials/msg/。

类似地，srv目录中的任何.srv文件都将生成支持语言的代码。对于C++，头文件将生成在消息的头文件的同一目录中。对于Python和Lisp，会在msg目录旁边的srv目录中。

编写发布者节点

“节点”是连接到ROS网络的可执行文件。

这里将创建talker（发布者）节点，该节点将不断广播消息。

1.将目录切换到创建的beginner_tutorials包中：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ roscd beginner_tutorials

2.创建一个scripts目录来存放Python脚本：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ mkdir scripts
jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ cd scripts

3.在scripts目录下创建talker.py文件：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ vim talker.py

输入：

#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

然后给它执行权限：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ chmod +x talker.py

可以通过下面的命令查看和编辑这个文件：

$ rosed beginner_tutorials talker.py 

4.将以下内容添加到CMakeLists.txt文件

catkin_install_python(PROGRAMS scripts/talker.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

确保正确安装Python脚本，并使用合适的Python解释器。

发布者节点代码解析

#!/usr/bin/env python

每个Python ROS节点的最开头都有这个声明。第一行确保脚本作为Python脚本执行。

import rospy
from std_msgs.msg import String

如果要编写ROS节点，则需要导入rospy。std_msgs.msg的导入则是为了使我们能够重用std_msgs/String消息类型（即一个简单的字符串容器）来发布。

    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)

这部分代码定义了talker与其他ROS部分的接口。

pub = rospy.Publisher("chatter", String, queue_size=10)声明该节点正在使用String消息类型发布到chatter话题。

这里的String实际上是std_msgs.msg.String类。

queue_size参数是在ROS Hydro及更新版本中新增的，用于在订阅者接收消息的速度不够快的情况下，限制排队的消息数量。

下一行的rospy.init_node(NAME, ...)非常重要，因为它把该节点的名称告诉了rospy。

只有rospy掌握了这一信息后，才会开始与ROS主节点进行通信。

在本例中，节点使用talker名称。注意：名称必须是基本名称，不能包含任何斜杠。

anonymous = True会让名称末尾添加随机数，来确保节点具有唯一的名称。

rate = rospy.Rate(10) # 10hz

此行创建一个Rate对象rate。借助其方法sleep()，它提供了一种方便的方法，来以你想要的速率循环。它的参数是10，即表示希望它每秒循环10次。

   while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

这个循环是一个相当标准的rospy结构：检查rospy.is_shutdown()标志，然后执行代码逻辑。

查看is_shutdown()以检查程序是否应该退出（例如有Ctrl+C或其他）。

在本例中，代码逻辑即对public .publish(hello_str)的调用，它将一个字符串发布到chatter话题。

循环的部分还调用了rate.sleep()，它在循环中可以用刚刚好的睡眠时间维持期望的速率。

此循环还调用了rospy.loginfo(str)，它有3个任务：打印消息到屏幕上；把消息写入节点的日志文件中；写入rosout。

rosout是一个方便的调试工具：可以使用rqt_console来拉取消息，而不必在控制台窗口找节点的输出。

    try:
        talker()
    except rospy.ROSInterruptException:
        pass

除了标准的Python __main__检查，它还会捕获一个rospy.ROSInterruptException异常，当按下Ctrl+C或节点因其他原因关闭时，这一异常就会被rospy.sleep()和rospy.Rate.sleep()抛出。

编写订阅者节点

1.将目录切换到scripts目录中：

$ roscd beginner_tutorials/scripts/

2.创建listener.py文件

$ vim listener.py

输入：

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
    
def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("chatter", String, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()

3.给它执行权限

$ chmod +x listener.py

4.编辑你CMakeLists.txt中的catkin_install_python()调用

可以用rosed打开CMakeLists.txt

rosed beginner_tutorials CMakeLists.txt

然后添加

catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

订阅者节点代码解析

listener.py的代码类似于talker.py，只不过我们为订阅消息引入了一种新的基于回调的机制。

   rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("chatter", String, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

这声明节点订阅了chatter话题，类型是std_msgs.msgs.String。当接收到新消息时，callback函数被调用，消息作为第一个参数。

稍微更改了对rospy.init_node()的调用。添加了anonymous=True关键字参数。

ROS要求每个节点都有一个唯一的名称，如果出现具有相同名称的节点，则会与前一个节点发生冲突，这样一来，出现故障的节点很容易地被踢出网络。

anonymous=True标志会告诉rospy为节点生成唯一的名称，这样就很容易可以有多个listener.py一起运行。

rospy.spin()只是不让节点退出，直到节点被明确关闭。

与roscpp不同，rospy.spin()不影响订阅者回调函数，因为它们有自己的线程。

构建节点

使用CMake作为构建系统。即使是Python节点也必须使用它。这是为了确保能为创建的消息和服务自动生成Python代码。

回到catkin工作空间，然后运行catkin_make：

$ cd ~/catkin_ws
$ catkin_make

运行发布者和订阅者节点

开三个终端：

第一个输入：

$ roscore

第二个：

运行talker发布者节点

$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials talker.py   

第三个：

$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials listener.py   

可以通过

$ rosrun rqt_graph rqt_graph

看到当前运行的节点和话题

发布者节点发出：

[INFO] [1635421895.494006]: hello world 1635421895.4938853
[INFO] [1635421895.594515]: hello world 1635421895.5943954
[INFO] [1635421895.694447]: hello world 1635421895.6943207
[INFO] [1635421895.793802]: hello world 1635421895.7936826
[INFO] [1635421895.893810]: hello world 1635421895.893687
[INFO] [1635421895.993590]: hello world 1635421895.9934704
[INFO] [1635421896.094563]: hello world 1635421896.0942173
[INFO] [1635421896.194204]: hello world 1635421896.1938524
[INFO] [1635421896.294536]: hello world 1635421896.2941887
[INFO] [1635421896.394086]: hello world 1635421896.3939602
[INFO] [1635421896.493973]: hello world 1635421896.4936652
[INFO] [1635421896.594645]: hello world 1635421896.5942914
[INFO] [1635421896.694563]: hello world 1635421896.6942132
[INFO] [1635421896.794407]: hello world 1635421896.7942781
[INFO] [1635421896.894170]: hello world 1635421896.8938253
^Cjym@ubuntu:~/catkin_ws$ 

订阅者节点收到：

[INFO] [1635421896.198918]: /listener_8151_1635421497025I heard hello world 1635421896.1938524
[INFO] [1635421896.299254]: /listener_8151_1635421497025I heard hello world 1635421896.2941887
[INFO] [1635421896.397918]: /listener_8151_1635421497025I heard hello world 1635421896.3939602
[INFO] [1635421896.498270]: /listener_8151_1635421497025I heard hello world 1635421896.4936652
[INFO] [1635421896.599628]: /listener_8151_1635421497025I heard hello world 1635421896.5942914
[INFO] [1635421896.698885]: /listener_8151_1635421497025I heard hello world 1635421896.6942132
[INFO] [1635421896.795746]: /listener_8151_1635421497025I heard hello world 1635421896.7942781
[INFO] [1635421896.898501]: /listener_8151_1635421497025I heard hello world 1635421896.8938253

可以看到在发布者节点添加ctrl+c之后，订阅者节点也不接收消息了。

编写服务节点

前面已经创建了需要的服务AddTwoInts.srv。

接下来：

在beginner_tutorials包中创建scripts/add_two_ints_server.py文件并粘贴以下内容进去：

#!/usr/bin/env python

from __future__ import print_function

from beginner_tutorials.srv import AddTwoInts,AddTwoIntsResponse
import rospy

def handle_add_two_ints(req):
    print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
    return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():
    rospy.init_node('add_two_ints_server')
    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
    print("Ready to add two ints.")
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

然后将以下内容添加到CMakeLists.txt文件。确保正确安装Python脚本，并使用合适的Python解释器。

catkin_install_python(PROGRAMS scripts/add_two_ints_server.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

以上流程在linux上输入的指令代码：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials$ roscd beginner_tutorials/scripts/
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ vim add_two_ints_server.py
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ chmod +x add_two_ints_server.py
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ rosed beginner_tutorials CMakeLists.txt

使用rospy编写服务。使用init_node()声明我们的节点，然后再声明我们的服务：

    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)

这声明了一个名为add_two_ints的新服务，其服务类型为AddTwoInts。所有的请求（request）都传递给了handle_add_two_ints函数。handle_add_two_ints被AddTwoIntsRequest的实例调用，返回AddTwoIntsResponse实例。

就像订阅者中的例子一样，rospy.spin()可以防止代码在服务关闭之前退出。

编写客户端节点

在beginner_tutorials包中创建scripts/add_two_ints_client.py文件并粘贴以下内容进去：

#!/usr/bin/env python

from __future__ import print_function

import sys
import rospy
from beginner_tutorials.srv import *

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        print(usage())
        sys.exit(1)
    print("Requesting %s+%s"%(x, y))
    print("%s + %s = %s"%(x, y, add_two_ints_client(x, y)))

同样将以下内容添加到CMakeLists.txt文件。

catkin_install_python(PROGRAMS scripts/add_two_ints_server.py scripts/add_two_ints_client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

以上流程在linux上输入的指令代码：

jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ roscd beginner_tutorials/scripts/
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ vim add_two_ints_client.py
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ chmod +x add_two_ints_client.py
jym@ubuntu:~/catkin_ws/src/beginner_tutorials/scripts$ rosed beginner_tutorials CMakeLists.txt

客户端（用来调用服务）的代码分析：

对于客户端来说不需要调用init_node()。首先调用：

    rospy.wait_for_service('add_two_ints')

这是一种很方便的方法，可以让在add_two_ints服务可用之前一直阻塞。

        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)

这里为服务的调用创建了一个句柄（handle）。

        resp1 = add_two_ints(x, y)
        return resp1.sum

然后可以使用这个句柄，就像普通的函数一样调用它。

因为已经将服务的类型声明为AddTwoInts，它会生成AddTwoIntsRequest对象 (you’re free to pass in your own instead)。如果调用失败，rospy.ServiceException将会抛出，所以应该弄一个合适的try/except部分。

使用CMake作为构建系统。为了确保能为创建的消息和服务自动生成Python代码。

切换当前目录到catkin工作空间，然后运行catkin_make：

$ cd ~/catkin_ws
$ catkin_make

现在已经编写了一个简单的服务和客户端

运行服务端和客户端节点

开三个终端：

第一个输入：

$ roscore

第二个：

运行talker发布者节点

$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials add_two_ints_server.py  

第三个：

$ cd ~/catkin_ws
$ source ./devel/setup.bash
$ rosrun beginner_tutorials add_two_ints_client.py 1 3 

服务节点运行效果：

jym@ubuntu:~/catkin_ws$ rosrun beginner_tutorials add_two_ints_server.py
Ready to add two ints.
Returning [1 + 3 = 4]

客户端节点运行效果：

jym@ubuntu:~/catkin_ws$ rosrun beginner_tutorials add_two_ints_client.py 1 3 
Requesting 1+3
1 + 3 = 4