44# you may not use this file except in compliance with the License.
55# You may obtain a copy of the License at
66#
7- # http://www.apache.org/licenses/LICENSE-2.0
7+ # http://www.apache.org/licenses/LICENSE-2.0
88#
99# Unless required by applicable law or agreed to in writing, software
1010# distributed under the License is distributed on an "AS IS" BASIS,
1111# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1212# See the License for the specific language governing permissions and
1313# limitations under the License.
1414
15-
16- from rcl_interfaces .srv import ListParameters
1715import rclpy
16+ from rclpy .parameter import parameter_value_to_python
17+ from ros2param .api import call_get_parameters
18+ from ros2param .api import call_list_parameters
19+
1820from ros2cli .node .direct import DirectNode
1921from ros2cli .node .strategy import NodeStrategy
2022from ros2doctor .api import DoctorCheck
2426from ros2doctor .api .format import doctor_warn
2527
2628
27- def call_list_parameters (node , node_name , namespace = '/' ):
28- """Call the list_parameters service for a specific node."""
29- try :
30- # Create service name and client for the target node's list_parameters service
31- service_name = f"{ namespace .rstrip ('/' )} /{ node_name } /list_parameters"
32- if service_name .startswith ('//' ):
33- service_name = service_name [1 :]
34-
35- client = node .create_client (ListParameters , service_name )
36-
37- if not client .wait_for_service (timeout_sec = 1.0 ):
38- return None
39-
40- request = ListParameters .Request ()
41- future = client .call_async (request )
42-
43- # Spin until the service call completes or times out
44- rclpy .spin_until_future_complete (node , future , timeout_sec = 2.0 )
45-
46- if future .done ():
47- response = future .result ()
48- node .destroy_client (client )
49- return response
50- else :
51- node .destroy_client (client )
52- return None
53-
54- except Exception :
55- return None
56-
57-
5829class ParameterCheck (DoctorCheck ):
59- """Check for nodes without parameter services ."""
30+ """Check for nodes with parameters using sim_time presence ."""
6031
6132 def category (self ):
6233 return 'parameter'
6334
6435 def check (self ):
6536 result = Result ()
37+ use_sim_time_values = {}
38+
6639 with NodeStrategy (None ) as node :
6740 try :
68- node_names_and_namespaces = node .get_node_names_and_namespaces ()
41+ node_names_and_namespaces = \
42+ node .get_node_names_and_namespaces ()
6943 except Exception :
7044 node_names_and_namespaces = []
45+
7146 with DirectNode (None ) as param_node :
7247 for node_name , namespace in node_names_and_namespaces :
73- response = call_list_parameters (param_node .node , node_name , namespace )
74- if response is None :
75- full_name = f"{ namespace .rstrip ('/' )} /{ node_name } "
76- doctor_warn (f'Node { full_name } has no parameter services.' )
77- result .add_warning ()
48+ full_name = f"{ namespace .rstrip ('/' )} /{ node_name } "
49+ try :
50+ response = call_list_parameters (
51+ node = param_node .node , node_name = full_name )
52+ if response is None :
53+ """ Service timed out, skip this node and continue """
54+ continue
55+ elif response .result () is None :
56+ """call failed, skip this node"""
57+ continue
58+
59+ param_names = response .result ().result .names
60+ if 'use_sim_time' in param_names :
61+ sim_time_return = call_get_parameters (
62+ node = param_node .node ,
63+ node_name = full_name ,
64+ parameter_names = ['use_sim_time' ])
65+ if sim_time_return and sim_time_return .values :
66+ sim_time_value = parameter_value_to_python (
67+ sim_time_return .values [0 ])
68+ use_sim_time_values [full_name ] = sim_time_value
69+ except RuntimeError :
70+ """ Node configured to make Parameter service unavailable ,expected behavior """
71+ pass
72+
73+ """ Warnings on presence of nodes with real and sim time for debugging"""
74+ if use_sim_time_values :
75+ values = set (use_sim_time_values .values ())
76+ if len (values ) > 1 :
77+ nodes_with_true = [
78+ name for name , val in use_sim_time_values .items () if val
79+ ]
80+ nodes_with_false = [
81+ name for name , val in use_sim_time_values .items () if not val
82+ ]
83+ if nodes_with_true and nodes_with_false :
84+ doctor_warn ('Inconsistent use_sim_time parameter detected:' )
85+ doctor_warn (
86+ ' Nodes with use_sim_time=True: '
87+ f'{ ", " .join (nodes_with_true )} '
88+ )
89+ doctor_warn (
90+ ' Nodes with use_sim_time=False: '
91+ f'{ ", " .join (nodes_with_false )} '
92+ )
93+ result .add_warning ()
94+
7895 return result
7996
8097
@@ -88,7 +105,8 @@ def report(self):
88105 report = Report ('PARAMETER LIST' )
89106 with NodeStrategy (None ) as node :
90107 try :
91- node_names_and_namespaces = node .get_node_names_and_namespaces ()
108+ node_names_and_namespaces = \
109+ node .get_node_names_and_namespaces ()
92110 except Exception :
93111 node_names_and_namespaces = []
94112 if not node_names_and_namespaces :
@@ -97,24 +115,59 @@ def report(self):
97115 report .add_to_report ('parameter' , 'none' )
98116 return report
99117
100- total_param_count = 0
118+ param_count = 0
101119 nodes_checked = 0
102120
103121 with DirectNode (None ) as param_node :
104122 for node_name , namespace in sorted (node_names_and_namespaces ):
105123 nodes_checked += 1
106- response = call_list_parameters (param_node .node , node_name , namespace )
107- if response and hasattr (response , 'result' ) and response .result :
108- result = response .result
109- param_names = result .names if hasattr (result , 'names' ) else []
124+ full_name = f"{ namespace .rstrip ('/' )} /{ node_name } "
125+ try :
126+ response = call_list_parameters (
127+ node = param_node .node , node_name = full_name )
128+ if response is None :
129+ continue
130+ elif response .result () is None :
131+ continue
132+
133+ param_names = response .result ().result .names
110134 if param_names :
111- total_param_count += len (param_names )
112- full_name = f"{ namespace .rstrip ('/' )} /{ node_name } "
113-
135+ param_count += len (param_names )
114136 report .add_to_report ('node' , full_name )
115- for param_name in sorted (param_names ):
116- report .add_to_report ('parameter' , param_name )
137+ try :
138+ param_response = call_get_parameters (
139+ node = param_node .node ,
140+ node_name = full_name ,
141+ parameter_names = param_names
142+ )
143+ param_values = None
144+ if param_response :
145+ param_values = param_response .values
146+ if param_values and len (param_values ) == len (
147+ param_names
148+ ):
149+ params_with_values = sorted (
150+ zip (param_names , param_values )
151+ )
152+ for name , value_msg in params_with_values :
153+ value = parameter_value_to_python (
154+ value_msg
155+ )
156+ report .add_to_report (
157+ 'parameter' , f'{ name } : { value } ' )
158+ else :
159+ for param_name in sorted (param_names ):
160+ report .add_to_report (
161+ 'parameter' , param_name
162+ )
163+ except RuntimeError :
164+ for param_name in sorted (param_names ):
165+ report .add_to_report (
166+ 'parameter' , param_name
167+ )
168+ except RuntimeError :
169+ pass
117170
118171 report .add_to_report ('total nodes checked' , nodes_checked )
119- report .add_to_report ('total parameter count' , total_param_count )
172+ report .add_to_report ('total parameter count' , param_count )
120173 return report
0 commit comments