Support to configure feedback subscription content filter for action client

Signed-off-by: Barry Xu <barry.xu@sony.com>

Author: Barry-Xu-2018

rclcpp_action/include/rclcpp_action/client.hpp

@@ -111,24 +111,30 @@ 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.
+   *
    * \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] enable_feedback_msg_optimization Enable feedback subscription content filter to
+   *   reduce network load.
    * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`.
    */
   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 +186,19 @@ 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.");
+            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 +538,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_.load()) {
+            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.");
+              this->enable_feedback_msg_optimization_.store(false);
+            }
+          }
           std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
           goal_handles_.erase(goal_handle->get_goal_id());
         });
@@ -539,9 +574,27 @@ 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_.load()) {
+          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.");
+            }
+            this->enable_feedback_msg_optimization_.store(false);
+          }
+        }
+
         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,14 @@ 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 configuration of feedback subscription content filter since
+  std::mutex configure_feedback_sub_content_filter_mutex_;
+
 private:
   std::unique_ptr<ClientBaseImpl> pimpl_;
 

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -30,11 +30,16 @@ 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.
  * \param[in] node_waitables_interface The node waitables interface of the corresponding node.
  * \param[in] name The action name.
+ * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
+ *   reduce network load.
  * \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`.
@@ -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);
@@ -106,7 +113,8 @@ 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 +123,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

@@ -47,7 +47,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);
 
 /// Create an action generic client.
 /**
@@ -66,7 +67,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 +78,8 @@ create_generic_client(
     name,
     type,
     group,
-    options);
+    options,
+    enable_feedback_msg_optimization);
 }
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/generic_client.hpp

@@ -112,7 +112,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);

rclcpp_action/src/generic_client.cpp

@@ -32,11 +32,13 @@ GenericClient::GenericClient(
   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)
+  const rcl_action_client_options_t & client_options,
+  bool enable_feedback_msg_optimization)
 : ClientBase(
     node_base, node_graph, node_logging, action_name,
     action_typesupport_handle,
-    client_options),
+    client_options,
+    enable_feedback_msg_optimization),
   ts_lib_(std::move(typesupport_lib))
 {
   auto goal_service_request_type_support_intro = get_message_typesupport_handle(
@@ -143,6 +145,19 @@ GenericClient::async_send_goal(
         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(uuid)) {
+          RCLCPP_ERROR(
+            this->get_logger(),
+            "Failed to add goal id to feedback subscription content filter for action "
+            "generic client.");
+          enable_feedback_msg_optimization_ = false;
+        }
+      }
+
       action_msgs::msg::GoalInfo goal_info;
       goal_info.goal_id.uuid = uuid;
       std::memcpy(
@@ -450,6 +465,22 @@ GenericClient::make_result_aware(typename GenericClientGoalHandle::SharedPtr goa
 
         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_.load()) {
+          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.");
+            this->enable_feedback_msg_optimization_.store(false);
+          }
+        }
+
         std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
         goal_handles_.erase(goal_handle->get_goal_id());
       });
@@ -469,9 +500,27 @@ GenericClient::async_cancel(
   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_.load()) {
+        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.");
+          }
+          this->enable_feedback_msg_optimization_.store(false);
+        }
+      }
+
       promise->set_value(cancel_response);
       if (cancel_callback) {
         cancel_callback(cancel_response);