bt : Create basic excavator behavior tree

ros2_ws/src/work_bt/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.8)
+project(work_bt)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}_interfaces
+  "msg/WorkResult.msg"
+  "action/DoWork.action"
+)
+
+add_executable (excavator_bt src/main.cpp)
+ament_target_dependencies(excavator_bt rclcpp rclcpp_action behaviortree_cpp)
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}_interfaces "rosidl_typesupport_cpp")
+target_link_libraries(excavator_bt "${cpp_typesupport_target}")
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS
+  excavator_bt
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros2_ws/src/work_bt/action/DoWork.action

@@ -0,0 +1,3 @@
+---
+work_bt/WorkResult result
+---

ros2_ws/src/work_bt/config/excavator_bt.xml

@@ -0,0 +1,73 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="ExcavatorBT">
+    <Sequence name="main_sequence">
+      <GoToWorksite name="go_to_worksite" />
+      <Fallback name="arm_sequence_and_recovery">
+        <Sequence name="arm_sequence_with_retry">
+          <RetryUntilSuccessful num_attempts="3" name="assume_ready_pose_with_retry">
+            <AssumeReadyPose />
+          </RetryUntilSuccessful>
+          <RetryUntilSuccessful num_attempts="3" name="excavate_with_retry">
+            <Excavate />
+          </RetryUntilSuccessful>
+        </Sequence>
+        <Sequence name="recovery">
+          <Fallback name="critical_fail_if_recovery_fails">
+            <RetryUntilSuccessful num_attempts="3" name="recovery">
+              <Stow name="recovery_stow" />
+            </RetryUntilSuccessful>
+            <CriticalFail />
+          </Fallback>
+          <Fail />
+        </Sequence>
+      </Fallback>
+      <Fallback name="critical_fail_if_stow_fails">
+        <RetryUntilSuccessful num_attempts="3" name="stow_with_retry">
+          <Stow />
+        </RetryUntilSuccessful>
+        <CriticalFail />
+      </Fallback>
+      <SendSuccess />
+
+      <!-- <Fallback name="assume_ready_pose_with_retry">
+        <AssumeReadyPose name="assume_ready_pose1" />
+        <AssumeReadyPose name="assume_ready_pose2" />
+        <AssumeReadyPose name="assume_ready_pose3" />
+        <Sequence name="recover">
+          <Fallback>
+            <Stow name="recovery_stow1" />
+            <Stow name="recovery_stow2" />
+            <Stow name="recovery_stow3" />
+            <SendCriticalFail name="critical_fail" />
+          </Fallback>
+          <SendFail name="fail" />
+        </Sequence>
+      </Fallback>
+      <Fallback name="excavate_with_retry">
+        <Excavate name="excavate1" />
+        <Excavate name="excavate2" />
+        <Excavate name="excavate3" />
+        <Sequence name="recover">
+          <Fallback>
+            <Stow name="recovery_stow1" />
+            <Stow name="recovery_stow2" />
+            <Stow name="recovery_stow3" />
+            <SendCriticalFail name="critical_fail" />
+          </Fallback>
+          <SendFail name="fail" />
+        </Sequence>
+      </Fallback> -->
+    </Sequence>
+  </BehaviorTree>
+  <BehaviorTree ID="StowBT">
+    <Fallback>
+      <Sequence>
+        <RetryUntilSuccessful num_attempts="3">
+          <Stow />
+        </RetryUntilSuccessful>
+        <SendSuccess />
+      </Sequence>
+      <CriticalFail />
+    </Fallback>
+  </BehaviorTree>
+</root>

ros2_ws/src/work_bt/launch/excavator_bt_launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PathJoinSubstitution, TextSubstitution
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='work_bt',
+            executable='excavator_bt',
+            name='excavator_bt',
+            output='screen',
+            parameters=[
+                {'bt_xml': PathJoinSubstitution([
+                    FindPackageShare('work_bt'),
+                    'config',
+                    'excavator_bt.xml'])}
+            ]
+        )
+    ])

ros2_ws/src/work_bt/msg/WorkResult.msg

@@ -0,0 +1,4 @@
+int32 SUCCESS=0
+int32 FAILURE=1
+int32 CRITICAL_FAILURE=2
+int32 result

ros2_ws/src/work_bt/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>work_bt</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">spaceros-user</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>behaviortree_cpp</depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2_ws/src/work_bt/src/main.cpp

