From d141ffdfb943c12ab0e7e0b4d16d659cd8984853 Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Thu, 13 Jun 2019 08:44:11 -0700
Subject: [PATCH] Revert back to CoroActionNodes instead of AsyncActionNodes
(#836)
---
.../include/nav2_tasks/bt_action_node.hpp | 36 +++++++------------
1 file changed, 13 insertions(+), 23 deletions(-)
diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
index 678ed2ed..5bfbe2e1 100644
--- a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
+++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
@@ -16,7 +16,6 @@
#define NAV2_TASKS__BT_ACTION_NODE_HPP_
#include <memory>
-#include <mutex>
#include <string>
#include "behaviortree_cpp/action_node.h"
@@ -27,16 +26,16 @@ namespace nav2_tasks
{
template<class ActionT>
-class BtActionNode : public BT::AsyncActionNode
+class BtActionNode : public BT::CoroActionNode
{
public:
explicit BtActionNode(const std::string & action_name)
- : BT::AsyncActionNode(action_name), action_name_(action_name)
+ : BT::CoroActionNode(action_name), action_name_(action_name)
{
}
BtActionNode(const std::string & action_name, const BT::NodeParameters & params)
- : BT::AsyncActionNode(action_name, params), action_name_(action_name)
+ : BT::CoroActionNode(action_name, params), action_name_(action_name)
{
}
@@ -52,7 +51,7 @@ public:
// but override on_init instead.
void onInit() final
{
- node_ = nav2_util::generate_internal_node(action_name_);
+ node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
// Initialize the input and output messages
goal_ = typename ActionT::Goal();
@@ -124,13 +123,7 @@ new_goal_received:
auto future_result = goal_handle_->async_result();
rclcpp::executor::FutureReturnCode rc;
do {
- {
- // Can only have one spin_until_future_complete at a time. Otherwise we get
- // the "already on a executor" error
- std::lock_guard<std::mutex> guard(spin_mutex_);
- rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
- }
-
+ rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
on_loop_timeout();
@@ -140,6 +133,9 @@ new_goal_received:
goal_updated_ = false;
goto new_goal_received;
}
+
+ // Yield to any other CoroActionNodes (coroutines)
+ setStatusRunningAndYield();
}
} while (rc != rclcpp::executor::FutureReturnCode::SUCCESS);
@@ -171,18 +167,15 @@ new_goal_received:
if (status() == BT::NodeStatus::RUNNING) {
action_client_->async_cancel_goal(goal_handle_);
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
- {
- std::lock_guard<std::mutex> guard(spin_mutex_);
- if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
- rclcpp::executor::FutureReturnCode::SUCCESS)
- {
- RCLCPP_ERROR(node_->get_logger(),
- "Failed to cancel action server for %s", action_name_.c_str());
- }
+ if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
+ rclcpp::executor::FutureReturnCode::SUCCESS) {
+ RCLCPP_ERROR(node_->get_logger(),
+ "Failed to cancel action server for %s", action_name_.c_str());
}
}
setStatus(BT::NodeStatus::IDLE);
+ CoroActionNode::halt();
}
protected:
@@ -201,9 +194,6 @@ protected:
// The timeout value while to use in the tick loop while waiting for
// a result from the server
std::chrono::milliseconds node_loop_timeout_;
-
- // A mutex to allow only one spin_until_future_complete at a time
- std::mutex spin_mutex_;
};
} // namespace nav2_tasks
--
GitLab