diff --git a/ros2action/ros2action/verb/send_goal.py b/ros2action/ros2action/verb/send_goal.py index 97970a41e..f1aa2c6e6 100644 --- a/ros2action/ros2action/verb/send_goal.py +++ b/ros2action/ros2action/verb/send_goal.py @@ -109,14 +109,20 @@ def send_goal(action_name, action_type, goal_values, feedback_callback): goal = action_module.Goal() + timestamp_fields = [] try: - set_message_fields(goal, goal_dict) + timestamp_fields = set_message_fields( + goal, goal_dict, expand_header_auto=True, expand_time_now=True) except Exception as ex: return 'Failed to populate message fields: {!r}'.format(ex) print('Waiting for an action server to become available...') action_client.wait_for_server() + stamp_now = node.get_clock().now().to_msg() + for field_setter in timestamp_fields: + field_setter(stamp_now) + print('Sending goal:\n {}'.format(message_to_yaml(goal))) goal_future = action_client.send_goal_async(goal, feedback_callback) rclpy.spin_until_future_complete(node, goal_future) @@ -175,6 +181,7 @@ def _sigint_cancel_handler(sig, frame): print('Result:\n {}'.format(message_to_yaml(result.result))) print('Goal finished with status: {}'.format(_goal_status_to_string(result.status))) + finally: if action_client is not None: action_client.destroy() diff --git a/ros2service/ros2service/verb/call.py b/ros2service/ros2service/verb/call.py index 4f75f5462..7ee5dd850 100644 --- a/ros2service/ros2service/verb/call.py +++ b/ros2service/ros2service/verb/call.py @@ -96,8 +96,10 @@ def requester(service_type, service_name, values, period): request = srv_module.Request() + timestamp_fields = [] try: - set_message_fields(request, values_dictionary) + timestamp_fields = set_message_fields( + request, values_dictionary, expand_header_auto=True, expand_time_now=True) except Exception as e: return 'Failed to populate field: {0}'.format(e) @@ -106,6 +108,9 @@ def requester(service_type, service_name, values, period): cli.wait_for_service() while True: + stamp_now = node.get_clock().now().to_msg() + for field_setter in timestamp_fields: + field_setter(stamp_now) print('requester: making request: %r\n' % request) last_call = time.time() future = cli.call_async(request)