Ros

roscore/roslaunch源码分析

为更直观,叙述时会涉及到一些路径,像roslaunch功能包路径,对不同操作系统、不同安装方式,即使同是rosluanch功能包也会是不同路径。这里示例环境是ubuntu下以源码方式安装ros,catkin工作空间路径:~/ros_catkin_ws。

  • roscore主要行为:1)启动一个绑定在随机端口的XML-RPC server。2)启动写在roscore.xml中的node,这node一般就是rosout。3)启动绑定在11311端口的master-node XML-RPC server。
  • roscore行为是roslaunch的子集。roslaunch比roscore多了第四步,解析并运行第三个参数指定的*.xml/launch。运行*.xml/launch包括向master-node写setParam,以及启动内中node。
  • 会启动两个XML-RPC server。master-node XML-RPC server固定绑定在11311端口,全ros环境只一个,由第一次运行的roscore或rosluanch创建。第二个XML-PRC server绑定在随机端口,每次roscore或roslaunch都会有一个。
  • 运行XML-RPC server不是在新进程,而是新线程。运行node则是在新进程。
  • 整个ros环境最多只允许运行一个master-node XML-RPC server,roscore和launch都能启动这个server。但若执行roscore,当前不能有master-node server,有会报错,roslaunch则没有这限制。这也就是为什么若要执行roscore,它必须在roslaunch之前执行。
  • 总是“一次性”启动master-node XML-RPC server和写在roscore.xml中的那些个node。也就是说,如果不是此次启动master-node XML-RPC server,那此次就不会启动写在roscore.xml中的那些个node。于是对这么个序列,1)执行roscore,2)执行roslaunch。roslaunch虽然也解析roscore.xml到config,但它不会启动当中的node。
  • 由于_launch_core_nodes()在_load_parameters()之前执行,roscore.xml中写的node其实用不了写在roscore.xml中的param。

 

一、入口

~/ros_catkin_ws/install_isolated/bin/roscore
------
if len(args) > 0:
    parser.error("roscore does not take arguments")

import roslaunch
roslaunch.main(['roscore', '--core'] + sys.argv[1:])

~/ros_catkin_ws/install_isolated/bin/roslaunch
------
import roslaunch
roslaunch.main()

roscore命令的位置在~/ros_catkin_ws/install_isolated/bin/roscore。roscore首先会调用roslaunch(跟roscore相同文件夹),由roslaunch来调用roslaunch功能包下的其他文件,roslaunch功能包的位置与roslaunch文件位置不同,但是sys.path中的一个目录,具体是~/ros_catkin_ws/install_isolated/lib/python3/dist-packages/roslaunch。

roslaunch命令的位置在~/ros_catkin_ws/install_isolated/bin/roslaunch,也是个python。除参数不同外,它的逻辑和roscore一样,执行roslaunch.main()。从这里也可以看出,roscore、roslaunch主体逻辑是一样,不同的只是内中一些细节。

 

二、roslaunch功能包

roslaunch首先调用的是_init_.py的main(),初始化各种参数和环境变量,如设置roslaunch的日志文件位置。然后检查已存在的日志文件大小,若已超过1GB则会提醒用户执行rosclean purge命令。

roslaunch日志文件在~/.ros/log下,每次执行roscore都会生成唯一的uuid,以uuid号为日志文件夹名,该文件夹下的roslaunch-<机器号-pid>.log便是roslaunch的日志。日志文件名示例:roslaunch-DESKTOP-DF7GFDR-1724.log、roslaunch-valian-TM1703-32434.log。

_init_.py各种参数配置好后就到了p.start()。

<ros_catkin_ws>/src/ros_comm/roslaunch/src/roslaunch/__init__.py
------
def main(argv=sys.argv):
    if options.child_name:
       # roscore/roslaunch都不会进这个入口
    else:
        p = roslaunch_parent.ROSLaunchParent(uuid, args, roslaunch_strs=roslaunch_strs,
                is_core=options.core, port=options.port, local_only=options.local_only,
                verbose=options.verbose, force_screen=options.force_screen,
                force_log=options.force_log,
                num_workers=options.num_workers, timeout=options.timeout,
                master_logger_level=options.master_logger_level,
                show_summary=not options.no_summary,
                force_required=options.force_required,
                sigint_timeout=options.sigint_timeout,
                sigterm_timeout=options.sigterm_timeout)
        p.start()
        p.spin()

构造ROSLaunchParent时刻的args值。

roscore时
------
[]

