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 import rosgraph
34 import rosnode
35 import rospy
36 import rosservice
37
38
40
41 """Base exception class of rosconsole-related errors."""
42
43 pass
44
45
47
48 """
49 Handles service calls for getting lists of nodes and loggers.
50
51 Also handles sending requests to change logger levels.
52 """
53
56
58
59
60 return ['debug', 'info', 'warn', 'error', 'fatal']
61
63 self._refresh_loggers(node)
64 return self._current_loggers
65
67 """
68 Get a list of available services via a ros service call.
69
70 :returns: a list of all nodes that provide the set_logger_level service, ''list(str)''
71 """
72 set_logger_level_nodes = []
73 nodes = rosnode.get_node_names()
74 for name in sorted(nodes):
75 for service in rosservice.get_service_list(name):
76 if service == name + '/set_logger_level':
77 set_logger_level_nodes.append(name)
78 return set_logger_level_nodes
79
81 """
82 Store a list of loggers available for passed in node.
83
84 :param node: name of the node to query, ''str''
85 :raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
86 """
87 self._current_loggers = []
88 self._current_levels = {}
89
90 servicename = rosgraph.names.ns_join(
91 rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
92 'get_loggers')
93
94 servicename = rosgraph.names.resolve_name(
95 servicename, rosgraph.names.get_ros_namespace())
96 try:
97 service = rosservice.get_service_class_by_name(servicename)
98 except rosservice.ROSServiceException as e:
99 raise ROSConsoleException(
100 "node '%s' doesn't exist or doesn't support query: %s" % (node, e))
101
102 request = service._request_class()
103 proxy = rospy.ServiceProxy(str(servicename), service)
104 try:
105 response = proxy(request)
106 except rospy.ServiceException as e:
107 raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
108
109 if response._slot_types[0] == 'roscpp/Logger[]':
110 for logger in getattr(response, response.__slots__[0]):
111 self._current_loggers.append(logger.name)
112 self._current_levels[logger.name] = logger.level
113 else:
114 raise ROSConsoleException(repr(response))
115
117 """
118 Send a logger level change request to 'node'.
119
120 :param node: name of the node to chaange, ''str''
121 :param logger: name of the logger to change, ''str''
122 :param level: name of the level to change, ''str''
123 :returns: True if the response is valid, ''bool''
124 :returns: False if the request raises an exception or would not change the state, ''bool''
125 """
126
127 servicename = rosgraph.names.ns_join(
128 rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
129 'set_logger_level')
130
131 servicename = rosgraph.names.resolve_name(
132 servicename, rosgraph.names.get_ros_namespace())
133 if self._current_levels[logger] == level:
134 return False
135
136 service = rosservice.get_service_class_by_name(servicename)
137 request = service._request_class()
138 request.logger = logger
139 request.level = level
140 proxy = rospy.ServiceProxy(str(servicename), service)
141 try:
142 proxy(request)
143 self._current_levels[logger] = level.upper()
144 except rospy.ServiceException as e:
145 raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
146
147 return True
148