Skip to content

Commit

Permalink
Add user defined functions for Tree creation and completion (#1)
Browse files Browse the repository at this point in the history
Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
  • Loading branch information
MarqRazz and facontidavide authored May 1, 2024
1 parent 0f6162c commit 6e729cb
Show file tree
Hide file tree
Showing 6 changed files with 149 additions and 22 deletions.
47 changes: 46 additions & 1 deletion action_server_bt/include/action_server_bt/action_server_bt.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
#include <functional>
#include <memory>
#include <thread>
#include <optional>

#include "action_server_bt_msgs/action/action_tree.hpp"
#include "action_server_bt_parameters.hpp"
Expand Down Expand Up @@ -42,6 +43,7 @@ class ActionServerBT
* starts an Action Server that takes requests to execute BehaviorTrees.
*
* @param options rclcpp::NodeOptions to pass to node_ when initializing it.
* after the tree is created, while its running and after it finishes.
*/
explicit ActionServerBT(const rclcpp::NodeOptions& options);

Expand All @@ -51,7 +53,46 @@ class ActionServerBT
*
* @return A shared_ptr to the NodeBaseInterface of node_.
*/
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr nodeBaseInterface();

// name of the tree being executed
const std::string& currentTreeName() const;

// tree being executed, nullptr if it doesn't exist yet.
BT::Tree* currentTree();

// pointer to the global blackboard
BT::Blackboard::Ptr globalBlackboard();

protected:
// To be overridden by the user.
// Callback invoked when the tree is created and before it is executed,
// Can be used to update the blackboard or to attach loggers.
virtual void onTreeCreated(BT::Tree& tree)
{
}

// To be overridden by the user.
// In addition to the built in mechanism to register nodes from plugins,
// you can use this method to register custom nodes into the factory.
virtual void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory)
{
}

// To be overridden by the user.
// Callback invoked after the tickOnce.
// If it returns something different than std::nullopt, the tree execution will
// be halted and the returned value will be the optional NodeStatus.
virtual std::optional<BT::NodeStatus> onLoopAfterTick(BT::NodeStatus status)
{
return std::nullopt;
}

// To be overridden by the user.
// Callback invoked when the tree execution is completed
virtual void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled)
{
}

private:
rclcpp::Node::SharedPtr node_;
Expand All @@ -66,6 +107,10 @@ class ActionServerBT
BT::BehaviorTreeFactory factory_;
std::shared_ptr<BT::Groot2Publisher> groot_publisher_;

std::string current_tree_name_;
std::shared_ptr<BT::Tree> tree_;
BT::Blackboard::Ptr global_blackboard_;