roslaunch时
------
['/home/valian/ros_catkin_ws_noetic/install_isolated/share/roscpp_tutorials/launch/talker_listener.launch']

args会直接赋值给ROSLaunchParent::roslaunch_files,正如变量名所示,它指示了在_load_config()时要加载的launch文件。正是有了args,在_load_config()时,roscore只会加载roscore.xml,roslaunch则会同时加载roscore.xml和roslaunch_files中的launch文件。

三、p.start()

p为ROSLaunchParent类,定义是在roslaunch文件夹下的parent.py。ROSLaunchParent成员函数start负责启动roslaunch,自然包括其下各自服务的启动:

class ROSLaunchParent(object):
    def start(self, auto_terminate=True):
        # 加载配置,启动进程监视器(pm)和启动XML-RPC server
        self._start_infrastructure()

        # 创建runner,赋值给成员self.runner 
        # 创建runner需要提供有效的config、pm、XML-RPC server和remote_runner
        self._init_runner()

        # Start the launch
        self.runner.launch()

        # inform process monitor that we are done with process registration
        if auto_terminate:
            self.pm.registrations_complete()
        
        self.logger.info("... roslaunch parent running, waiting for process exit")
        if self.process_listeners:
            for l in self.process_listeners:
                self.runner.pm.add_process_listener(l)
                # Add listeners to server as well, otherwise they won't be
                # called when a node on a remote machine dies.
                self.server.add_process_listener(l)

接下依次分析ROSLaunchParent::start的各项操作。

3.1、 _start_infrastructure():加载配置,启动进程监视器(pm)和启动XML-RPC server

<ros_catkin_ws>/src/ros_comm/roslaunch/src/roslaunch/parent.py
---
class ROSLaunchParent(object):
    def _start_infrastructure(self):
        """
        load config, start XMLRPC servers and process monitor
        """
        if self.config is None:
            self._load_config()

        # Start the process monitor
        if self.pm is None:
            self._start_pm()

        # Startup the roslaunch runner and XMLRPC server.
        # Requires pm
        if self.server is None:
            self._start_server()

        # Startup the remote infrastructure.
        # Requires config, pm, and server
        self._start_remote()

_start_infrastructure()首先调用_load_config(),功能是读取若干*.xm/launch,解析成配置,存放在ROSLaunchParent::config。具体要解析哪些*.xml/launch?——roscore、roslaunch都会调用ROSLaunchConfig::load_config_default,它依次会解析三种xml文件。

  1. roscore.xml。具体是/home/valian/ros_catkin_ws_noetic/install_isolated/etc/ros/roscore.xml。
  2. ROSLaunchParent::roslaunch_files中的那些个*.launch。“二、roslaunch功能包”已说过,roscore时,这部分是空,roslaunch则是要启动的*.launch文件。
  3. ROSLaunchParent::roslaunch_strs中那些个xml语句。roscore、roslaunch都可认为是空。

也就是说,roscore只会解析roscore.xml,roslaunch则同时会解析roscore.xml、以及*.launch。文件解析后配置放在ROSLaunchParent::config。解析过程只是把*.xml/launch文件变成config,不会执行当中操作,像遇到<param />不会执行向master-node发set param请求。

ROSLaunchParent::config变量的类型是ROSLaunchConfig,以下是它的几个成员变量。

变量类型注释
nodes_core [Node]roscore.xml中的node。<roslaunch>/src/roslaunch/core.py定义了class Node(object)
nodes[Node]roslaunch指定的*.launch中的node
params.values[key-->val]映射要set param的param。 val已是处理过的值。举个例子:<param name="rosversion" command="rosversion roslaunch" />,val值已是执行rosversion roslaunch后的值:1.15.11

生成ROSLaunchParent::config后,调用_start_pm()来启动进程监视器,进程监视器的代码部分在pemon.py实现。

接着是_start_server(),该函数创建并运行一个XML-RPC server,关于XML-RPC server见后面“五、wmlrpc.py”。这里注意两点,一是该server给定的端口参数是0,也就是将由wmlrpc.py自主决定绑定哪个端口。二是_start_server()是阻塞式调用,即wmlrpc.py执行到self.server.serve_forever(),确认XMLRPC-server服务器已运行,才会返回。

最后是_start_remote(),用于初始化并运行远程进程运行器,若不存在远程运行器,则不执行相关操作。

 

3.2、_init_runner():创建roslaunch运行器

_init_runner()创建在launch.py的类ROSLaunchRunner,再调用config.py的summary()将roslaunch的相关启动信息输出给用户。

