WindowsでROS(Python)を動作させてみる(その2)
Simple Wiki Based Contents Management System
関心分野 >> WindowsでROS(Python)を動作させてみる(その2)

WindowsでROS(Python)を動作させてみる(その2)

Windows上でmelodicを動作させようとした第2弾です。
最初のドキュメント(その1)は、こちらです。
前のドキュメントでは、roscoreを使わずに直接rosmasterを起動していました。これは、roscoreの場合には、Logのセッティングはもちろんですが、rosのバージョンの取得にrosversionというコマンド(Pythonスクリプトなのですが)を使っており、roslaunch経由でrosoutノードの自動実行もさせていたからです。
Pythonスクリプトのconsole_scriptであれば、なんとか設定できるのですが、rosoutのみroscppのアプリケーションでしたので、roscppを移植していない状況で簡単にはいきません。そこで、rosoutと同等の機能をPythonで実装することにしました。
すべての設定はこのページに添付していますが、下記に変更点を説明していきます。

rosversion,rosmasterを実行ファイルに

rosversionもrosmasterも実体はPythonスクリプトです。しかし、rosversionは、コンソールスクリプトになっていましたので、自力で実行ファイル形式(rosversion.exe, rosmaster.exe)にしていきます。
Pythonスクリプトの実行ファイル化には、setuptoolsが行っているentry_pointsを使ったコンソールスクリプト生成機能を使います。通常は、setuptoolsのsetup関数を使って entry_pointsで指定したスクリプトを実行ファイルとして動作させるものなのですが、ここでは手動で作成していきます。
実行ファイルは、Python27\Lib\site-packages\setuptoolsにあるcli-64.exe(ここでは64ビット版を使ったので)をコピーしてファイル名を変更すれば、同じディレクトリにある '-script.py'の内容に従ってPythonを起動して指定した関数を実行してくれます。
この機能を使って、rosversionとrosmasterを実行ファイル形式にします。
まずは、Python27\Lib\site-packages\setuptools\cli-64.exeを \opt\ros\melodic\binの下にrosversion.exe, rosmaster.exeとしてコピーします。
次に、rosversion-script.py, rosmaster-script.pyを同じディレクトリに作成していきます。
  • rosversion-script.py
rosversionは、Linux版のROSでもコマンドラインスクリプトになっていましたので、それとほぼ同じ内容で作成すれば大丈夫なはずですが、Windows用に少し付け加える必要もあります。Python27がC:\Python27にインストールされていると仮定すると、rosversion-script.pyは以下のようになります。
#!C:\Python27\python.exe
# EASY-INSTALL-ENTRY-SCRIPT: 'rospkg==1.1.7','console_scripts','rosversion'
__requires__ = 'rospkg==1.1.7'
import re
import sys
from pkg_resources import load_entry_point

if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
    sys.exit(
        load_entry_point('rospkg==1.1.7', 'console_scripts', 'rosversion')()
    )
このコードで1,2行目は削除してはいけません、また、1行目のpython.exeのパスがインストールした場所と同じかどうかも確認してください。
  • rosmaster-script.py
rosversionと同じようにrosmaster-script.pyを作成します。この時ちょっと問題があります。rosmasterは、そもそもconsole_scriptがなかったために、パッケージをインストロールされたときの rosmaster-1.14.3.egg-info がファイルになっています。entry_pointsを設定したい場合にが、.egg-infoがディレクトリ形式でなければいけないようですので、 rosmaster-1.14.3.egg-infoを一旦別名に変更して、 rosmaster-1.14.3.egg-infoというディレクトリを作成してください。元のrosmaster-1.14.3.egg-infoファイルは、rosmaster-1.14.3.egg-info\PKG-INFO となります。
このプロセスをまとめると下のようになります。
 > cd \opt\ros\melodic\lib\python2.7\dist-packages
 > ren rosmaster-1.14.3.egg-info rosmaster-1.14.3.egg-info.txt
 > mkdir rosmaster-1.14.3.egg-info
 > move rosmaster-1.14.3.egg-info.txt rosmaster-1.14.3.egg-info\PKG-INFO
この後に、dependency_links.txt, entry_points.txt, requires.txt, top_level.txtを作成します。dependency_links.txt, と top_level.txtは、空のファイルでOKです。requires.txt と entry_points.txtは、以下のようになります。
  • requires.txt