/**
* @brief handle the goal requested: accept or reject. This implementation always accepts.
* @param uuid Goal ID
Expand Down
89 changes: 71 additions & 18 deletions action_server_bt/src/action_server_bt.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,13 @@ namespace action_server_bt
ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options)
: node_{ std::make_shared<rclcpp::Node>("action_server_bt", options) }
{
// parameter setup
param_listener_ = std::make_shared<action_server_bt::ParamListener>(node_);
params_ = param_listener_->get_params();

// create the action server
const auto action_name = params_.action_name;
RCLCPP_INFO(kLogger, "Starting Action Server: %s", action_name.c_str());

action_server_ = rclcpp_action::create_server<ActionTree>(
node_, action_name,
[this](const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const ActionTree::Goal> goal) {
Expand All @@ -40,9 +41,13 @@ ActionServerBT::ActionServerBT(const rclcpp::NodeOptions& options)

// register the users Plugins and BehaviorTree.xml files into the factory
register_behavior_trees(params_, factory_, node_);
registerNodesIntoFactory(factory_);

// No one "own" this blackboard
global_blackboard_ = BT::Blackboard::create();
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::getNodeBaseInterface()
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ActionServerBT::nodeBaseInterface()
{
return node_->get_node_base_interface();
}
Expand Down Expand Up @@ -83,68 +88,116 @@ void ActionServerBT::execute(const std::shared_ptr<GoalHandleActionTree> goal_ha
const auto goal = goal_handle->get_goal();
BT::NodeStatus status = BT::NodeStatus::RUNNING;
auto action_result = std::make_shared<ActionTree::Result>();
rclcpp::WallRate loopRate(std::chrono::milliseconds(static_cast<int>(1000.0 / params_.behavior_tick_frequency)));

// Before executing check if we have new Behaviors or Subtrees to reload
if (param_listener_->is_old(params_))
{
params_ = param_listener_->get_params();
register_behavior_trees(params_, factory_, node_);
registerNodesIntoFactory(factory_);
}

// Loop until something happens with ROS or the node completes
try
{
auto tree = factory_.createTree(goal->target_tree);
// This blackboard will be owned by "MainTree". It parent is global_blackboard_
auto root_blackboard = BT::Blackboard::create(global_blackboard_);

tree_ = std::make_shared<BT::Tree>();
*tree_ = factory_.createTree(goal->target_tree, root_blackboard);
current_tree_name_ = goal->target_tree;

// log to console and Groot
BT::StdCoutLogger logger_cout(tree);
// call user defined function after the tree has been created
onTreeCreated(*tree_);
groot_publisher_.reset();
groot_publisher_ = std::make_shared<BT::Groot2Publisher>(tree, params_.groot2_port);
groot_publisher_ = std::make_shared<BT::Groot2Publisher>(*tree_, params_.groot2_port);

// Loop until the tree is done or a cancel is requested
const auto period = std::chrono::milliseconds(static_cast<int>(1000.0 / params_.behavior_tick_frequency));
auto loop_deadline = std::chrono::steady_clock::now() + period;

// operations to be done if the tree execution is aborted, either by
// cancel_requested_ or by onLoopAfterTick()
auto stop_action = [this, &action_result](BT::NodeStatus status, const std::string& message) {
action_result->node_status = convert_node_status(status);
action_result->error_message = message;
RCLCPP_WARN(kLogger, action_result->error_message.c_str());
tree_->haltTree();
onTreeExecutionCompleted(status, true);
};

while (rclcpp::ok() && status == BT::NodeStatus::RUNNING)
{
if (cancel_requested_)
{
action_result->error_message = "Action Server canceling and halting Behavior Tree";
RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
tree.haltTree();
stop_action(status, "Action Server canceling and halting Behavior Tree");
goal_handle->canceled(action_result);
return;
}

// tick the tree once and publish the action feedback
status = tree.tickExactlyOnce();
status = tree_->tickExactlyOnce();

if (const auto res = onLoopAfterTick(status); res.has_value())
{
stop_action(res.value(), "Action Server aborted by onLoopAfterTick()");
goal_handle->abort(action_result);
return;
}

auto feedback = std::make_shared<ActionTree::Feedback>();
feedback->node_status = convert_node_status(status);
goal_handle->publish_feedback(feedback);

if (!loopRate.sleep())
const auto now = std::chrono::steady_clock::now();
if (now < loop_deadline)
{
RCLCPP_WARN(kLogger, "Behavior Tree tick frequency %d Hz was exceeded!",
static_cast<int>(1.0 / (loopRate.period().count() * 1.0e-9)));
tree_->sleep(loop_deadline - now);
}
loop_deadline += period;
}
}
catch (const std::exception& ex)
{
action_result->error_message = std::string("Behavior Tree exception: ") + ex.what();
action_result->error_message = "Behavior Tree exception: %s", ex.what();
RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
goal_handle->abort(action_result);
return;
}

// The only options for NodeStatus here are SUCCESS or FAILURE
// call user defined execution complete function
onTreeExecutionCompleted(status, false);

// set the node_status result to the action
action_result->node_status = convert_node_status(status);

// return success or aborted for the action result
if (status == BT::NodeStatus::SUCCESS)
{
RCLCPP_INFO(kLogger, "BT finished with status: SUCCESS");
RCLCPP_INFO(kLogger, "BT finished with status: %s", BT::toStr(status).c_str());
goal_handle->succeed(action_result);
}
else
{
action_result->error_message = "Behavior Tree failed during execution with status: FAILURE";
action_result->error_message = "Behavior Tree failed during execution with status: %s", BT::toStr(status);
RCLCPP_ERROR(kLogger, action_result->error_message.c_str());
goal_handle->abort(action_result);
}
}

const std::string& ActionServerBT::currentTreeName() const
{
return current_tree_name_;
}

BT::Tree* ActionServerBT::currentTree()
{
return tree_ ? tree_.get() : nullptr;
}

BT::Blackboard::Ptr ActionServerBT::globalBlackboard()
{
return global_blackboard_;
}

} // namespace action_server_bt
31 changes: 28 additions & 3 deletions action_server_bt/src/action_server_bt_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,19 +13,44 @@

#include <action_server_bt/action_server_bt.hpp>

// Example that shows how to customize ActionServerBT.
// Here, we simply add an extra logger
class MyActionServer : public action_server_bt::ActionServerBT
{
public:
MyActionServer(const rclcpp::NodeOptions& options) : ActionServerBT(options)
{
}

void onTreeCreated(BT::Tree& tree) override
{
logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
}

void onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override
{
// NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree
// at this point
logger_cout_.reset();
}

private:
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
};

int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);

rclcpp::NodeOptions options;
auto node = std::make_shared<action_server_bt::ActionServerBT>(options);
auto action_server = std::make_shared<MyActionServer>(options);

// TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout.
// Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning.
rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
exec.add_node(node->getNodeBaseInterface());
exec.add_node(action_server->nodeBaseInterface());
exec.spin();
exec.remove_node(node->getNodeBaseInterface());
exec.remove_node(action_server->nodeBaseInterface());

rclcpp::shutdown();
}
2 changes: 2 additions & 0 deletions action_server_bt/src/bt_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ action_server_bt_msgs::msg::NodeStatus convert_node_status(BT::NodeStatus& statu
action_status.status = action_server_bt_msgs::msg::NodeStatus::FAILURE;
case BT::NodeStatus::IDLE:
action_status.status = action_server_bt_msgs::msg::NodeStatus::IDLE;
case BT::NodeStatus::SKIPPED:
action_status.status = action_server_bt_msgs::msg::NodeStatus::SKIPPED;
}

return action_status;
Expand Down
1 change: 1 addition & 0 deletions action_server_bt_msgs/action/ActionTree.action
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ string target_tree
---
# Result
string error_message
NodeStatus node_status
---
# Feedback
NodeStatus node_status
1 change: 1 addition & 0 deletions action_server_bt_msgs/msgs/NodeStatus.msg
Original file line number Diff line number Diff line change
Expand Up @@ -3,5 +3,6 @@ uint8 IDLE = 0
uint8 RUNNING = 1
uint8 SUCCESS = 2
uint8 FAILURE = 3
uint8 SKIPPED = 4

uint8 status

0 comments on commit 6e729cb

Please sign in to comment.