diff --git a/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp index 16ee058a07..dfaffc2a72 100644 --- a/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp @@ -158,6 +158,17 @@ class ActionClientIntraProcessBase : public rclcpp::Waitable event_info_multi_map_.erase(goal_id); } + bool has_goal_id(size_t goal_id) const + { + // Check if the intra-process client has this goal_id + auto it = event_info_multi_map_.find(goal_id); + if (it != event_info_multi_map_.end()) { + return true; + } + + return false; + } + private: std::string action_name_; QoS qos_profile_; diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index fcac2e02b3..dfa55ce6f2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -97,6 +97,16 @@ class ClientBase : public rclcpp::Waitable ); } + /// Return true if there is an intra-process action server that is ready to take goal requests. + bool + intra_process_action_server_is_available() + { + if (auto ipm = weak_ipm_.lock()) { + return ipm->action_server_is_available(ipc_action_client_id_); + } + return false; + } + // ------------- // Waitables API @@ -513,13 +523,17 @@ class Client : public ClientBase // If there's not, we fall back into inter-process communication, since // the server might be available in another process or was configured to not use IPC. if (intra_process_server_available) { - ipc_action_client_->store_goal_response_callback( - hashed_guuid, goal_response_callback); + bool goal_sent_by_ipc = ipc_action_client_->has_goal_id(hashed_guuid); - intra_process_send_done = ipm->template intra_process_action_send_goal_request( - ipc_action_client_id_, - std::move(goal_request), - hashed_guuid); + if (goal_sent_by_ipc) { + ipc_action_client_->store_goal_response_callback( + hashed_guuid, goal_response_callback); + + intra_process_send_done = ipm->template intra_process_action_send_goal_request( + ipc_action_client_id_, + std::move(goal_request), + hashed_guuid); + } } } @@ -835,12 +849,17 @@ class Client : public ClientBase // the server might be available in another process or was configured to not use IPC. if (intra_process_server_available) { size_t hashed_guuid = std::hash()(goal_handle->get_goal_id()); - ipc_action_client_->store_result_response_callback( - hashed_guuid, result_response_callback); - intra_process_send_done = ipm->template intra_process_action_send_result_request( - ipc_action_client_id_, - std::move(goal_result_request)); + bool goal_sent_by_ipc = ipc_action_client_->has_goal_id(hashed_guuid); + + if (goal_sent_by_ipc) { + ipc_action_client_->store_result_response_callback( + hashed_guuid, result_response_callback); + + intra_process_send_done = ipm->template intra_process_action_send_result_request( + ipc_action_client_id_, + std::move(goal_result_request)); + } } } @@ -891,12 +910,17 @@ class Client : public ClientBase // the server might be available in another process or was configured to not use IPC. if (intra_process_server_available) { size_t hashed_guuid = std::hash()(cancel_request->goal_info.goal_id.uuid); - ipc_action_client_->store_cancel_goal_callback( - hashed_guuid, cancel_goal_callback); - intra_process_send_done = ipm->template intra_process_action_send_cancel_request( + bool goal_sent_by_ipc = ipc_action_client_->has_goal_id(hashed_guuid); + + if (goal_sent_by_ipc) { + ipc_action_client_->store_cancel_goal_callback( + hashed_guuid, cancel_goal_callback); + + intra_process_send_done = ipm->template intra_process_action_send_cancel_request( ipc_action_client_id_, std::move(cancel_request)); + } } }