max_k_len = len(v.key) for v in s.values: print(f"\t {v.key:<{max_k_len+1}}= {v.value}") print("-----") def timer_cb(): print(f"\nDiagnostics timeout from [{frame_id}]\n") timer.cancel() rclpy.init(args=sys.argv) node = Node("diag_listener", namespace=namespace) if namespace == "/": time.sleep(1) nns = node.get_node_names_and_namespaces() discovered_ns = {ns for (_, ns) in nns if ns != "/"} if discovered_ns: ns_selection = { i + 1: ns for (i, ns) in enumerate(sorted(discovered_ns)) } node.get_logger().warn( f"Namespace not specified, use '__ns:=/XXXX' argument for specific namespace" ) print("Discovered namespaces:") for i, ns in ns_selection.items(): print(f"\t{i}: {ns}") try: selection = input("Please enter desired namespace number: ") except KeyboardInterrupt:
p_value.integer_value = value elif p_type == ParameterType.PARAMETER_DOUBLE: p_value.double_value = value elif p_type == ParameterType.PARAMETER_STRING: p_value.string_value = value return p_value rclpy.init(args=sys.argv) node = Node("param_cli_setter") time.sleep(1) done = False while not done: nodes = [((ns + "/") if ns != "/" else "/") + name for name, ns in node.get_node_names_and_namespaces()] nodes = [ fqn for fqn in nodes if "ros2cli" not in fqn and node.get_name() not in fqn ] if not nodes: print("No nodes found, trying again", end="\r") time.sleep(1) continue req_node = select_bullet("Select a node to examine parameters", nodes) list_params_client = node.create_client(ListParameters, req_node + "/list_parameters") get_params_client = node.create_client(GetParameters, req_node + "/get_parameters") set_params_client = node.create_client(SetParameters, req_node + "/set_parameters")