roslib
rospkg
  • entry_points.txt
[console_scripts]
rosmaster = rosmaster:rosmaster_main
最後に、rosmaster-script.pyを下のように作成して、 binの下に配置してください。
#!C:\Python27\python.exe
# EASY-INSTALL-ENTRY-SCRIPT: 'rosmaster==1.14.3','console_scripts','rosmaster'
__requires__ = 'rosmaster==1.14.3'
import re
import sys
from pkg_resources import load_entry_point

if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
    sys.exit(
        load_entry_point('rosmaster==1.14.3', 'console_scripts', 'rosmaster')()
    )
以上でrosversion.exe と rosmaster.exeの作成は終了です。
それぞれの実行ファイルが正常に動作するかコマンドプロンプトを生成して確認してください。

rosoutをPythonで実装

次に、rosoutを作成していきます。rosoutのソースコードは、C++バージョンですが、Githubに公開されています。
要は、このコードをPythonで書き直すだけです。
実装したrosout.pyを下に示します。このコードはあくまでもサンプルであり、ほぼオリジナルと同じだと思います。
from __future__ import print_function
import sys
import os
import rospy
from rosgraph_msgs.msg import Log

class Rosout(object):
    def __init__(self):
        self.log_file_name = os.path.join(getLogDirectory(), 'rosout.log')
        self.handle=None
        self.max_file_size=100*1024*1024
        self.current_file_size=0
        self.max_backup_index=0
        self.current_backup_index=0
        self.node=None
        self.rosout_sub=None
        self.agg_pub=None
        self.omit_topics=False

        self.initialize()
    #
    #
    def initialize(self):
        disable_file_logging_env=os.getenv('ROSOUT_DISABLE_FILE_LOGGING')
        disable_file_logging=""
        if disable_file_logging_env : disable_file_logging=disable_file_logging_env.lower()
        if disable_file_logging in ["", "0", "false", "off", "no"]:
            self.handle, self.current_file_size = open_log_file(self.log_file_name)
            if self.handle:
                print("logging to %s" % self.log_file_name)
                ss="\n\n"+str(rospy.Time.now())+" Node Startup\n"
                self.current_file_size += output_to_log(self.handle, ss)
                self.current_file_size += output_to_log(self.handle, str(sys.argv))

        self.agg_pub=rospy.Publisher('/rosout_agg', Log, queue_size=0)
        print("re-publishing aggregated message to /rosout_agg")
        self.rosout_sub=rospy.Subscriber('/rosout', Log, self.rosoutCallback)
        print("subscribed to /rosout")
    #
    #
    def rosoutCallback(self, msg):
        self.agg_pub.publish(msg)
        if not self.handle: return

        ss = str(msg.header.stamp) + " "
        if msg.level == Log.FATAL:
            ss += "FATAL "
        elif msg.level == Log.ERROR:
            ss += "ERROR "
        elif msg.level == Log.WARN:
            ss += "WARN "
        elif msg.level == Log.DEBUG:
            ss += "DEBUG "
        elif msg.level == Log.INFO:
            ss += "INFO "
        else:
            ss += str(msg.level) +" "

        ss += "%s [%s:%d(%s)]" % (msg.name, msg.file, msg.line, msg.function)
        if rospy.has_param('/rosout/omit_topics'):
            self.omit_topics = rospy.getParam('/rosout/omit_topics')        

        if not self.omit_topics:
            ss += "[topics: %s] %s\n" % (",".join(msg.topics), msg.msg)

        self.current_file_size += output_to_log(self.handle, ss)

        # File rotation
        if self.current_file_size > self.max_file_size:
            self.handle.close()
            self.current_backup_index = log_file_rotation(self.log_file_name, self.max_backup_index, self.current_backup_index)
            self.handle, self.current_file_size = open_log_file(self.log_file_name)
