Added BT layer

CMakeLists.txt

@@ -7,10 +7,11 @@ add_definitions(-std=c++11)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib move_base_msgs tf nav_msgs std_srvs iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib move_base_msgs tf nav_msgs std_srvs iri_base_algorithm behaviortree_cpp_v3 iri_behaviortree)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
+find_package(ZMQ)
 
 find_package(iriutils REQUIRED)
 
@@ -101,8 +102,8 @@ generate_dynamic_reconfigure_options(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES iri_ana_nav_module
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib move_base_msgs tf nav_msgs std_srvs iri_base_algorithm
+ LIBRARIES iri_ana_nav_module iri_ana_nav_module_bt
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib move_base_msgs tf nav_msgs std_srvs iri_base_algorithm behaviortree_cpp_v3 iri_behaviortree
 
 #  DEPENDS system_lib
 )
@@ -121,8 +122,12 @@ include_directories(${iriutils_INCLUDE_DIR})
 add_library(iri_ana_nav_module
   src/iri_ana_nav_module.cpp
 )
+add_library(iri_ana_nav_module_bt
+  src/iri_ana_nav_module_bt.cpp
+)
 
 target_link_libraries(iri_ana_nav_module ${catkin_LIBRARIES})
+target_link_libraries(iri_ana_nav_module_bt ${catkin_LIBRARIES})
 target_link_libraries(iri_ana_nav_module ${iriutils_LIBRARY})
 ##Link to other modules
 ##target_link_libraries(new_module <module path>/lib<module_name>.so)
@@ -131,6 +136,7 @@ target_link_libraries(iri_ana_nav_module ${iriutils_LIBRARY})
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
 add_dependencies(iri_ana_nav_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(iri_ana_nav_module_bt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 add_executable(iri_ana_nav_client src/iri_ana_nav_client_alg.cpp src/iri_ana_nav_client_alg_node.cpp)
 

include/iri_ana_nav_module/iri_ana_nav_module_bt.h

@@ -52,7 +52,7 @@ class CIriAnaNavModuleBT
       */
     CIriAnaNavModuleBT();
 
-    void init(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file);
+    void init(IriBehaviorTreeFactory *factory);
     /**
     * \brief Destructor
     *

package.xml

@@ -60,6 +60,11 @@
   <run_depend>nav_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>std_srvs</run_depend>
+  <run_depend>behaviortree_cpp_v3</run_depend>
+  <run_depend>iri_behaviortree</run_depend>
+
+  <build_depend>behaviortree_cpp_v3</build_depend>
+  <build_depend>iri_behaviortree</build_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->
 

src/iri_ana_nav_module_bt.cpp

@@ -10,7 +10,7 @@ CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
 
 }
 
-void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
+void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
 {
   // definition of ports
   BT::PortsList orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
@@ -591,19 +591,19 @@ BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled()
   return BT::NodeStatus::SUCCESS;
 }
 
-void AnaNavBtAlgNode::tick()
+void CIriAnaNavModuleBT::tick()
 {
   ROS_INFO("-------Tick-------");
   this->status = this->tree.root_node->executeTick();
 }
 
-void AnaNavBtAlgNode::stop()
+void CIriAnaNavModuleBT::stop()
 {
   ROS_INFO("-------STOP-------");
   nav.stop();
 }
 
-void AnaNavBtAlgNode::restart()
+void CIriAnaNavModuleBT::restart()
 {
   if (nav.is_finished())
   {
@@ -624,7 +624,7 @@ void AnaNavBtAlgNode::restart()
   }
 }
 
-void AnaNavBtAlgNode::create_tree(std::string path, std::string tree_xml_file)
+void CIriAnaNavModuleBT::create_tree(std::string path, std::string tree_xml_file)
 {
   this->path=path;
   this->tree_xml_file=tree_xml_file;
@@ -678,7 +678,7 @@ void AnaNavBtAlgNode::create_tree(std::string path, std::string tree_xml_file)
   }
 }
 
-void AnaNavBtAlgNode::enable_cout_logger()
+void CIriAnaNavModuleBT::enable_cout_logger()
 {
   try
   {
@@ -690,7 +690,7 @@ void AnaNavBtAlgNode::enable_cout_logger()
   }
 }
 
-void AnaNavBtAlgNode::enable_minitrace_logger()
+void CIriAnaNavModuleBT::enable_minitrace_logger()
 {
   try
   {
@@ -702,7 +702,7 @@ void AnaNavBtAlgNode::enable_minitrace_logger()
   }
 }
 
-void AnaNavBtAlgNode::enable_file_logger()
+void CIriAnaNavModuleBT::enable_file_logger()
 {
   if (this->logger_file == NULL)
   {
@@ -714,7 +714,7 @@ void AnaNavBtAlgNode::enable_file_logger()
   }
 }
 
-void AnaNavBtAlgNode::enable_zmq_publisher()
+void CIriAnaNavModuleBT::enable_zmq_publisher()
 {
   try
   {
@@ -726,7 +726,7 @@ void AnaNavBtAlgNode::enable_zmq_publisher()
   }
 }
 
-void AnaNavBtAlgNode::enable_print_tree()
+void CIriAnaNavModuleBT::enable_print_tree()
 {
   if (!print_tree_enabled)
   {
@@ -737,30 +737,30 @@ void AnaNavBtAlgNode::enable_print_tree()
     print_tree_enabled = true;
   }}
 
-void AnaNavBtAlgNode::disable_cout_logger()
+void CIriAnaNavModuleBT::disable_cout_logger()
 {
   delete this->logger_cout;
   this->logger_cout = NULL;
 }
 
-void AnaNavBtAlgNode::disable_minitrace_logger()
+void CIriAnaNavModuleBT::disable_minitrace_logger()
 {
   delete this->logger_minitrace;
   this->logger_minitrace = NULL;
 }
 
-void AnaNavBtAlgNode::disable_file_logger()
+void CIriAnaNavModuleBT::disable_file_logger()
 {
   delete this->logger_file;
   this->logger_file = NULL;
 }
 
-void AnaNavBtAlgNode::disable_zmq_publisher()
+void CIriAnaNavModuleBT::disable_zmq_publisher()
 {
   delete this->publisher_zmq;
   this->publisher_zmq = NULL;
 }
-void AnaNavBtAlgNode::disable_print_tree()
+void CIriAnaNavModuleBT::disable_print_tree()
 {
   print_tree_enabled = false;
 }