在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 copyif __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 copyif __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 = Nonedef switch_node_callback(msg):global file_to_launchfile_to_launch = msg.dataprint("file_to_launch:"+file_to_launch)returnif __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)# mainwhile 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_launchif 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 = Nonerospy.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文件的时候加一下。