[rclcpp_action] Action client holds weak pointers to goal handles

rclcpp_action/include/rclcpp_action/client.hpp

@@ -331,7 +331,9 @@ class Client : public ClientBase
    * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`.
    * If the goal is rejected by an action server, then the future is set to a `nullptr`.
    *
-   * The goal handle is used to monitor the status of the goal and get the final result.
+   * The returned goal handle is used to monitor the status of the goal and get the final result.
+   * It is valid as long as you hold a reference to the shared pointer or until the
+   * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN.
    *
    * \param[in] goal The goal request.
    * \param[in] options Options for sending the goal request. Contains references to callbacks for
@@ -386,6 +388,26 @@ class Client : public ClientBase
           }
         }
       });
+
+    // TODO(jacobperron): Encapsulate into it's own function and
+    //                    consider exposing an option to disable this cleanup
+    // To prevent the list from growing out of control, forget about any goals
+    // with no more user references
+    {
+      std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+      auto goal_handle_it = goal_handles_.begin();
+      while (goal_handle_it != goal_handles_.end()) {
+        if (!goal_handle_it->second.lock()) {
+          RCLCPP_DEBUG(
+            this->get_logger(),
+            "Dropping weak reference to goal handle during send_goal()");
+          goal_handle_it = goal_handles_.erase(goal_handle_it);
+        } else {
+          ++goal_handle_it;
+        }
+      }
+    }
+
     return future;
   }
 
@@ -494,7 +516,10 @@ class Client : public ClientBase
     std::lock_guard<std::mutex> guard(goal_handles_mutex_);
     auto it = goal_handles_.begin();
     while (it != goal_handles_.end()) {
-      it->second->invalidate();
+      typename GoalHandle::SharedPtr goal_handle = it->second.lock();
+      if (goal_handle) {
+        goal_handle->invalidate();
+      }
       it = goal_handles_.erase(it);
     }
   }
@@ -546,7 +571,15 @@ class Client : public ClientBase
         "Received feedback for unknown goal. Ignoring...");
       return;
     }
-    typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+    typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
+    // Forget about the goal if there are no more user references
+    if (!goal_handle) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "Dropping weak reference to goal handle during feedback callback");
+      goal_handles_.erase(goal_id);
+      return;
+    }
     auto feedback = std::make_shared<Feedback>();
     *feedback = feedback_message->feedback;
     goal_handle->call_feedback_callback(goal_handle, feedback);
@@ -575,16 +608,16 @@ class Client : public ClientBase
           "Received status for unknown goal. Ignoring...");
         continue;
       }
-      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
-      goal_handle->set_status(status.status);
-      const int8_t goal_status = goal_handle->get_status();
-      if (
-        goal_status == GoalStatus::STATUS_SUCCEEDED ||
-        goal_status == GoalStatus::STATUS_CANCELED ||
-        goal_status == GoalStatus::STATUS_ABORTED)
-      {
+      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
+      // Forget about the goal if there are no more user references
+      if (!goal_handle) {
+        RCLCPP_DEBUG(
+          this->get_logger(),
+          "Dropping weak reference to goal handle during status callback");
         goal_handles_.erase(goal_id);
+        continue;
       }
+      goal_handle->set_status(status.status);
     }
   }
 
@@ -639,7 +672,7 @@ class Client : public ClientBase
     return future;
   }
 
-  std::map<GoalUUID, typename GoalHandle::SharedPtr> goal_handles_;
+  std::map<GoalUUID, typename GoalHandle::WeakPtr> goal_handles_;
   std::mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action