Fixes for intra-process actions (irobot-ros#144)

* Fixes for intra-process Actions * Fixes for Clang builds * Fix deadlock * Server to store results until client requests them * Fix feedback/result data race See ros2#2451 * Add missing mutex * Check return value of intra_process_action_send --------- Co-authored-by: Mauro Passerino <mpasserino@irobot.com>
apojomovsky · Aug 8, 2024 · 4c19071 · 4c19071
1 parent ccdd52e
commit 4c19071
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 15 deletions.
diff --git 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<ActionT>(
-            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<ActionT>(
+              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<GoalUUID>()(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<ActionT>(
-              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<ActionT>(
+                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<GoalUUID>()(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<ActionT>(
+        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<ActionT>(
             ipc_action_client_id_,
             std::move(cancel_request));
+        }
       }
     }
 

diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -675,7 +675,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
       rclcpp::get_logger("rclcpp_action"),
       "Action server can't send result response, missing IPC Action client: %s. "
       "Will do inter-process publish",
-      this->action_name_.c_str());
+      this->action_name_);
+
     return true;
   }