def _init_runner(self):
        // 创建roslaunch runner
        self.runner = roslaunch.launch.ROSLaunchRunner(self.run_id, self.config,
            server_uri=self.server.uri, pmon=self.pm, 
            is_core=self.is_core, remote_runner=self.remote_runner, 
            is_rostest=self.is_rostest, num_workers=self.num_workers, 
            timeout=self.timeout, master_logger_level=self.master_logger_level,
            sigint_timeout=self.sigint_timeout, sigterm_timeout=self.sigterm_timeout)
        if self.show_summary:    
            print(self.config.summary(local=self.remote_runner is None))

3.3、self.runner.launch()

launch()调用_setup()来设置ROS网络状态,包括参数服务器状态和核心服务(master-node XML-PRC server + rosout),_launch_nodes()来启动roslaunch声明的节点。

class ROSLaunchRunner(object):
------
    def launch(self):
        """
        Run the launch. Depending on usage, caller should call
        spin_once or spin as appropriate after launch().
        @return ([str], [str]): tuple containing list of nodes that
            successfully launches and list of nodes that failed to launch
        @rtype: ([str], [str])
        @raise RLException: if launch fails (e.g. run_id parameter does
        not match ID on parameter server)
        """
        self._setup()        
        succeeded, failed = self._launch_nodes()
        return succeeded, failed

 

3.3.1、self._setup()

class ROSLaunchRunner(object):
    def _setup(self):
        """
        Setup the state of the ROS network, including the parameter
        server state and core services
        """
        # this may have already been done, but do just in case
        self.config.assign_machines()
        
        if self.remote_runner:
            # hook in our listener aggregator
            self.remote_runner.add_process_listener(self.listeners)            

        # start up the core: master + core nodes defined in core.xml
        launched = self._launch_master()
        # 如果是此次启动了master-node XML-RPC server,则此处的返回值launched是true。
        if launched:
            self._launch_core_nodes()
        
        # run executables marked as setup period. this will block
        # until these executables exit. setup executable have to run
        # *before* parameters are uploaded so that commands like
        # rosparam delete can execute.
        self._launch_setup_executables()

        # no parameters for a child process
        if not self.is_child:
            self._load_parameters()

self._launch_master()首先会调用config.Master::is_running(),判断当前是否已经有一个master-node XML-RPC server在运行,没有则启动,否则不必启动了。对要启动master-node XML-RPC server,它会去执行~/ros_catkin_ws_noetic/install_isolated/bin/rosmaster

~/ros_catkin_ws_noetic/install_isolated/bin/rosmaster
------
import rosmaster
rosmaster.rosmaster_main()

rosmater是个python文件,它执行rosmaster功能包中的rosmaster.rosmaster_main()。rosmaster功能包在~/ros_catkin_ws/install_isolated/lib/python3/dist-packages/rosmaster。这里不具体展开rosmaster功能包了,只说下结论:它新建并运行第二个XML-RPC server,关于XML-RPC server见后面“五、wmlrpc.py”。要注意的是,该server必须绑定在11311端口,对应的handler是rosmaster.master_api.ROSMasterHandler。

如果的确是此次启动了master-node XML-RPC server,_launch_master()返回值launched是true,是true时会调用_launch_core_nodes()。

3.3.2 self._launch_core_nodes()

_launch_core_nodes()负责启动写在roscore.xml中的那些个node。

<ros_catkin_ws>/src/ros_comm/roslaunch/resources/roscore.xml
------
<launch>
  <group ns="/">
    <param name="rosversion" command="rosversion roslaunch" />
    <param name="rosdistro" command="rosversion -d" />
    <node pkg="rosout" type="rosout" name="rosout" respawn="true"/>
  </group>
</launch>

以上是roscore.xml内容,在_launch_core_nodes会启动rosout进程。

3.3.3 self._load_parameters()

_load_parameters()负责把config.params中的参数写(setParam)到master-node。参数就是*.xml/launch中以<param />标签修饰的param。像roscore.xml中的“rosversion”、“rosdistro”。注意,这里要写的参数除了roscore.xml的,还有ROSLaunchParent::roslaunch_files中的参数。_load_config()在解析xml/launch时,不会像node一样区分是否来自roscore.xml,它们混合放在了config.params。

和node不一样,即使不是此次启动master-node XML-RPC server,roscore.xml中的param还是会写到master-node。

由于_launch_core_nodes()在_load_parameters()之前执行,roscore.xml中写的node其实用不了写在roscore.xml中的param。

