From 2ae9e89870da2181d84c431370c22c27c1806459 Mon Sep 17 00:00:00 2001 From: Rufus Wong Date: Sat, 25 Jan 2025 01:21:39 -0500 Subject: [PATCH] bt : Use RosActionNode instead and split out SleepBtNode --- ros2_ws/src/work_bt/CMakeLists.txt | 7 +- ros2_ws/src/work_bt/action/EmptyAction.action | 2 + ros2_ws/src/work_bt/package.xml | 2 + .../src/work_bt/src/action_client_bt_node.hpp | 89 -------------- ros2_ws/src/work_bt/src/main.cpp | 109 +++++++++--------- .../work_bt/src/simple_ros_action_node.hpp | 30 +++++ ros2_ws/src/work_bt/src/sleep_bt_node.hpp | 39 +++++++ 7 files changed, 132 insertions(+), 146 deletions(-) create mode 100644 ros2_ws/src/work_bt/action/EmptyAction.action delete mode 100644 ros2_ws/src/work_bt/src/action_client_bt_node.hpp create mode 100644 ros2_ws/src/work_bt/src/simple_ros_action_node.hpp create mode 100644 ros2_ws/src/work_bt/src/sleep_bt_node.hpp diff --git a/ros2_ws/src/work_bt/CMakeLists.txt b/ros2_ws/src/work_bt/CMakeLists.txt index f072bcc..fae37dc 100644 --- a/ros2_ws/src/work_bt/CMakeLists.txt +++ b/ros2_ws/src/work_bt/CMakeLists.txt @@ -11,17 +11,22 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) find_package(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rover_interfaces REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME}_interfaces "msg/WorkResult.msg" "action/DoWork.action" + "action/EmptyAction.action" + DEPENDENCIES std_msgs ) add_executable (excavator_bt src/main.cpp) -ament_target_dependencies(excavator_bt rclcpp rclcpp_action behaviortree_cpp) +ament_target_dependencies(excavator_bt rclcpp rclcpp_action behaviortree_cpp behaviortree_ros2 rover_interfaces) rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}_interfaces "rosidl_typesupport_cpp") target_link_libraries(excavator_bt "${cpp_typesupport_target}") diff --git a/ros2_ws/src/work_bt/action/EmptyAction.action b/ros2_ws/src/work_bt/action/EmptyAction.action new file mode 100644 index 0000000..a845151 --- /dev/null +++ b/ros2_ws/src/work_bt/action/EmptyAction.action @@ -0,0 +1,2 @@ +--- +--- diff --git a/ros2_ws/src/work_bt/package.xml b/ros2_ws/src/work_bt/package.xml index fb21e26..9697ec4 100644 --- a/ros2_ws/src/work_bt/package.xml +++ b/ros2_ws/src/work_bt/package.xml @@ -10,6 +10,8 @@ rclcpp rclcpp_action behaviortree_cpp + behaviortree_ros2 + rover_interfaces rosidl_default_generators rosidl_default_runtime diff --git a/ros2_ws/src/work_bt/src/action_client_bt_node.hpp b/ros2_ws/src/work_bt/src/action_client_bt_node.hpp deleted file mode 100644 index a5420af..0000000 --- a/ros2_ws/src/work_bt/src/action_client_bt_node.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once -#include - -#include -#include - -#include "behaviortree_cpp/behavior_tree.h" -#include "work_bt/action/bt_action_client.hpp" - -template -class ActionClientBtNode : public BT::StatefulActionNode { - -public: - ActionClientBtNode(const std::string& name, const BT::NodeConfig& config, rclcpp::Node& node, std::string action_server_name, std::function goal_generator) - : BT::StatefulActionNode(name, config), node(node), goal_generator(goal_generator) { - action_client = rclcpp_action::create_client(&node, action_server_name); - while (rclcpp::ok() && !action_client->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_WARN(node.get_logger(), "Waiting for %s to appear...", action_server_name.c_str()); - } - } - - BT::NodeStatus onStart() override { - is_done = false; - auto goal_msg = ActionType::Goal(); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = std::bind(&ActionClientBtNode::goal_response_callback, this, std::placeholders::_1); - send_goal_options.feedback_callback = - std::bind(&ActionClientBtNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options.result_callback = - std::bind(&ActionClientBtNode::result_callback, this, std::placeholders::_1); - action_client->async_send_goal(goal_msg, send_goal_options); - return BT::NodeStatus::RUNNING; - } - - BT::NodeStatus onRunning() override { - if (is_done) { - return BT::NodeStatus::SUCCESS; - } else { - return BT::NodeStatus::RUNNING; - } - - } - - void onHalted() override {} - - /* Required implementation */ - static BT::PortsList providedPorts() - { - return {}; - } - -private: - void goal_response_callback(const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(node.get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_INFO(node.get_logger(), "Goal accepted by server, waiting for result"); - } - } - - void feedback_callback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { - //TODO - } - - void result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult &result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(node.get_logger(), "Goal succeeded"); - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(node.get_logger(), "Goal was aborted"); - break; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(node.get_logger(), "Goal was canceled"); - break; - default: - RCLCPP_ERROR(node.get_logger(), "Unknown result code"); - break; - } - is_done = true; - } - - rclcpp::Node& node; - rclcpp_action::Client::SharedPtr action_client; - std::atomic_bool is_done; - std::function goal_generator; -}; diff --git a/ros2_ws/src/work_bt/src/main.cpp b/ros2_ws/src/work_bt/src/main.cpp index 97a4632..11f0e08 100644 --- a/ros2_ws/src/work_bt/src/main.cpp +++ b/ros2_ws/src/work_bt/src/main.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -7,86 +8,68 @@ // #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" +#include "rover_interfaces/action/navigate_to_position.hpp" #include "work_bt/action/do_work.hpp" +#include "work_bt/action/empty_action.hpp" + +#include "sleep_bt_node.hpp" +#include "simple_ros_action_node.hpp" using DoWork = work_bt::action::DoWork; +using EmptyAction = work_bt::action::EmptyAction; +using NavigateToPosition = rover_interfaces::action::NavigateToPosition; using namespace std::placeholders; using namespace std::chrono_literals; -class SleepBtNode : public BT::StatefulActionNode { -public: - SleepBtNode(const std::string& name, const BT::NodeConfig& config, rclcpp::Node& node, std::chrono::milliseconds duration) - : BT::StatefulActionNode(name, config), node(node), duration(duration) { - } - - BT::NodeStatus onStart() override { - start_time = std::chrono::system_clock::now(); - return BT::NodeStatus::RUNNING; - } - - BT::NodeStatus onRunning() override { - RCLCPP_INFO_THROTTLE(node.get_logger(), *node.get_clock(), 500, "Running %s", this->name().c_str()); - if (std::chrono::system_clock::now() - start_time > duration) { - return BT::NodeStatus::SUCCESS; - } else { - return BT::NodeStatus::RUNNING; - } - } - - void onHalted() override {} - - /* Required implementation */ - static BT::PortsList providedPorts() - { - return {}; - } - -private: - rclcpp::Node& node; - std::chrono::system_clock::time_point start_time; - std::chrono::milliseconds duration; -}; - -class ExcavatorWork : public rclcpp::Node { +/** + * @brief We do not use TreeExecutionServer because we have complex cancellation logic. + * We follow TreeExecutionServer in keeping a member Node instead of inheriting from Node + * + */ +class ExcavatorWork { public: - ExcavatorWork() : Node("excavator_bt") { + ExcavatorWork() : node(std::make_shared("excavator_bt")) { setup(factory); - this->declare_parameter("bt_xml", std::string("my_tree.xml")); + node->declare_parameter("bt_xml", std::string("my_tree.xml")); is_working = false; is_cancelling = false; - std::string bt_xml = this->get_parameter("bt_xml").as_string(); + std::string bt_xml = node->get_parameter("bt_xml").as_string(); factory.registerBehaviorTreeFromFile(bt_xml); bt_server = rclcpp_action::create_server( - this, "do_work", std::bind(&ExcavatorWork::handle_goal, this, _1, _2), + node, "do_work", std::bind(&ExcavatorWork::handle_goal, this, _1, _2), std::bind(&ExcavatorWork::handle_cancel, this, _1), std::bind(&ExcavatorWork::handle_accepted, this, _1) ); } + std::shared_ptr get_node() { + return node; + } + private: rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { if (active_goal_handle) { - RCLCPP_WARN(this->get_logger(), "Received work goal but already working"); + RCLCPP_WARN(node->get_logger(), "Received work goal but already working"); return rclcpp_action::GoalResponse::REJECT; } - RCLCPP_INFO(this->get_logger(), "Received new work goal"); + RCLCPP_INFO(node->get_logger(), "Received new work goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr> goal_handle) { std::scoped_lock lock(mtx); - RCLCPP_INFO(this->get_logger(), "Received cancel request"); + RCLCPP_INFO(node->get_logger(), "Received cancel request"); if (!active_goal_handle) { - RCLCPP_WARN(this->get_logger(), "No active goal, rejecting cancel request"); + RCLCPP_WARN(node->get_logger(), "No active goal, rejecting cancel request"); return rclcpp_action::CancelResponse::REJECT; } else if (active_goal_handle != goal_handle) { - RCLCPP_ERROR(this->get_logger(), "Received cancel request for different goal, this shouldn't happen..."); + RCLCPP_ERROR(node->get_logger(), "Received cancel request for different goal, this shouldn't happen..."); return rclcpp_action::CancelResponse::REJECT; } else if (is_cancelling) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Already cancelling, accepting duplicate cancel request"); + RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Already cancelling, accepting duplicate cancel request"); return rclcpp_action::CancelResponse::ACCEPT; } assert(active_goal_handle == goal_handle); @@ -94,8 +77,9 @@ class ExcavatorWork : public rclcpp::Node { std::thread(&ExcavatorWork::cancel, this).detach(); return rclcpp_action::CancelResponse::ACCEPT; } + void handle_accepted(const std::shared_ptr> goal_handle) { - RCLCPP_INFO(this->get_logger(), "Start executing goal..."); + RCLCPP_INFO(node->get_logger(), "Start executing goal..."); std::scoped_lock lock(mtx); active_goal_handle = goal_handle; is_working = true; @@ -115,7 +99,7 @@ class ExcavatorWork : public rclcpp::Node { work_bt.haltTree(); return; } - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Ticking work_bt"); + RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Ticking work_bt"); if (work_bt.tickOnce() != BT::NodeStatus::RUNNING) { return; } @@ -134,7 +118,7 @@ class ExcavatorWork : public rclcpp::Node { while (rclcpp::ok()) { { std::scoped_lock lock(mtx); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Ticking stow_bt"); + RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Ticking stow_bt"); if (stow_bt.tickOnce() != BT::NodeStatus::RUNNING) { is_cancelling = false; /* A successful cancel is treated as a succeeded goal */ @@ -153,6 +137,7 @@ class ExcavatorWork : public rclcpp::Node { active_goal_handle.reset(); } + std::shared_ptr node; rclcpp_action::Server::SharedPtr bt_server; std::shared_ptr> active_goal_handle; @@ -162,28 +147,39 @@ class ExcavatorWork : public rclcpp::Node { /********** BT Functions **********/ BT::NodeStatus SendSuccess(BT::TreeNode &self) { - RCLCPP_INFO(this->get_logger(), "SendSuccess"); + RCLCPP_INFO(node->get_logger(), "SendSuccess"); send_result(work_bt::msg::WorkResult::SUCCESS); return BT::NodeStatus::SUCCESS; } BT::NodeStatus Fail(BT::TreeNode &self) { - RCLCPP_INFO(this->get_logger(), "Fail"); + RCLCPP_INFO(node->get_logger(), "Fail"); send_result(work_bt::msg::WorkResult::FAILURE); return BT::NodeStatus::FAILURE; } BT::NodeStatus CriticalFail(BT::TreeNode &self) { - RCLCPP_INFO(this->get_logger(), "CriticalFail"); + RCLCPP_INFO(node->get_logger(), "CriticalFail"); send_result(work_bt::msg::WorkResult::CRITICAL_FAILURE); return BT::NodeStatus::FAILURE; } void setup(BT::BehaviorTreeFactory& factory) { - factory.registerNodeType("GoToWorksite", std::ref(*this), 5000ms); - factory.registerNodeType("AssumeReadyPose", std::ref(*this), 2000ms); - factory.registerNodeType("Excavate", std::ref(*this), 10000ms); - factory.registerNodeType("Stow", std::ref(*this), 2000ms); + factory.registerNodeType>("GoToWorksite", node, "navigate_to_position", []() { + auto goal = NavigateToPosition::Goal(); + goal.target.position.latitude = 80.0; + goal.target.position.longitude = 80.0; + return goal; + }); + factory.registerNodeType>("AssumeReadyPose", node, "assume_ready_pose", []() { + return EmptyAction::Goal(); + }); + factory.registerNodeType>("Excavate", node, "excavate", []() { + return EmptyAction::Goal(); + }); + factory.registerNodeType>("Stow", node, "stow", []() { + return EmptyAction::Goal(); + }); factory.registerSimpleAction("SendSuccess", std::bind(&ExcavatorWork::SendSuccess, this, _1)); factory.registerSimpleAction("Fail", std::bind(&ExcavatorWork::Fail, this, _1)); factory.registerSimpleAction("CriticalFail", std::bind(&ExcavatorWork::CriticalFail, this, _1)); @@ -196,7 +192,8 @@ class ExcavatorWork : public rclcpp::Node { int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto excavator_work = std::make_shared(); + rclcpp::spin(excavator_work->get_node()); rclcpp::shutdown(); return 0; diff --git a/ros2_ws/src/work_bt/src/simple_ros_action_node.hpp b/ros2_ws/src/work_bt/src/simple_ros_action_node.hpp new file mode 100644 index 0000000..2442ead --- /dev/null +++ b/ros2_ws/src/work_bt/src/simple_ros_action_node.hpp @@ -0,0 +1,30 @@ +#pragma once +#include + +#include +#include + +#include "behaviortree_ros2/bt_action_node.hpp" +#include "work_bt/action/bt_action_client.hpp" + +template +class SimpleRosActionNode : public BT::RosActionNode { +using RosActionNode = BT::RosActionNode; +public: + SimpleRosActionNode(const std::string& name, const BT::NodeConfig& config, std::shared_ptr node, std::string action_server_name, std::function goal_generator) + : RosActionNode(name, config, BT::RosNodeParams(node, action_server_name)), node(node), goal_generator(goal_generator) { + } + + bool setGoal(typename RosActionNode::Goal& goal) override { + goal = goal_generator(); + return true; + } + + BT::NodeStatus onResultReceived(const typename RosActionNode::WrappedResult &result) override { + return BT::NodeStatus::SUCCESS; + } + +private: + std::shared_ptr node; + std::function goal_generator; +}; diff --git a/ros2_ws/src/work_bt/src/sleep_bt_node.hpp b/ros2_ws/src/work_bt/src/sleep_bt_node.hpp new file mode 100644 index 0000000..582dcff --- /dev/null +++ b/ros2_ws/src/work_bt/src/sleep_bt_node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include "behaviortree_cpp/behavior_tree.h" + +class SleepBtNode : public BT::StatefulActionNode { +public: + SleepBtNode(const std::string& name, const BT::NodeConfig& config, rclcpp::Node& node, std::chrono::milliseconds duration) + : BT::StatefulActionNode(name, config), node(node), duration(duration) { + } + + BT::NodeStatus onStart() override { + start_time = std::chrono::system_clock::now(); + return BT::NodeStatus::RUNNING; + } + + BT::NodeStatus onRunning() override { + RCLCPP_INFO_THROTTLE(node.get_logger(), *node.get_clock(), 500, "Running %s", this->name().c_str()); + if (std::chrono::system_clock::now() - start_time > duration) { + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::RUNNING; + } + } + + void onHalted() override {} + + /* Required implementation */ + static BT::PortsList providedPorts() + { + return {}; + } + +private: + rclcpp::Node& node; + std::chrono::system_clock::time_point start_time; + std::chrono::milliseconds duration; +}; +