temporary workaroundto problems with registry

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

@@ -97,7 +97,18 @@ class RosActionNode : public BT::ActionNodeBase
  explicit RosActionNode(const std::string& instance_name, const BT::NodeConfig& conf,
  const RosNodeParams& params);
 
- virtual ~RosActionNode() = default;
+ virtual ~RosActionNode()
+ {
+ std::unique_lock lk(getMutex());
+ auto& registry = getRegistry();
+ for(auto it = registry.begin(); it != registry.end(); ) {
+ if (it->second.expired()) {
+ it = registry.erase(it);
+ } else {
+ ++it;
+ }
+ }
+ }
 
  /**
  * @brief Any subclass of RosActionNode that has ports must implement a
@@ -229,7 +240,6 @@ class RosActionNode : public BT::ActionNodeBase
 
  rclcpp::Time time_goal_sent_;
  NodeStatus on_feedback_state_change_;
- bool goal_received_;
  WrappedResult result_;
 
  bool createClient(const std::string& action_name);
@@ -302,22 +312,25 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
  throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the "
  "ownership of the node.");
  }
- action_client_key_ = std::string(node->get_fully_qualified_name()) + "/" + action_name;
 
- auto& registry = getRegistry();
- auto it = registry.find(action_client_key_);
- if(it == registry.end() || it->second.expired())
+ const auto key = std::string(node->get_fully_qualified_name()) + "/" + action_name;
+ if(key != action_client_key_ || action_name_ != action_name || !client_instance_)
  {
- client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);
- registry.insert({ action_client_key_, client_instance_ });
- }
- else
- {
- client_instance_ = it->second.lock();
+ auto& registry = getRegistry();
+ auto it = registry.find(key);
+ if(it == registry.end() || it->second.expired())
+ {
+ client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);
+ registry[key] = client_instance_;
+ }
+ else
+ {
+ client_instance_ = it->second.lock();
+ }
+ action_client_key_ = key;
+ action_name_ = action_name;
  }
 
- action_name_ = action_name;
-
  bool found =
  client_instance_->action_client->wait_for_action_server(wait_for_server_timeout_);
  if(!found)
@@ -381,7 +394,7 @@ inline NodeStatus RosActionNode<T>::tick()
  {
  setStatus(NodeStatus::RUNNING);
 
- goal_received_ = false;
+ goal_handle_.reset();
  future_goal_handle_ = {};
  on_feedback_state_change_ = NodeStatus::RUNNING;
  result_ = {};
@@ -417,9 +430,8 @@ inline NodeStatus RosActionNode<T>::tick()
  };
  //--------------------
  goal_options.goal_response_callback =
- [this](typename GoalHandle::SharedPtr const future_handle) {
- auto goal_handle_ = future_handle.get();
- if(!goal_handle_)
+ [this](typename GoalHandle::SharedPtr const goal_handle) {
+ if(!goal_handle || !(*goal_handle))
  {
  RCLCPP_ERROR(logger(), "Goal was rejected by server");
  }
@@ -447,7 +459,7 @@ inline NodeStatus RosActionNode<T>::tick()
  client_instance_->callback_executor.spin_some();
 
  // FIRST case: check if the goal request has a timeout
- if(!goal_received_)
+ if(!goal_handle_)
  {
  auto nodelay = std::chrono::milliseconds(0);
  auto timeout =
@@ -468,7 +480,6 @@ inline NodeStatus RosActionNode<T>::tick()
  }
  else
  {
- goal_received_ = true;
  goal_handle_ = future_goal_handle_.get();
  future_goal_handle_ = {};