roslaunchのコードAPIを使う part2

Day9の記事を受けて。
→ https://qiita.com/Nasupl_r/items/a1449cf192c57f03ffa5

「Q. roslaunchをPythonから叩きたいやつおるか？ｗ」

A. ここにおるでw

場面によっては使えるツールだと思います

例えばテキトーにwrapして…

import roslaunch
import os

class SimpleRosLaunch(object):
    def __init__(self, ):
        self.launchfile = ""
        self.reinitialize()

    def __del__(self,):
        self.stop()

    def load_file(self, launchfile):
        if self.is_alive():
            raise RuntimeError("you need to stop before you load another launch file")
        self.launchfile = os.path.abspath(os.path.expanduser(launchfile))
        self.launch.parent.config = roslaunch.config.load_config_default([self.launchfile], None)

    def start(self,):
        local_machine = roslaunch.core.local_machine()
        for n in self.launch.parent.config.nodes:
            n.machine = local_machine
        self.launch.start()

    def is_alive(self,):
        if self.launch.parent.pm is not None and self.launch.parent.pm.is_alive() and len(self.launch.parent.pm.get_active_names())>0:
            return True
        return False

    def current_file(self, ):
        return self.launchfile

    def reinitialize(self, ):
        self.launch = roslaunch.scriptapi.ROSLaunch()

    def stop(self,):
        self.launch.stop()
        self.reinitialize()
        self.load_file(self.launchfile)

こうしてやると…

import os
import roslaunch
import rospkg
from simple_roslaunch import SimpleRosLaunch

# 初期化
simple_roslaunch = SimpleRosLaunch()
# signalをいい感じに処理する
roslaunch.pmon._sig_initialized = True
rospy.on_shutdown(simple_roslaunch.stop)
launchfile = os.path.join(rospkg.RosPack().get_path('sample_pkg'), 'launch', 'sample.launch')

simple_roslaunch.load_file(launchfile)


# スタート
def start():
    if simple_roslaunch.is_alive():
        rospy.logerr("you've already executed launch file")
        return False
    simple_roslaunch.start()
    return True

# ストップ
def stop():
    if not simple_roslaunch.is_alive():
        rospy.logerr("no launch file is launched")
        return False
    simple_roslaunch.stop()
    return True

start()とstop()で起動したり止めたりできます

なにが嬉しいかというと、
例えばOccupancyGrid(/map)を保存したい時に、

<launch>
    <node pkg="map_server" type="map_saver" name="map_saver">
    </node>
</launch>

のようなlaunch fileを準備しておき、

from map_msgs.msg import SaveMap

def run_map_saver(self, req):
    global simple_roslaunch
    try:
        for n in simple_roslaunch.launch.parent.config.nodes:
            if (n.package == "map_server" and n.type == "map_saver"):
                # <node>のargsが変更できる
                n.args = unicode('-f %s'%(req.filename.data, ))
        # rosparamも変更できる
        # simple_roslaunch.launch.parent.config.params['/hoge/fuga/piyo'].value = '...'
        simple_roslaunch.start()
        while not rospy.is_shutdown():
            rospy.loginfo('saving map...')
            if not simple_roslaunch.is_alive():
                rospy.loginfo('map saved!')
                break
            rospy.sleep(0.2)
        simple_launch.stop()
    except Exception as e:
        rospy.logerr(e)

sample_service = rospy.Service('~sample_service', SaveMap, run_map_saver)

このようにlaunch fileに定義された引数などを動的に変更できます
mapに何か後処理を加えて保存したい場合もlaunch fileに後処理ノードを加えて、<remap>でmap_saverとtopicを繋げばよい、ということになります

当然ながらlaunch fileをわざわざ準備する必要もなくて

launch = roslaunch.scriptapi.ROSLaunch()
node = roslaunch.core.Node('map_server', 'map_saver')
node.args = unicode('-f %s'%(req.filename.data, ))
# namespaceも変更できる
# node.namespace = '/hogefuga'
launch.start()
launch.launch(node)
while not rospy.is_shutdown():
    rospy.loginfo("saving map...")
    if len(launch.parent.pm.get_active_names()) == 0:
        rospy.loginfo("map saved!")
        break
    rospy.sleep(0.2)
launch.stop()
del launch

その場でプロセスが起動できちゃう！プロセス管理はroslaunchにお任せ！（？）

まとめ

roslaunch APIは very unstableです
なるべく使わないようにしましょう