diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index c214a145f8..41993b0463 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -459,13 +459,6 @@ class Client : public ClientBase using GoalRequest = typename ActionT::Impl::SendGoalService::Request; auto goal_request = std::make_shared(); goal_request->goal_id.uuid = this->generate_goal_id(); - if (!add_goal_uuid(goal_request->goal_id.uuid)) { - RCLCPP_DEBUG( - get_logger(), - "failed to set content filtered topic for action subscriptions: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 9982ea5a14..bab152a4fb 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -304,9 +304,26 @@ ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback ca { std::unique_lock guard(pimpl_->goal_requests_mutex); int64_t sequence_number; + // goal_id, which type is unique_identifier_msgs::msg::UUID, + // is the first member in ActionT::Impl::SendGoalService::Request + auto goal_id = std::static_pointer_cast(request); + if (!add_goal_uuid(goal_id->uuid)) { + RCLCPP_DEBUG( + get_logger(), + "failed to add goal uuid for setting content filtered topic for action subscriptions: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } rcl_ret_t ret = rcl_action_send_goal_request( pimpl_->client_handle.get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { + if (!remove_goal_uuid(goal_id->uuid)) { + RCLCPP_DEBUG( + get_logger(), + "failed to remove goal uuid: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); } assert(pimpl_->pending_goal_responses.count(sequence_number) == 0);