3.3.4 self._launch_nodes()

_launch_nodes()负责启动写在ROSLaunchParent::roslaunch_files中的那些个node。也就是说,roslaunch指定要启动*.launch是在_launch_nodes()启动的。

3.4、self.pm.registrations_complete()

pmon.py的registrations_complete()设置registrations_complete标志后,如果没有其他进程需要监控,进程监视器将退出。

到这一步p.start()执行完毕,已经将各种参数初始化,完成好了准备工作。回到_init_.py,下面开始运行p.spin。

 

四、p.spin()

_init_.py的p.spin()调用的是parent.py的spin(),运行ROSLaunch直到程序退出。

class ROSLaunchParent(object):
    def spin(self):
        """
        Run the parent roslaunch until exit
        """
        if not self.runner:
            raise RLException("parent not started yet")
        # Blocks until all processes dead/shutdown
            self.runner.spin()

class ROSLaunchRunner(object):
    def spin(self):
        """
        spin() must be run from the main thread. spin() is very
        important for roslaunch as it picks up jobs that the process
        monitor need to be run in the main thread.
        """
        self.logger.info("spin")

        # #556: if we're just setting parameters and aren't launching
        # any processes, exit.
        if not self.pm or not self.pm.get_active_names():
            printlog_bold("No processes to monitor")
            self.stop()
            return # no processes
        self.pm.mainthread_spin()
        # 要没意外,roscore/roslaunch进程阻塞在这条mainthread_spin()语句。
        # CTRL+C后,开始继续执行下面语句
        #self.pm.join()
        self.logger.info("process monitor is done spinning, initiating full shutdown")
        self.stop()
        printlog_bold("done")

 

五、wmlrpc.py

wmlrpc.py实现了一个用python写的XML-RPC server。该server用了python内置的SimpleXMLRPCServer技术,会一直运行在一个新建线程。通过register_instance,client发到该server的请求会交由handler的方法去处理。

<ros_catkin_ws>/src/ros_comm/rosmaster/src/rosmaster/master.py
-----
import rosgraph.xmlrpc
handler = rosmaster.master_api.ROSMasterHandler(self.num_workers)
master_node = rosgraph.xmlrpc.XmlRpcNode(self.port, handler)
master_node.start()

以上4条语句的功能是创建并运行master-node server,也展示了如何使用wmlrpc。总的来说分两步,一是构造XmlRpcNode,二是start。

5.1 构造XmlRpcNode

class XmlRpcNode(object):
    def __init__(self, port=0, rpc_handler=None, on_run_error=None):
        """
        XML RPC Node constructor
        :param port(int): server要绑定的端口,如果0,则让自动选绑定到端口
        :param rpc_handler(XmlRpcHandler): 当server收到client发来的请求时,要调用该handler中的方法
        """
        super(XmlRpcNode, self).__init__()

        self.handler = rpc_handler
        self.uri = None # initialize the property now so it can be tested against, will be filled in later
        self.server = None
        if port and isstring(port):
            port = int(port)
        self.port = port
        self.is_shutdown = False
        self.on_run_error = on_run_error

构造时的操作基本就是初始化成员变量。构造XmlRpcNode须三个参数,但roscore、roslaunch都只使用两个,一是端口,二是handler。参数XmlRpcNode是抽像类,因而具体创建时必须提供派类生类,roscore是master_api.ROSMasterHandler,roslauncher是??。

XmlRpcNode,roslaunch是直接用它定义的node,有时会用它的派生类,像ROSLaunchParent::_start_server时用ROSLaunchParentNode。

5.2 start()

start流程

  1. 新建线程,server将运行在该线程,线程主函数是run()。
  2. (run内)_run_init创建服务器,赋值给self.server,实现上用了python内置的SimpleXMLRPCServer技术。
  3. (run内)执行server.server_forver(),直到关掉server
