如何在代码中启动与关闭ROS节点

在ROS开发中，节点的管理是很重要的一部分，其中有一些节点大部分时候用不到，只会在特定情况下被启动（比如建图节点）同时这些节点在使用完后还需要被关闭，因此我们就需要在程序中对这些节点进行启动与关闭的管理。

在python中，针对这个问题ROS中可以使用roslaunch功能包解决这个问题。通过简单的几行代码就可以实现具体功能：

uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_file])
launch.start()

我们可以通过一个简单的测试程序来看看这个代码是如何使用的：
首先，我们建立两个简单的节点node1.py与node2.py:

node1.py:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import math
import sagepy
import rospy
import copy

if __name__ == '__main__':
    rospy.init_node('node1')
    rospy.sleep(3)
    while not rospy.is_shutdown():
        print("node1")
        rospy.sleep(3)

node2.py:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import math
import sagepy
import rospy
import copy

if __name__ == '__main__':
    rospy.init_node('node2')
    rospy.sleep(3)
    while not rospy.is_shutdown():
        print("node2")
        rospy.sleep(3)
        pintt(node[2])

同时建立两个launch文件：

node1.launch:

<launch>
    <node pkg="rosnodekill" type="node1.py" name="node1" output="screen" respawn="true"/>
</launch>

node2.launch:

<launch>
    <node pkg="rosnodekill" type="node2.py" name="node2" output="screen" respawn="true"/>
</launch>

最后建立一个主函数：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import os
import subprocess
import time
import roslaunch
from rosnodekill.srv import data
launch = None
file_to_launch = None

def switch_node_callback(msg):
    global file_to_launch
    file_to_launch = msg.data
    print("file_to_launch:"+file_to_launch)
    return

if __name__ == '__main__':
    rospy.init_node('get_robotid_db_server')
    rospy.loginfo(rospy.get_name()+" started!")
    get_map_pose_server = rospy.Service('switch_node', data, switch_node_callback)
    # main
    while not rospy.is_shutdown():   
        print('debug')
        if file_to_launch is not None:
            print(file_to_launch)
            launch_file = '/home/computer_name/addtxt/src/rosnodekill/launch/'+file_to_launch
            if launch is not None:
                launch.shutdown()
            uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
            roslaunch.configure_logging(uuid)
            launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_file])
            launch.start()
            file_to_launch = None
            rospy.sleep(1)
        rospy.sleep(1)

简单看一下这个主函数，它可以分为两个部分：一个是回调函数接受来自外部给定的launch文件名称，一个是节点管理负责启动与关闭当前launch文件的节点。启动这个程序时，终端显示如下：
在这里插入图片描述
此时代表主程序已经开启，同时程序一直在执行while循环。此时新开一个终端，调用switch_node服务传入一个文件名：

rosservice call /switch_node "data: 'node1.launch'"

则主程序会响应该请求并打开node1.launch文件：
在这里插入图片描述根据这个我们可以看出node1已经被执行，主程序中开启了我们需要的节点。然后我们再次请求switch_node将节点进行切换到node2.launch:

rosservice call /switch_node "data: 'node2.launch'"

此时终端显示如下：
在这里插入图片描述
可以看到原来的node1被关闭后，node2被开启。如此，则完成了两个节点间的切换。

另外注意到一个问题：在前面的两个launch文件中，都加了respawn="true"选项，该选项的作用是当节点被关闭时会重新开启。但是在这里它的优先级是低于launch节点程序的。因为node1被关闭后并没有重启。所以launch管理相当于是关闭了这个launch问不是这个节点。

但同时，如果这个节点不是被launch.shutdown()的话，它是可以被重启的，例如在node2中，故意最后多加了个打印输出，该输出是异常输出会导致节点崩溃，但是崩溃后我们可以看到该节点是会再次重启的：
在这里插入图片描述如果这里不加respawn="true"的话则是不会重启的，所以最好在写launch文件的时候加一下。