[rclcpp_action] Action client holds weak pointers to goal handles (#1122)

* [rclcpp_action] Action client holds weak pointers to goal handles

Fixes #861

It is against the design of ROS actions to rely on the status topic for the core implementation,
instead it should just be used for introspection.

Rather than relying on the status topic to remove references to goal handles, the action client
instead holds weak pointers to the goal handles. This way as long as a user holds a reference to
the goal handle they can use it to interact with the action client.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Move cleanup logic to the end of the function

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Add TODO

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Log debug messages when dropping a weak references to goal handles

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Improve documentation

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

rclcpp_action/include/rclcpp_action/client.hpp

@@ -331,7 +331,9 @@ public:
    * 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 @@ public:
           }
         }
       });
+
+    // 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 @@ public:
     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 @@ private:
         "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 @@ private:
           "Received status for unknown goal. Ignoring...");
         continue;
       }
-      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
-      goal_handle->set_status(status.status);
+      // Forget about the goal if there are no more user references
-      const int8_t goal_status = goal_handle->get_status();
+      if (!goal_handle) {
-      if (
+        RCLCPP_DEBUG(
-        goal_status == GoalStatus::STATUS_SUCCEEDED ||
+          this->get_logger(),
-        goal_status == GoalStatus::STATUS_CANCELED ||
+          "Dropping weak reference to goal handle during status callback");
-        goal_status == GoalStatus::STATUS_ABORTED)
-      {
         goal_handles_.erase(goal_id);
+        continue;
       }
+      goal_handle->set_status(status.status);
     }
   }
 
@@ -639,7 +672,7 @@ private:
     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