class XmlRpcNode(object):
    def start(self):
        // 新建线程,server将运行在该线程,线程主函数是run()
        _thread.start_new_thread(self.run, ())

    def run(self):
        // 为什么没把_run作为线程主函数?run也是XmlRpcNode向外暴露的接口。
        self._run()

    def _run(self):
        // 运行server分两步:
        // 1: _run_init创建服务器,赋值给self.server
        // 2: 执行server.server_forver(),直到关掉server
        self._run_init()
        while not self.is_shutdown:
            self.server.serve_forever()

    def _run_init(self):
        // _run_init功能:创建服务器,赋值给self.server
        logger = logging.getLogger('xmlrpc')            
        try:
            log_requests = 0
            port = self.port or 0 #0 = any

            bind_address = rosgraph.network.get_bind_address()
            logger.info("XML-RPC server binding to %s:%d" % (bind_address, port))
            // ThreadingXMLRPCServer封装server,该类也在wmlrpc.py内定义。
            // 查看ThreadingXMLRPCServer会发现,在实现了,它用了python内置的SimpleXMLRPCServer技术。
            self.server = ThreadingXMLRPCServer((bind_address, port), log_requests)
            self.port = self.server.server_address[1] #set the port to whatever server bound to
            if not self.port:
                self.port = self.server.socket.getsockname()[1] #Python 2.4

            assert self.port, "Unable to retrieve local address binding"

            ......

            self.server.register_multicall_functions()
            // client发来的请求会由hander的方法去处理  
            self.server.register_instance(self.handler)

 

六、ROSLaunchRunner::launch_node

不论是启动roscore.xml中node,还是roslaunch指定*.launch中node,都要调用ROSLaunchRunner::launch_node。launch_node功能是启动参数指定的某个node。启动node一定会新建进程,在那进程运行node。

<ros_catkin_ws>/src/ros_comm/roslaunch/src/roslaunch/launch.py
------
class ROSLaunchRunner(object):
    def launch_node(self, node, core=False):
        """
        Launch a single node locally. Remote launching is handled separately by the remote module.
        If node name is not assigned, one will be created for it.
        
        @param node(Node): 要启动的node
        @param core(bool): 是否是roscore.xml中的node。true表示roscore.xml中node
        @return (obj, bool): Process handle, successful launch. If success, return actual Process instance. Otherwise return name.
        """
        self.logger.info("... preparing to launch node of type [%s/%s]", node.package, node.type)
        
        # TODO: should this always override, per spec?. I added this
        # so that this api can be called w/o having to go through an
        # extra assign machines step.
        if node.machine is None:
            node.machine = self.config.machines['']
        if node.name is None:
            node.name = rosgraph.names.anonymous_name(node.type)
            
        master = self.config.master
        import roslaunch.node_args
        try:
            process = create_node_process(self.run_id, node, master.uri, sigint_timeout=self.sigint_timeout, sigterm_timeout=self.sigterm_timeout)
        except roslaunch.node_args.NodeParamsException as e:
            self.logger.error(e)
            printerrlog("ERROR: cannot launch node of type [%s/%s]: %s"%(node.package, node.type, str(e)))
            if node.name:
                return node.name, False
            else:
                return "%s/%s"%(node.package,node.type), False                

        self.logger.info("... created process [%s]", process.name)
        if core:
            self.pm.register_core_proc(process)
        else:
            self.pm.register(process)            
        node.process_name = process.name #store this in the node object for easy reference
        self.logger.info("... registered process [%s]", process.name)

        # note: this may raise FatalProcessLaunch, which aborts the entire launch
        success = process.start()
        if not success:
            if node.machine.name:
                printerrlog("launch of %s/%s on %s failed"%(node.package, node.type, node.machine.name))
            else:
                printerrlog("local launch of %s/%s failed"%(node.package, node.type))   
        else:
            self.logger.info("... successfully launched [%s]", process.name)
        return process, success

启动node分两步。第一步是调用create_node_process,用它创建一个LocalProcess对象。第二步是调用LocalProcess的start方法启动新进程。启动进程时,用了python提供的subprocess技术。

<ros_catkin_ws>/src/ros_comm/roslaunch/src/roslaunch/nodeprocess.py
------
class LocalProcess(Process):
    def start(self):
        # 启动进程
        super(LocalProcess, self).start()
        try:
            # subprocess是python内置提供的一种启动新进程技术,只需一个Popen方法。
            self.popen = subprocess.Popen(self.args, cwd=cwd, 
                stdout=logfileout, stderr=logfileerr, env=full_env,
                close_fds=close_file_descriptor, preexec_fn=preexec_function)

            self.started = True
            # Check that the process is either still running (poll returns
            # None) or that it completed successfully since when we
            # launched it above (poll returns the return code, 0).
            poll_result = self.popen.poll()
            if poll_result is None or poll_result == 0:
                self.pid = self.popen.pid
                printlog_bold("process[%s]: started with pid [%s]"%(self.name, self.pid))
                return True
            else:
                printerrlog("failed to start local process: %s"%(' '.join(self.args)))
                return False

全部评论: 0

    写评论: