Skip to content

temporary workaroundto problems with registry #96

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

temporary workaroundto problems with registry #96

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 31 additions & 20 deletions behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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_ = {};
Expand Down Expand Up @@ -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");
}
Expand Down Expand Up @@ -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 =
Expand All @@ -468,7 +480,6 @@ inline NodeStatus RosActionNode<T>::tick()
}
else
{
goal_received_ = true;
goal_handle_ = future_goal_handle_.get();
future_goal_handle_ = {};

Expand Down
Loading