1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34 """
35 Internal use: rospy initialization.
36
37 This is mainly routines for initializing the master or slave based on
38 the OS environment.
39 """
40
41 import os
42 import sys
43 import logging
44 import time
45 import traceback
46
47 import rosgraph
48 import rosgraph.xmlrpc
49
50 from ..names import _set_caller_id
51 from ..core import is_shutdown, signal_shutdown, rospyerr
52 from ..rostime import is_wallclock, get_time
53
54 from .tcpros import init_tcpros
55 from .masterslave import ROSHandler
56
57 DEFAULT_NODE_PORT = 0
58 DEFAULT_MASTER_PORT=11311
59 DEFAULT_MASTER_URI = 'http://localhost:%s/'%DEFAULT_MASTER_PORT
60
61
62
63
65 """
66 If XML-RPC errors out of the run() method, this handler is invoked
67 """
68 rospyerr(traceback.format_exc())
69 signal_shutdown('error in XML-RPC server: %s'%(e))
70
71 -def start_node(environ, resolved_name, master_uri=None, port=None):
72 """
73 Load ROS slave node, initialize from environment variables
74 @param environ: environment variables
75 @type environ: dict
76 @param resolved_name: resolved node name
77 @type resolved_name: str
78 @param master_uri: override ROS_MASTER_URI: XMlRPC URI of central ROS server
79 @type master_uri: str
80 @param port: override ROS_PORT: port of slave xml-rpc node
81 @type port: int
82 @return: node server instance
83 @rtype rosgraph.xmlrpc.XmlRpcNode
84 @raise ROSInitException: if node has already been started
85 """
86 init_tcpros()
87 if not master_uri:
88 master_uri = rosgraph.get_master_uri()
89 if not master_uri:
90 master_uri = DEFAULT_MASTER_URI
91
92
93 _set_caller_id(resolved_name)
94
95 handler = ROSHandler(resolved_name, master_uri)
96 node = rosgraph.xmlrpc.XmlRpcNode(port, handler, on_run_error=_node_run_error)
97 node.start()
98 while not node.uri and not is_shutdown():
99 time.sleep(0.00001)
100 logging.getLogger("rospy.init").info("ROS Slave URI: [%s]", node.uri)
101
102 while not handler._is_registered() and not is_shutdown():
103 time.sleep(0.1)
104 logging.getLogger("rospy.init").info("registered with master")
105 return node
106
107 _logging_to_rospy_names = {
108 'DEBUG': 'DEBUG',
109 'INFO': 'INFO',
110 'WARNING': 'WARN',
111 'ERROR': 'ERROR',
112 'CRITICAL': 'FATAL',
113 }
114
116 - def emit(self, record):
117
118 level = _logging_to_rospy_names[record.levelname]
119 if is_wallclock():
120 msg = "[%s] [WallTime: %f] %s\n"%(level, time.time(),
121 record.getMessage())
122 else:
123 msg = "[%s] [WallTime: %f] [%f] %s\n"%(level, time.time(), get_time(),
124 record.getMessage())
125 if record.levelno < logging.WARNING:
126 sys.stdout.write(msg)
127 else:
128 sys.stderr.write(msg)
129
130 _loggers_initialized = False
137