diff --git a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py index ae7ac0c13..40dd28382 100755 --- a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py +++ b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_client.py @@ -13,12 +13,23 @@ # See the License for the specific language governing permissions and # limitations under the License. +# The program has a short runtime, so you can directly set the parameter +# "action_client_configure_introspection" at execution command +# e.g. +# ros2 run action_tutorials_py fibonacci_action_client --ros-args -p +# "action_client_configure_introspection:=contents" + from example_interfaces.action import Fibonacci +from rcl_interfaces.msg import SetParametersResult + import rclpy from rclpy.action import ActionClient from rclpy.executors import ExternalShutdownException from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState class FibonacciActionClient(Node): @@ -26,6 +37,49 @@ class FibonacciActionClient(Node): def __init__(self): super().__init__('fibonacci_action_client') self._action_client = ActionClient(self, Fibonacci, 'fibonacci') + self.add_on_set_parameters_callback(self.on_set_parameters_callback) + self.add_post_set_parameters_callback(self.on_post_set_parameters_callback) + self.declare_parameter('action_client_configure_introspection', 'disabled') + + def _check_parameter(self, parameter_list, parameter_name): + result = SetParametersResult() + result.successful = True + for param in parameter_list: + if param.name != parameter_name: + continue + + if param.type_ != Parameter.Type.STRING: + result.successful = False + result.reason = 'must be a string' + break + + if param.value not in ('disabled', 'metadata', 'contents'): + result.successful = False + result.reason = "must be one of 'disabled', 'metadata', or 'contents" + break + + return result + + def on_set_parameters_callback(self, parameter_list): + return self._check_parameter(parameter_list, 'action_client_configure_introspection') + + def on_post_set_parameters_callback(self, parameter_list): + for param in parameter_list: + if param.name != 'action_client_configure_introspection': + continue + + introspection_state = ServiceIntrospectionState.OFF + if param.value == 'disabled': + introspection_state = ServiceIntrospectionState.OFF + elif param.value == 'metadata': + introspection_state = ServiceIntrospectionState.METADATA + elif param.value == 'contents': + introspection_state = ServiceIntrospectionState.CONTENTS + + self._action_client.configure_introspection(self.get_clock(), + qos_profile_system_default, + introspection_state) + break def send_goal(self, order): goal_msg = Fibonacci.Goal() diff --git a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py index c948d3593..ab2298587 100755 --- a/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py +++ b/action_tutorials/action_tutorials_py/action_tutorials_py/fibonacci_action_server.py @@ -17,10 +17,15 @@ from example_interfaces.action import Fibonacci +from rcl_interfaces.msg import SetParametersResult + import rclpy from rclpy.action import ActionServer from rclpy.executors import ExternalShutdownException from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_system_default +from rclpy.service_introspection import ServiceIntrospectionState class FibonacciActionServer(Node): @@ -32,6 +37,49 @@ def __init__(self): Fibonacci, 'fibonacci', self.execute_callback) + self.add_on_set_parameters_callback(self.on_set_parameters_callback) + self.add_post_set_parameters_callback(self.on_post_set_parameters_callback) + self.declare_parameter('action_server_configure_introspection', 'disabled') + + def _check_parameter(self, parameter_list, parameter_name): + result = SetParametersResult() + result.successful = True + for param in parameter_list: + if param.name != parameter_name: + continue + + if param.type_ != Parameter.Type.STRING: + result.successful = False + result.reason = 'must be a string' + break + + if param.value not in ('disabled', 'metadata', 'contents'): + result.successful = False + result.reason = "must be one of 'disabled', 'metadata', or 'contents" + break + + return result + + def on_set_parameters_callback(self, parameter_list) -> SetParametersResult: + return self._check_parameter(parameter_list, 'action_server_configure_introspection') + + def on_post_set_parameters_callback(self, parameter_list): + for param in parameter_list: + if param.name != 'action_server_configure_introspection': + continue + + introspection_state = ServiceIntrospectionState.OFF + if param.value == 'disabled': + introspection_state = ServiceIntrospectionState.OFF + elif param.value == 'metadata': + introspection_state = ServiceIntrospectionState.METADATA + elif param.value == 'contents': + introspection_state = ServiceIntrospectionState.CONTENTS + + self._action_server.configure_introspection(self.get_clock(), + qos_profile_system_default, + introspection_state) + break def execute_callback(self, goal_handle): self.get_logger().info('Executing goal...')