Fix the BT

Author: LuisLechugaRuiz

src/task_behavior_tree/CMakeLists.txt

@@ -9,6 +9,7 @@ endif()
 find_package(behaviortree_cpp_v3 REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(task_msgs REQUIRED)
 
@@ -32,6 +33,7 @@ set(dependencies
   behaviortree_cpp_v3
   geometry_msgs
   rclcpp
+  rclcpp_action
   rclcpp_lifecycle
   task_msgs
 )
@@ -46,24 +48,31 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-add_library(task_initialize_navigation_action_bt_node SHARED plugins/src/action/initialize_navigation.cpp)
+add_library(task_initialize_navigation_action_bt_node SHARED plugins/action/initialize_navigation.cpp)
 list(APPEND plugin_libs task_initialize_navigation_action_bt_node)
 
 foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${dependencies})
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
 endforeach()
 
-install(TARGETS ${plugin_libs}
+install(TARGETS ${library_name}
+                ${plugin_libs}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
 install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -77,6 +86,7 @@ ament_export_include_directories(
 )
 
 ament_export_libraries(
+  ${library_name}
   ${plugin_libs}
 )
 

src/task_behavior_tree/behavior_trees/navigate_and_find.xml

@@ -0,0 +1,10 @@
+<!--
+  This Behavior Tree initialize navigation at the moment, but will navigate and find an object
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">
+          <InitializeNavigation timeout_s="{timeout_s}"/>
+      </Sequence>
+  </BehaviorTree>
+</root>

src/task_behavior_tree/include/action/initialize_navigation.hpp

@@ -40,8 +40,7 @@ class InitializeNavigation : public nav2_behavior_tree::BtActionNode<task_msgs::
   {
     return providedBasicPorts(
       {
-        BT::InputPort<geometry_msgs::msg::PoseStamped>("pose", "Initial pose"),
-        BT::InputPort<double>("timeout_s", "Timeout in seconds"),
+        BT::InputPort<double>("timeout_s", 10.0, "Timeout in seconds"),
       });
   }
 };

src/task_behavior_tree/include/bt_navigate_and_find.hpp

@@ -7,17 +7,13 @@
 #include "behaviortree_cpp_v3/xml_parsing.h"
 #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace bt_navigate_and_find {
-
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-
 /**
  * @class bt_navigate_and_find::BtNavigateAndFind
  * @brief A bt class which navigates and tries to find and specific object
  */
-class BtNavigateAndFind : public rclcpp_lifecycle::LifecycleNode {
+class BtNavigateAndFind : public rclcpp::Node {
 public:
   /**
    * @brief Constructor
@@ -30,38 +26,14 @@ class BtNavigateAndFind : public rclcpp_lifecycle::LifecycleNode {
 
 protected:
   /**
-   * @brief Configures member variables
-   *
-   * Builds behavior tree from xml file.
-   * @param state Reference to LifeCycle node state
-   * @return SUCCESS or FAILURE
+   * @brief Configures the behavior tree
+   * @return bool with the status
    */
-  CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override;
+  bool ConfigureBT();
   /**
-   * @brief Activates action server
-   * @param state Reference to LifeCycle node state
-   * @return SUCCESS or FAILURE
+   * @brief Run the behavior tree
    */
-  CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override;
-  /**
-   * @brief Deactivates action server
-   * @param state Reference to LifeCycle node state
-   * @return SUCCESS or FAILURE
-   */
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override;
-  /**
-   * @brief Resets member variables
-   * @param state Reference to LifeCycle node state
-   * @return SUCCESS or FAILURE
-   */
-  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override;
-  /**
-   * @brief Called when in shutdown state
-   * @param state Reference to LifeCycle node state
-   * @return SUCCESS or FAILURE
-   */
-  CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override;
-
+  void RunBT();
   /**
    * @brief Load the desired behavior tree
    * @param bt_xml_filename The file containing the new BT
@@ -75,6 +47,8 @@ class BtNavigateAndFind : public rclcpp_lifecycle::LifecycleNode {
   BT::BehaviorTreeFactory factory_;
   // The tree of the BT
   BT::Tree tree_;
+  // A regular, non-spinning ROS node that we can use for calls to the action client
+  rclcpp::Node::SharedPtr client_node_;
   // The XML file that cointains the Behavior Tree to create
   std::string default_bt_xml_filename_;
   // Libraries to pull plugins (BT Nodes) from

src/task_behavior_tree/package.xml

@@ -12,12 +12,14 @@
   <build_depend>behaviortree_cpp_v3</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>task_msgs</build_depend>
 
   <exec_depend>behaviortree_cpp_v3</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
   <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>task_msgs</exec_depend>
 

src/task_behavior_tree/plugins/src/action/initialize_navigation.cpp renamed to src/task_behavior_tree/plugins/action/initialize_navigation.cpp

@@ -17,12 +17,6 @@ InitializeNavigation::InitializeNavigation(
 void InitializeNavigation::on_tick()
 {
   // TODO: Make a template to check that we get the goal_ parameters correctly
-  if (!getInput("pose", goal_.pose)) {
-    RCLCPP_ERROR(
-      node_->get_logger(),
-      "InitializeNavigation: initial pose not provided");
-    return;
-  }
   if (!getInput("timeout_s", goal_.timeout_s)) {
     RCLCPP_ERROR(
       node_->get_logger(),

src/task_behavior_tree/src/bt_navigate_and_find.cpp

@@ -11,44 +11,43 @@
 
 #include "behaviortree_cpp_v3/utils/shared_library.h"
 
+#define LOG(level, ...) RCLCPP_##level(this->get_logger(), __VA_ARGS__)
+
 namespace bt_navigate_and_find {
 
-BtNavigateAndFind::BtNavigateAndFind()
-: rclcpp_lifecycle::LifecycleNode("bt_navigate_and_find") {
-  RCLCPP_INFO(get_logger(), "Constructor");
+BtNavigateAndFind::BtNavigateAndFind() : rclcpp::Node("bt_navigate_and_find") {
+  LOG(INFO, "Constructor");
 
   const std::vector<std::string> plugin_libs = {
+    "nav2_pipeline_sequence_bt_node",
     "task_initialize_navigation_action_bt_node",
   };
 
   // Declare this node's parameters
   declare_parameter("default_bt_xml_filename");
   declare_parameter("plugin_lib_names", plugin_libs);
 
-  // TODO: Make this an util later
-  // How to construct a nav2::SimpleActionServer
-  // initialize_navigation_action_server_ = std::make_unique<T>(
-  // get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(),
-  // get_node_waitables_interface(), name, std::bind(&Class::Member, this));
+  if (ConfigureBT()) {
+    LOG(INFO, "BT configured, starting the run.");
+    RunBT();
+  } else {
+    LOG(ERROR, "Fail to configure BT.");
+  }
 }
 
-BtNavigateAndFind::~BtNavigateAndFind()
-{
-  RCLCPP_INFO(get_logger(), "Destructor");
+BtNavigateAndFind::~BtNavigateAndFind() {
+  LOG(INFO, "Destructor");
 }
 
-CallbackReturn BtNavigateAndFind::on_configure(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Configuring");
+bool BtNavigateAndFind::ConfigureBT() {
+  LOG(INFO, "Configuring");
+
+  // Support for handling the topic-based goal pose from rviz
+  client_node_ = std::make_shared<rclcpp::Node>("_");
 
   // Get the BT filename to use from the node parameter
   get_parameter("default_bt_xml_filename", default_bt_xml_filename_);
 
-  if (!loadBehaviorTree(default_bt_xml_filename_)) {
-    RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str());
-    return CallbackReturn::FAILURE;
-  }
-
   // Register the plugins at the factory
   auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();
   BT::SharedLibrary loader;
@@ -58,53 +57,36 @@ CallbackReturn BtNavigateAndFind::on_configure(const rclcpp_lifecycle::State & /
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create();
-
   // Put items on the blackboard
-  // Example:
-  // blackboard_->set<int>("number_recoveries", 0);
+  blackboard_->set<double>("timeout_s", 10.0);
+  blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);
+  blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(1000));
+
+  // Load the behavior tree
+  if (!loadBehaviorTree(default_bt_xml_filename_)) {
+    LOG(ERROR, "Error loading XML file: %s", default_bt_xml_filename_.c_str());
+    return false;
+  }
 
-  return CallbackReturn::SUCCESS;
+  return true;
 }
 
-CallbackReturn BtNavigateAndFind::on_activate(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Activating");
+void BtNavigateAndFind::RunBT() {
+  LOG(INFO, "Activating");
   rclcpp::WallRate loopRate(std::chrono::milliseconds(10)); // TODO: Make it a cfg
   BT::NodeStatus result = BT::NodeStatus::RUNNING;
   // Loop until something happens with ROS or the node completes
   while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
     result = tree_.tickRoot();
     loopRate.sleep();
   }
-
-  return (result == BT::NodeStatus::SUCCESS) ? CallbackReturn::SUCCESS : CallbackReturn::FAILURE;
-}
-
-CallbackReturn BtNavigateAndFind::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Deactivating");
-  return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn BtNavigateAndFind::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Shutting down");
-  return CallbackReturn::SUCCESS;
-}
-
-CallbackReturn BtNavigateAndFind::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Cleaning up");
-  return CallbackReturn::SUCCESS;
-}
-
-
-bool BtNavigateAndFind::loadBehaviorTree(const std::string & bt_xml_filename)
-{
+bool BtNavigateAndFind::loadBehaviorTree(const std::string & bt_xml_filename) {
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(bt_xml_filename);
   if (!xml_file.good()) {
-    RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str());
+    LOG(ERROR, "Couldn't open input XML file: %s", bt_xml_filename.c_str());
     return false;
   }
   auto xml_string = std::string(

src/task_monitor/include/task_monitor.hpp

@@ -1,12 +1,15 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+#include "lifecycle_msgs/srv/get_state.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "task_msgs/action/initialize_navigation.hpp"
 
 namespace brain {
+using namespace std::chrono_literals;
 
-class TaskMonitor {
+class TaskMonitor : public rclcpp::Node {
   public:
     /**
      * @brief Default constructor
@@ -16,9 +19,6 @@ class TaskMonitor {
      * @brief Default destructor
      */
     ~TaskMonitor();
-
-    // The local node
-    rclcpp::Node::SharedPtr rclcpp_node_; // TODO: Make it private and access a method from main
 
   private:
     /**
@@ -54,6 +54,9 @@ class TaskMonitor {
     using InitializeNavigationAction = task_msgs::action::InitializeNavigation;
     using InitializeNavigationActionServer = nav2_util::SimpleActionServer<InitializeNavigationAction>;
     std::unique_ptr<InitializeNavigationActionServer> initialize_navigation_action_server_;
+
+    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_bt_state_;
+    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_bt_state_;
 };
 
 } // namespace brain

src/task_monitor/src/main.cpp

@@ -7,7 +7,7 @@ int main(int argc, char * argv[])
   // Need to wait some time for the publisher to stablish connection, bug ?
   std::chrono::seconds sleep_time_sec(1);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(sleep_time_sec));
-  rclcpp::spin(task_monitor_node->rclcpp_node_);
+  rclcpp::spin(task_monitor_node->get_node_base_interface());
   rclcpp::shutdown();
 
   return 0;