#
#
def log_file_rotation(log_file_name, max_index, current_index):
    print("rosout log file %s reached max size, rotating log files" % log_file_name)
    if current_index == max_index:
        backup_file_name = log_file_name+"."+str(max_index)
        os.remove(backup_file_name)

    i=min(max_index, current_index + 1)
    while i > 0:
        current_file_name = log_file_name
        if i > 1: current_file_name = current_file_name+"."+ str(i-1)

        rotated_file_name = log_file_name+"."+str(i)
        if os.path.exists(rotated_file_name): os.remove(rotated_file_name)
        os.rename(current_file_name, rotated_file_name)
        i = i-1

    if current_index < max_index:
        current_index += 1
    
    return current_index
#
#
def open_log_file(log_file_name):
    try:
        handle = open(log_file_name, "w")
    except:
        print("Error opening rosout log file '%s'" % log_file_name)
        handle=None
    return (handle, 0)
#
#
def output_to_log(handle, msg):
    try:
        handle.write(msg)
        handle.flush()
        size = len(msg)
    except:
        print("== ERROR in output to log ==")
        size=0
    return size
#
#
def getLogDirectory():
    logopt=get_option_val(sys.argv, '__log:=')
    ros_log_dir = os.getenv('ROS_LOG_DIR')
    ros_home = os.getenv('ROS_LOG_DIR')
    if logopt:
        res=os.path.dirname(logopt[7:])
    elif ros_log_dir:
        ret=ros_log_dir
    elif ros_home:
        res=os.path.join(ros_home, 'log')
    else:
        res=""
    return res

def get_option_val(args, pattern):
    for x in args:
        if x.startswith(pattern):
            return x
    return None
#
#
def main():
    rospy.init_node('rosout')
    rosout=Rosout()
    rospy.spin()

if __name__=='__main__':
    main()

このファイルは、roslunchで呼び出される場合には、rosoutのパッケージディレクトリに配置されている必要があります。従って、このファイルを、 \opt\ros\melodic\share\rosout\rosout.py として配置すればOKです。

roslauchを修正

下の修正は、現在Githubで公開されているソースコードには、修正が加えられており、下の修正は必要ありません。
最後に、roslaunchの修正を行います。ROSのPythonモジュールは、よく見るとWindowsでも動作するように改変された部分が多くみられます。私が動作確認したところ、roslaunchでsubuprocess,Popenでコマンドを実行させるときに、Windowsではサポートされていないオプションが設定されていました。
修正するファイルは、roslaunch\nodeprocess.pyの551行目の部分です。
  • 修正前
            try:
                self.popen = subprocess.Popen(self.args, cwd=cwd, stdout=logfileout, stderr=logfileerr, env=full_env, close_fds=True, preexec_fn=os.setsid)
            except OSError as e:
  • 修正後
            try:
                if os.name == 'nt':
                    self.popen = subprocess.Popen(self.args, cwd=cwd, stdout=logfileout, stderr=logfileerr, env=full_env)
                else:
                    self.popen = subprocess.Popen(self.args, cwd=cwd, stdout=logfileout, stderr=logfileerr, env=full_env, close_fds=True, preexec_fn=os.setsid)
            except OSError as e:
以上で、roslaunchの修正は終了です。

動作確認

上記の修正が正常に動作するかの動作確認を行ってください。
動作させるものは、その1でも使ったtalker.pyとlistener.pyでよいと思います。rosoutも動作すると思いますので、print文をrospy.loginfoに戻してください。
 > \opt\ros\melodic\setup.bat
 > start roscore
 > start python talker.py
 > python listener.py
最終的な差分をこのページに添付しておきます。この差分は、D:\Python27 に 64ビット版のPythonがインストールされていることを前提にしていますので、CドライブにPythonをインストールされている場合には、rosversion-script.pyとrosmaster-script.pyを修正する必要があります。また、32ビット版の場合には、 rosversion.exe と rosmaster.exeも cli-32.exeに置き換える必要があると思います。
また、ファイルを上書きする場合には、lib/python2.7/dist-packages/rosmaster-1.14.3.egg-info を予め削除しておく必要がありますので注意してください。
以上の修正で、TalkerとListenerは動作しますが、%%rqt_graphなどが動作していません。次は、rqt_graphを動作させてみます。%%rqt_graphは、graphvizをインストールすれば、アイコン等が表示されませんが動作します。
これまでの修正では、C++のノードが動作していませんでしたので(その3)では、Githubに公開されているソースコードからのビルドについて説明します。

資料