Support to configure feedback subscription content filter for action client

rclcpp_action/include/rclcpp_action/client.hpp

@@ -111,24 +111,31 @@ class Client : public ClientBase
    * This constructs an action client, but it will not work until it has been added to a node.
    * Use `rclcpp_action::create_client()` to both construct and add to a node.
    *
+   * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+   * simultaneously. If the number of goals exceeds the limit, optimization is automatically
+   * disabled.
+   *
    * \param[in] node_base A pointer to the base interface of a node.
    * \param[in] node_graph A pointer to an interface that allows getting graph information about
    *   a node.
    * \param[in] node_logging A pointer to an interface that allows getting a node's logger.
    * \param[in] action_name The action name.
    * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`.
+   * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+   *   optimize the handling of feedback messages.
    */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const std::string & action_name,
-    const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options()
+    const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(),
+    bool enable_feedback_msg_optimization = false
   )
   : ClientBase(
       node_base, node_graph, node_logging, action_name,
       rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
-      client_options)
+      client_options, enable_feedback_msg_optimization)
   {
   }
 
@@ -180,6 +187,20 @@ class Client : public ClientBase
           }
           return;
         }
+
+        // If feedback message optimization is enabled, add goal id to feedback subscription
+        // content filter
+        if (enable_feedback_msg_optimization_) {
+          std::lock_guard<std::mutex> lock(configure_feedback_sub_content_filter_mutex_);
+          if (!configure_feedback_subscription_filter_add_goal_id(goal_request->goal_id.uuid)) {
+            RCLCPP_ERROR(
+              this->get_logger(),
+              "Failed to add goal id to feedback subscription content filter for action client."
+              " Disable feedback message optimization.");
+            enable_feedback_msg_optimization_ = false;
+          }
+        }
+
         GoalInfo goal_info;
         goal_info.goal_id.uuid = goal_request->goal_id.uuid;
         goal_info.stamp = goal_response->stamp;
@@ -519,6 +540,22 @@ class Client : public ClientBase
           wrapped_result.goal_id = goal_handle->get_goal_id();
           wrapped_result.code = static_cast<ResultCode>(result_response->status);
           goal_handle->set_result(wrapped_result);
+
+          // If feedback message optimization is enabled, remove goal id from feedback subscription
+          // content filter
+          if (this->enable_feedback_msg_optimization_) {
+            std::lock_guard<std::mutex> lock(
+              this->configure_feedback_sub_content_filter_mutex_);
+            if (!this->configure_feedback_subscription_filter_remove_goal_id(
+                goal_handle->get_goal_id()))
+            {
+              RCLCPP_ERROR(
+                this->get_logger(),
+                "Failed to remove goal id from feedback subscription content filter for action "
+                "client. Disable feedback message optimization.");
+              this->enable_feedback_msg_optimization_ = false;
+            }
+          }
           std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
           goal_handles_.erase(goal_handle->get_goal_id());
         });
@@ -539,9 +576,30 @@ class Client : public ClientBase
     std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
     this->send_cancel_request(
       std::static_pointer_cast<void>(cancel_request),
-      [cancel_callback, promise](std::shared_ptr<void> response) mutable
+      [this, cancel_callback, promise](std::shared_ptr<void> response) mutable
       {
         auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
+
+        // If feedback message optimization is enabled, remove goal id from feedback subscription
+        // content filter
+        if (this->enable_feedback_msg_optimization_) {
+          for (const auto & goal_info : cancel_response->goals_canceling) {
+            std::lock_guard<std::mutex> lock(this->configure_feedback_sub_content_filter_mutex_);
+            if (!this->configure_feedback_subscription_filter_remove_goal_id(
+                goal_info.goal_id.uuid))
+            {
+              RCLCPP_ERROR(
+                this->get_logger(),
+                "Failed to remove goal id from feedback subscription content filter for action "
+                "client. Disable feedback message optimization.");
+              this->enable_feedback_msg_optimization_ = false;
+              // When an error occurs, the rcl layer will disable the content filter, so there is
+              // no need to continue removing the goal id.
+              break;
+            }
+          }
+        }
+
         promise->set_value(cancel_response);
         if (cancel_callback) {
           cancel_callback(cancel_response);

rclcpp_action/include/rclcpp_action/client_base.hpp

@@ -201,7 +201,8 @@ class ClientBase : public rclcpp::Waitable
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const std::string & action_name,
     const rosidl_action_type_support_t * type_support,
-    const rcl_action_client_options_t & options);
+    const rcl_action_client_options_t & options,
+    bool enable_feedback_msg_optimization);
 
   /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
   RCLCPP_ACTION_PUBLIC
@@ -295,6 +296,22 @@ class ClientBase : public rclcpp::Waitable
   void
   handle_feedback_message(std::shared_ptr<void> message) = 0;
 
+  /// \internal
+  /// \return true if goal id was added to feedback subscription content filter successfully
+  /// \return false if an error occurred during calling rcl function or
+  ///   if the used rmw middleware doesn't support Content Filtering feature.
+  bool
+  configure_feedback_subscription_filter_add_goal_id(const GoalUUID & goal_id);
+
+
+  /// \internal
+  /// \return true if goal id was removed from feedback subscription content filter successfully or
+  ///   goal id was not found in feedback subscription content filter or
+  ///   if the used rmw middleware doesn't support Content Filtering feature.
+  /// \return false if an error occurred during calling rcl function.
+  bool
+  configure_feedback_subscription_filter_remove_goal_id(const GoalUUID & goal_id);
+
   /// \internal
   virtual
   std::shared_ptr<void>
@@ -322,6 +339,15 @@ class ClientBase : public rclcpp::Waitable
   // Storage for std::function callbacks to keep them in scope
   std::unordered_map<EntityType, std::function<void(size_t)>> entity_type_to_on_ready_callback_;
 
+  // Enable feedback subscription content filter
+  // Initialization is configured by the user.
+  // When an error occurs while adding or removing a goal ID from the content filter, it will
+  // automatically be set to false.
+  std::atomic_bool enable_feedback_msg_optimization_;
+  // Mutex to protect feedback subscription content filter configuration because the related rcl
+  // function is not thread-safe.
+  std::mutex configure_feedback_sub_content_filter_mutex_;
+
 private:
   std::unique_ptr<ClientBaseImpl> pimpl_;
 

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -30,6 +30,9 @@ namespace rclcpp_action
  * This function is equivalent to \sa create_client()` however is using the individual
  * node interfaces to create the client.
  *
+ * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+ * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled.
+ *
  * \param[in] node_base_interface The node base interface of the corresponding node.
  * \param[in] node_graph_interface The node graph interface of the corresponding node.
  * \param[in] node_logging_interface The node logging interface of the corresponding node.
@@ -38,6 +41,8 @@ namespace rclcpp_action
  * \param[in] group The action client will be added to this callback group.
  *   If `nullptr`, then the action client is added to the default callback group.
  * \param[in] options Options to pass to the underlying `rcl_action_client_t`.
+ * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+ *   optimize the handling of feedback messages.
  */
 template<typename ActionT>
 typename Client<ActionT>::SharedPtr
@@ -48,7 +53,8 @@ create_client(
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
   const std::string & name,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
-  const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
+  const rcl_action_client_options_t & options = rcl_action_client_get_default_options(),
+  bool enable_feedback_msg_optimization = false)
 {
   std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
     node_waitables_interface;
@@ -85,7 +91,8 @@ create_client(
       node_graph_interface,
       node_logging_interface,
       name,
-      options),
+      options,
+      enable_feedback_msg_optimization),
     deleter);
 
   node_waitables_interface->add_waitable(action_client, group);
@@ -94,19 +101,26 @@ create_client(
 
 /// Create an action client.
 /**
+ *
+ * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+ * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled.
+ *
  * \param[in] node The action client will be added to this node.
  * \param[in] name The action name.
  * \param[in] group The action client will be added to this callback group.
  *   If `nullptr`, then the action client is added to the default callback group.
  * \param[in] options Options to pass to the underlying `rcl_action_client_t`.
+ * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+ *   optimize the handling of feedback messages.
  */
 template<typename ActionT, typename NodeT>
 typename Client<ActionT>::SharedPtr
 create_client(
   NodeT node,
   const std::string & name,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
-  const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
+  const rcl_action_client_options_t & options = rcl_action_client_get_default_options(),
+  bool enable_feedback_msg_optimization = false)
 {
   return rclcpp_action::create_client<ActionT>(
     node->get_node_base_interface(),
@@ -115,7 +129,8 @@ create_client(
     node->get_node_waitables_interface(),
     name,
     group,
-    options);
+    options,
+    enable_feedback_msg_optimization);
 }
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/create_generic_client.hpp

@@ -26,6 +26,9 @@ namespace rclcpp_action
  * This function is equivalent to \sa create_generic_client()` however is using the individual
  * node interfaces to create the client.
  *
+ * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+ * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled.
+ *
  * \param[in] node_base_interface The node base interface of the corresponding node.
  * \param[in] node_graph_interface The node graph interface of the corresponding node.
  * \param[in] node_logging_interface The node logging interface of the corresponding node.
@@ -35,6 +38,8 @@ namespace rclcpp_action
  * \param[in] group The action client will be added to this callback group.
  *   If `nullptr`, then the action client is added to the default callback group.
  * \param[in] options Options to pass to the underlying `rcl_action_client_t`.
+ * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+ *   optimize the handling of feedback messages.
  * \return newly created generic client.
  */
 RCLCPP_ACTION_PUBLIC
@@ -47,16 +52,23 @@ create_generic_client(
   const std::string & name,
   const std::string & type,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
-  const rcl_action_client_options_t & options = rcl_action_client_get_default_options());
+  const rcl_action_client_options_t & options = rcl_action_client_get_default_options(),
+  bool enable_feedback_msg_optimization = false);
 
 /// Create an action generic client.
 /**
+ *
+ * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+ * simultaneously. If the number of goals exceeds the limit, optimization is automatically disabled.
+ *
  * \param[in] node The action client will be added to this node.
  * \param[in] name The action name.
  * \param[in] type The action type.
  * \param[in] group The action client will be added to this callback group.
  *   If `nullptr`, then the action client is added to the default callback group.
  * \param[in] options Options to pass to the underlying `rcl_action_client_t`.
+ * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+ *   optimize the handling of feedback messages.
  * \return newly created generic client.
  */
 template<typename NodeT>
@@ -66,7 +78,8 @@ create_generic_client(
   const std::string & name,
   const std::string & type,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
-  const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
+  const rcl_action_client_options_t & options = rcl_action_client_get_default_options(),
+  bool enable_feedback_msg_optimization = false)
 {
   return rclcpp_action::create_generic_client(
     node->get_node_base_interface(),
@@ -76,7 +89,8 @@ create_generic_client(
     name,
     type,
     group,
-    options);
+    options,
+    enable_feedback_msg_optimization);
 }
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/generic_client.hpp

@@ -95,6 +95,10 @@ class GenericClient : public ClientBase
    * node.
    * Use `rclcpp_action::create_generic_client()` to both construct and add to a node.
    *
+   * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
+   * simultaneously. If the number of goals exceeds the limit, optimization is automatically
+   * disabled.
+   *
    * \param[in] node_base A pointer to the base interface of a node.
    * \param[in] node_graph A pointer to an interface that allows getting graph information about
    *   a node.
@@ -103,6 +107,8 @@ class GenericClient : public ClientBase
    * \param[in] typesupport_lib A pointer to type support library for the action type
    * \param[in] action_typesupport_handle the action type support handle
    * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`.
+   * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+   *   optimize the handling of feedback messages.
    */
   RCLCPP_ACTION_PUBLIC
   GenericClient(
@@ -112,7 +118,8 @@ class GenericClient : public ClientBase
     const std::string & action_name,
     std::shared_ptr<rcpputils::SharedLibrary> typesupport_lib,
     const rosidl_action_type_support_t * action_typesupport_handle,
-    const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options());
+    const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(),
+    bool enable_feedback_msg_optimization = false);
 
   /// Send an action goal and asynchronously get the result.
   /**

rclcpp_action/src/client_base.cpp

@@ -190,8 +190,10 @@ ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   const std::string & action_name,
   const rosidl_action_type_support_t * type_support,
-  const rcl_action_client_options_t & client_options)
-: pimpl_(new ClientBaseImpl(
+  const rcl_action_client_options_t & client_options,
+  bool enable_feedback_msg_optimization)
+: enable_feedback_msg_optimization_(enable_feedback_msg_optimization),
+  pimpl_(new ClientBaseImpl(
       node_base, node_graph, node_logging, action_name, type_support, client_options))
 {
 }
@@ -830,4 +832,23 @@ ClientBase::configure_introspection(
   }
 }
 
+bool
+ClientBase::configure_feedback_subscription_filter_add_goal_id(
+  const GoalUUID & goal_id)
+{
+  const rcl_ret_t ret = rcl_action_client_configure_feedback_subscription_filter_add_goal_id(
+    pimpl_->client_handle.get(), goal_id.data(), goal_id.size());
+
+  return ret == RCL_RET_OK;
+}
+
+bool
+ClientBase::configure_feedback_subscription_filter_remove_goal_id(
+  const GoalUUID & goal_id)
+{
+  const rcl_ret_t ret = rcl_action_client_configure_feedback_subscription_filter_remove_goal_id(
+    pimpl_->client_handle.get(), goal_id.data(), goal_id.size());
+
+  return ret == RCL_RET_OK;
+}
 }  // namespace rclcpp_action

rclcpp_action/src/create_generic_client.cpp

@@ -32,7 +32,8 @@ create_generic_client(
   const std::string & name,
   const std::string & type,
   rclcpp::CallbackGroup::SharedPtr group,
-  const rcl_action_client_options_t & options)
+  const rcl_action_client_options_t & options,
+  bool enable_feedback_msg_optimization)
 {
   std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
     node_waitables_interface;
@@ -75,7 +76,8 @@ create_generic_client(
       name,
       typesupport_lib,
       action_typesupport_handle,
-      options),
+      options,
+      enable_feedback_msg_optimization),
     deleter);
 
   node_waitables_interface->add_waitable(action_client, group);