feedback_callback support added

Author: mgonzs13

simple_node/include/simple_node/actions/action_client.hpp

@@ -4,6 +4,7 @@
 
 #include <chrono>
 #include <future>
+#include <memory>
 #include <string>
 #include <thread>
 
@@ -20,11 +21,15 @@ template <typename ActionT>
 class ActionClient : public rclcpp_action::Client<ActionT> {
 
   using Goal = typename ActionT::Goal;
+  using Feedback = typename ActionT::Feedback;
   using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
   using Result = typename GoalHandle::Result::SharedPtr;
+  using FeedbackCallback = std::function<void(
+      typename GoalHandle::SharedPtr, const std::shared_ptr<const Feedback>)>;
 
 public:
-  ActionClient(rclcpp::Node *node, std::string action_name)
+  ActionClient(rclcpp::Node *node, std::string action_name,
+               FeedbackCallback feedback_cb = nullptr)
       : rclcpp_action::Client<ActionT>(
             node->get_node_base_interface(), node->get_node_graph_interface(),
             node->get_node_logging_interface(), action_name) {
@@ -34,6 +39,7 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
     this->goal_handle = nullptr;
     this->goal_thread = nullptr;
     this->result = nullptr;
+    this->feedback_cb = feedback_cb;
   }
 
   ~ActionClient() { delete this->goal_thread; }
@@ -42,9 +48,17 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
 
   Result get_result() { return this->result; }
 
-  void send_goal(Goal goal) {
+  void send_goal(Goal goal, FeedbackCallback feedback_cb = nullptr) {
+
+    FeedbackCallback _feedback_cb = this->feedback_cb;
+
+    if (feedback_cb != nullptr) {
+      _feedback_cb = feedback_cb;
+    }
+
     this->set_status(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
-    this->goal_thread = new std::thread(&ActionClient::_send_goal, this, goal);
+    this->goal_thread =
+        new std::thread(&ActionClient::_send_goal, this, goal, _feedback_cb);
   }
 
   bool cancel_goal() {
@@ -102,17 +116,22 @@ class ActionClient : public rclcpp_action::Client<ActionT> {
   std::thread *goal_thread;
   Result result;
   std::shared_ptr<GoalHandle> goal_handle;
+  FeedbackCallback feedback_cb;
 
   void set_status(int8_t status) {
     const std::lock_guard<std::mutex> lock(this->status_mtx);
     this->status = status;
   }
 
-  void _send_goal(Goal goal) {
+  void _send_goal(Goal goal, FeedbackCallback feedback_cb) {
 
     this->result = nullptr;
 
-    auto send_goal_future = this->async_send_goal(goal);
+    auto send_goal_options =
+        typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+    send_goal_options.feedback_callback = feedback_cb;
+
+    auto send_goal_future = this->async_send_goal(goal, send_goal_options);
 
     // wait for acceptance
     while (send_goal_future.wait_for(std::chrono::seconds(1)) !=

simple_node/include/simple_node/node.hpp

@@ -27,13 +27,17 @@ class Node : public rclcpp::Node {
   void join_spin();
 
   template <typename ActionT>
-  typename std::shared_ptr<actions::ActionClient<ActionT>>
-  create_action_client(std::string action_name) {
+  typename std::shared_ptr<actions::ActionClient<ActionT>> create_action_client(
+      std::string action_name,
+      std::function<
+          void(typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
+               const std::shared_ptr<const typename ActionT::Feedback>)>
+          feedback_cb = nullptr) {
 
     rclcpp::CallbackGroup::SharedPtr group = nullptr;
 
     std::shared_ptr<actions::ActionClient<ActionT>> action_client(
-        new actions::ActionClient<ActionT>(this, action_name),
+        new actions::ActionClient<ActionT>(this, action_name, feedback_cb),
         this->create_action_deleter<rclcpp_action::Client<ActionT>>(group));
 
     this->get_node_waitables_interface()->add_waitable(action_client, group);

simple_node/simple_node/actions/client/action_client.py

@@ -1,4 +1,5 @@
 
+from typing import Callable
 import time
 from threading import Thread, Lock
 from action_msgs.msg import GoalStatus
@@ -9,12 +10,13 @@
 
 class ActionClient(ActionClient2):
 
-    def __init__(self, node: Node, action_type, action_name: str):
+    def __init__(self, node: Node, action_type, action_name: str, feedback_cb: Callable = None):
         self._status = GoalStatus.STATUS_UNKNOWN
         self.__status_lock = Lock()
         self.__goal_handle = None
         self.__goal_thread = None
         self.__result = None
+        self.feedback_cb = feedback_cb
         super().__init__(node, action_type, action_name,
                          callback_group=ReentrantCallbackGroup())
 
@@ -56,11 +58,12 @@ def wait_for_result(self):
     def get_result(self):
         return self.__result
 
-    def __send_goal(self, goal):
+    def __send_goal(self, goal, feedback_cb: Callable = None):
 
         self.__result = None
 
-        send_goal_future = self.send_goal_async(goal)
+        send_goal_future = self.send_goal_async(
+            goal, feedback_callback=feedback_cb)
 
         # wait for acceptance
         while not send_goal_future.done():
@@ -100,8 +103,15 @@ def __send_goal(self, goal):
         self._set_status(get_result_future.result().status)
         self.__result = get_result_future.result().result
 
-    def send_goal(self, goal):
-        self.__goal_thread = Thread(target=self.__send_goal, args=(goal,))
+    def send_goal(self, goal, feedback_cb: Callable = None):
+
+        _feedback_cb = self.feedback_cb
+
+        if not feedback_cb is None:
+            _feedback_cb = feedback_cb
+
+        self.__goal_thread = Thread(
+            target=self.__send_goal, args=(goal, _feedback_cb))
         self._set_status(GoalStatus.STATUS_UNKNOWN)
         self.__goal_thread.start()
 

simple_node/simple_node/node.py

@@ -65,7 +65,7 @@ def create_client(self, srv_type, srv_name: str) -> Client:
 
         return super().create_client(srv_type, srv_name, callback_group=ReentrantCallbackGroup())
 
-    def create_action_client(self, action_type, action_name: str) -> ActionClient:
+    def create_action_client(self, action_type, action_name: str, feedback_cb: Callable = None) -> ActionClient:
         """ create action client from node
 
         Args:
@@ -76,7 +76,7 @@ def create_action_client(self, action_type, action_name: str) -> ActionClient:
             ActionClient: client created
         """
 
-        return ActionClient(self, action_type, action_name)
+        return ActionClient(self, action_type, action_name, feedback_cb)
 
     def create_action_server(self,
                              action_type,