Add new interfaces 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,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.
+   *
    * \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,
+    bool enable_feedback_msg_optimization = false,
     const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options()
   )
   : 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_(enable_feedback_msg_optimization)
   {
   }
 
@@ -180,6 +187,18 @@ 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.");
+          }
+        }
+
         GoalInfo goal_info;
         goal_info.goal_id.uuid = goal_request->goal_id.uuid;
         goal_info.stamp = goal_response->stamp;
@@ -519,6 +538,21 @@ 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.");
+            }
+          }
           std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
           goal_handles_.erase(goal_handle->get_goal_id());
         });
@@ -539,9 +573,26 @@ 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.");
+            }
+          }
+        }
+
         promise->set_value(cancel_response);
         if (cancel_callback) {
           cancel_callback(cancel_response);
@@ -552,6 +603,10 @@ class Client : public ClientBase
 
   std::map<GoalUUID, typename GoalHandle::WeakPtr> goal_handles_;
   std::recursive_mutex goal_handles_mutex_;
+
+  // Enable feedback subscription content filter
+  const bool enable_feedback_msg_optimization_;
+  std::mutex configure_feedback_sub_content_filter_mutex_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/client_base.hpp

@@ -295,6 +295,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>

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`.
@@ -47,6 +52,7 @@ create_client(
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
   const std::string & name,
+  bool enable_feedback_msg_optimization = false,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
   const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
 {
@@ -85,6 +91,7 @@ create_client(
       node_graph_interface,
       node_logging_interface,
       name,
+      enable_feedback_msg_optimization,
       options),
     deleter);
 
@@ -105,6 +112,7 @@ typename Client<ActionT>::SharedPtr
 create_client(
   NodeT node,
   const std::string & name,
+  bool enable_feedback_msg_optimization = false,
   rclcpp::CallbackGroup::SharedPtr group = nullptr,
   const rcl_action_client_options_t & options = rcl_action_client_get_default_options())
 {
@@ -114,6 +122,7 @@ create_client(
     node->get_node_logging_interface(),
     node->get_node_waitables_interface(),
     name,
+    enable_feedback_msg_optimization,
     group,
     options);
 }

rclcpp_action/src/client_base.cpp

@@ -830,4 +830,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