@@ -0,0 +1,165 @@
+#include <functional>
+#include <thread>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+// #include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+
+#include "work_bt/action/do_work.hpp"
+
+using DoWork = work_bt::action::DoWork;
+using namespace std::placeholders;
+
+class ExcavatorWork : public rclcpp::Node {
+public:
+  ExcavatorWork() : Node("excavator_bt") {
+    BT::BehaviorTreeFactory factory;
+    setup(factory);
+    this->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();
+    // work_bt = factory.createTreeFromFile(bt_xml);
+    factory.registerBehaviorTreeFromFile(bt_xml);
+    work_bt = factory.createTree("ExcavatorBT");
+    stow_bt = factory.createTree("StowBT");
+
+    bt_server = rclcpp_action::create_server<DoWork>(
+        this, "work_bt", std::bind(&ExcavatorWork::handle_goal, this, _1, _2),
+        std::bind(&ExcavatorWork::handle_cancel, this, _1),
+        std::bind(&ExcavatorWork::handle_accepted, this, _1)
+    );
+  }
+
+private:
+  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid,
+                   std::shared_ptr<const DoWork::Goal> goal) {
+    if (active_goal_handle) {
+      RCLCPP_WARN(this->get_logger(), "Received work goal but already working");
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    RCLCPP_INFO(this->get_logger(), "Received new work goal");
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> goal_handle) {
+    std::scoped_lock<std::mutex> lock(mtx);
+    RCLCPP_INFO(this->get_logger(), "Received cancel request");
+    if (!active_goal_handle) {
+      RCLCPP_WARN(this->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...");
+      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");
+      return rclcpp_action::CancelResponse::ACCEPT;
+    }
+    assert(active_goal_handle == goal_handle);
+    is_working = false;
+    is_cancelling = true;
+    std::thread(&ExcavatorWork::cancel, this).detach();
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+  void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> goal_handle) {
+    std::scoped_lock<std::mutex> lock(mtx);
+    active_goal_handle = goal_handle;
+    is_working = true;
+    is_cancelling = false;
+    std::thread(&ExcavatorWork::run_work_bt, this).detach();
+  }
+
+  void run_work_bt() {
+    rclcpp::Rate rate(10);
+    while (rclcpp::ok() && work_bt.tickOnce() == BT::NodeStatus::RUNNING) {
+      rate.sleep();
+    }
+  }
+
+  void cancel() {
+    rclcpp::Rate rate(10);
+    while (rclcpp::ok() && stow_bt.tickOnce() == BT::NodeStatus::RUNNING) {
+      rate.sleep();
+    }
+    active_goal_handle->canceled({});
+    active_goal_handle.reset();
+  }
+
+  void send_result(work_bt::msg::WorkResult::_result_type result) {
+    auto msg = std::make_shared<DoWork::Result>();
+    msg->result.result = result;
+    active_goal_handle->succeed(msg);
+    active_goal_handle.reset();
+  }
+
+  rclcpp_action::Server<DoWork>::SharedPtr bt_server;
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> active_goal_handle;
+
+  std::mutex mtx;
+  BT::Tree work_bt;
+  std::atomic_bool is_working;
+  BT::Tree stow_bt;
+  std::atomic_bool is_cancelling;
+
+  /********** BT Functions **********/
+  BT::NodeStatus GoToWorksite(BT::TreeNode &self) {
+    RCLCPP_INFO(this->get_logger(), "GoToWorksite");
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  BT::NodeStatus AssumeReadyPose(BT::TreeNode &self) {
+    RCLCPP_INFO(this->get_logger(), "AssumeReadyPose");
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  BT::NodeStatus Excavate(BT::TreeNode &self) {
+    RCLCPP_INFO(this->get_logger(), "Excavate");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus Stow(BT::TreeNode &self) {
+    RCLCPP_INFO(this->get_logger(), "Stow");
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  BT::NodeStatus SendSuccess(BT::TreeNode &self) {
+    RCLCPP_INFO(this->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");
+    send_result(work_bt::msg::WorkResult::FAILURE);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus CriticalFail(BT::TreeNode &self) {
+    RCLCPP_INFO(this->get_logger(), "CriticalFail");
+    send_result(work_bt::msg::WorkResult::CRITICAL_FAILURE);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  void setup(BT::BehaviorTreeFactory& factory) {
+    factory.registerSimpleAction("GoToWorksite", std::bind(&ExcavatorWork::GoToWorksite, this, _1));
+    factory.registerSimpleAction("AssumeReadyPose", std::bind(&ExcavatorWork::AssumeReadyPose, this, _1));
+    factory.registerSimpleAction("Excavate", std::bind(&ExcavatorWork::Excavate, this, _1));
+    factory.registerSimpleAction("Stow", std::bind(&ExcavatorWork::Stow, this, _1));
+    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));
+  }
+  /********************/
+
+};
+
+int main(int argc, char * argv[]) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ExcavatorWork>());
+  rclcpp::shutdown();
+  return 0;
+
+}