Migrate current nav2 BT wrapper with small tweaks

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.16)
+cmake_minimum_required(VERSION 3.5)
 project(behaviortree_ros2)
 
 set(CMAKE_CXX_STANDARD 17)
@@ -7,74 +7,67 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 ######################################################
 
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-    rclcpp
-    rclcpp_action
-    ament_index_cpp
-    behaviortree_cpp_v3)
-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED )
 find_package(rclcpp_action REQUIRED )
-find_package(behaviortree_cpp_v3 REQUIRED )
+find_package(behaviortree_cpp REQUIRED )
 find_package(ament_index_cpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(Boost COMPONENTS coroutine REQUIRED)
 
-find_package(rosidl_default_generators REQUIRED)
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+    rclcpp
+    rclcpp_action
+    ament_index_cpp
+    behaviortree_cpp
+    geometry_msgs
+    std_msgs
+    Boost)
 
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "action/Sleep.action"
+include_directories(
+  include
 )
 
-
-######################################################
-
-######################################################
-# TESTS
-
-add_executable(sleep_server test/sleep_server.cpp)
-target_include_directories(sleep_server PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-ament_target_dependencies(sleep_server
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+add_library(${PROJECT_NAME} SHARED
+  src/behavior_tree_engine.cpp
 )
-rosidl_target_interfaces(sleep_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
-
-
-add_executable(test_client test/test_client.cpp)
-target_include_directories(test_client PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-ament_target_dependencies(test_client
+ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
-rosidl_target_interfaces(test_client ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
+add_library(pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
+list(APPEND plugin_libs pipeline_sequence_bt_node)
 
-#add_executable(test_server test/test_server.cpp)
-#add_dependencies(test_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-#target_link_libraries(test_server ${catkin_LIBRARIES} )
+add_library(rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp)
+list(APPEND plugin_libs rate_controller_bt_node)
 
-######################################################
-# INSTALL
+foreach(bt_plugin ${plugin_libs})
+  ament_target_dependencies(${bt_plugin} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
 
-install(TARGETS
-  sleep_server
-  test_client
-  RUNTIME DESTINATION bin
+install(TARGETS 
+  ${PROJECT_NAME}
+  ${plugin_libs}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
-  )
-
-install(
-  DIRECTORY include/
-  DESTINATION include
+  RUNTIME DESTINATION bin
 )
 
-######################################################
+install(DIRECTORY include/
+  DESTINATION include/
+)
 
-ament_export_include_directories(include)
+ament_export_include_directories(
+  include
+)
 
-ament_package()
+ament_export_libraries(
+  ${PROJECT_NAME}
+  ${plugin_libs}
+)
 
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+ament_package()

include/behaviortree_ros2/behavior_tree_engine.hpp

@@ -0,0 +1,100 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_
+#define BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/xml_parsing.h"
+#include "behaviortree_cpp/loggers/bt_zmq_publisher.h"
+
+
+namespace BT
+{
+
+/**
+ * @enum nav2_behavior_tree::BtStatus
+ * @brief An enum class representing BT execution status
+ */
+enum class BtStatus { SUCCEEDED, FAILED, CANCELED };
+
+/**
+ * @class nav2_behavior_tree::BehaviorTreeEngine
+ * @brief A class to create and handle behavior trees
+ */
+class BehaviorTreeEngine
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
+   * @param plugin_libraries vector of BT plugin library names to load
+   */
+  explicit BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries);
+  virtual ~BehaviorTreeEngine() {}
+
+  /**
+   * @brief Function to execute a BT at a specific rate
+   * @param tree BT to execute
+   * @param onLoop Function to execute on each iteration of BT execution
+   * @param cancelRequested Function to check if cancel was requested during BT execution
+   * @param loopTimeout Time period for each iteration of BT execution
+   * @return nav2_behavior_tree::BtStatus Status of BT execution
+   */
+  BtStatus run(
+    Tree * tree,
+    std::function<void()> onLoop,
+    std::function<bool()> cancelRequested,
+    std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
+
+  /**
+   * @brief Function to create a BT from a XML string
+   * @param xml_string XML string representing BT
+   * @param blackboard Blackboard for BT
+   * @return Tree Created behavior tree
+   */
+  Tree createTreeFromText(
+    const std::string & xml_string,
+    Blackboard::Ptr blackboard);
+
+  /**
+   * @brief Function to create a BT from an XML file
+   * @param file_path Path to BT XML file
+   * @param blackboard Blackboard for BT
+   * @return Tree Created behavior tree
+   */
+  Tree createTreeFromFile(
+    const std::string & file_path,
+    Blackboard::Ptr blackboard);
+
+  /**
+   * @brief Function to explicitly reset all BT nodes to initial state
+   * @param root_node Pointer to BT root node
+   */
+  void haltAllActions(TreeNode * root_node);
+
+protected:
+  // The factory that will be used to dynamically construct the behavior tree
+  BehaviorTreeFactory factory_;
+  std::unique_ptr<PublisherZMQ> groot_monitor_;
+};
+
+}  // namespace BT
+
+#endif // BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_