diff --git a/CMakeLists.txt b/CMakeLists.txt index e5662c009157fd8bc1833c2a78e2d61d4e0c345e..97303cda18d3cfb637aede8650bbd3bed738c09c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(iri_base_bt_client) -set(CMAKE_BUILD_TYPE "DEBUG") +#set(CMAKE_BUILD_TYPE "DEBUG") + +add_definitions(-std=c++11) ## Add support for C++11, supported in ROS Kinetic and newer # add_definitions(-std=c++11) @@ -13,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure diagnostic_updater + behaviortree_cpp_v3 iri_behaviortree ) @@ -90,7 +93,7 @@ catkin_python_setup() catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater iri_behaviortree + CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree # DEPENDS system_lib DEPENDS Boost ) @@ -101,7 +104,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) +include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) @@ -110,7 +113,7 @@ include_directories( # add_library(foo # src/${PROJECT_NAME}/foo.cpp # ) -add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iri_base_bt_client.h) +add_library(${PROJECT_NAME} include/${PROJECT_NAME}/iri_base_bt_client.h src/iri_bt_basic_nodes.cpp) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) ## Declare a cpp executable diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h new file mode 100644 index 0000000000000000000000000000000000000000..c7b0e57bccabc02e53c7edbb0c405e88d082db86 --- /dev/null +++ b/include/iri_bt_basic_nodes.h @@ -0,0 +1,53 @@ +#ifndef IRI_BT_BASIC_NODES_H +#define IRI_BT_BASIC_NODES_H + +#include "iri_bt_factory.h" + +class IriBTBasicNodes +{ + public: + IriBTBasicNodes(); + + /** + * \brief Register all basic conditions and actions + * + * This function registers all basic conditions and actions needed for + * any generic application. + * + * \param factory (IriBehaviorTreeFactory) + * + */ + void init(IriBehaviorTreeFactory &factory); + + void start_tree(); + void stop_tree(); + private: + // variables to know whether start or stop have been demanded by the user + bool tree_start; + bool tree_stop; + + protected: + /** + * \brief Does nothing + * + * This function does nothing and it is useful for synchronous functions + * in order to wait for the action to finish (checking is_finished()) + * + * \return a BT:NodeStatus::RUNNING always + * + */ + BT::NodeStatus NOP(void); + // functions to check whether start or restart have been demanded by the user + BT::NodeStatus is_not_start_tree(void); + BT::NodeStatus is_not_stop_tree(void); + + // functions set start or restart variables + BT::NodeStatus set_start_tree(void); + BT::NodeStatus set_stop_tree(void); + + // functions reset start or restart variables + BT::NodeStatus reset_start_tree(void); + BT::NodeStatus reset_stop_tree(void); +}; + +#endif diff --git a/package.xml b/package.xml index 9d34ac470caccecd9483487c28f023598e085682..47b5bd93c59beedf574bc1ad1451a5f5e4b9ffe8 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ <depend>roscpp</depend> <depend>dynamic_reconfigure</depend> <depend>diagnostic_updater</depend> + <depend>behaviortree_cpp_v3</depend> <depend>iri_behaviortree</depend> diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dbf66403b68cf8012b83372859d3191205d20765 --- /dev/null +++ b/src/iri_bt_basic_nodes.cpp @@ -0,0 +1,79 @@ +#include "iri_bt_basic_nodes.h" +#include <ros/ros.h> + +IriBTBasicNodes::IriBTBasicNodes() +{ + this->tree_start = false; + this->tree_stop = false; +} + +void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory) +{ + factory.registerIriAsyncAction("NOP", std::bind(&IriBTBasicNodes::NOP, this)); + + factory.registerSimpleCondition("is_not_start_tree", std::bind(&IriBTBasicNodes::is_not_start_tree, this)); + factory.registerSimpleCondition("is_not_stop_tree", std::bind(&IriBTBasicNodes::is_not_stop_tree, this)); + + factory.registerSimpleAction("set_start_tree", std::bind(&IriBTBasicNodes::set_start_tree, this)); + factory.registerSimpleAction("set_stop_tree", std::bind(&IriBTBasicNodes::set_stop_tree, this)); + factory.registerSimpleAction("reset_start_tree", std::bind(&IriBTBasicNodes::reset_start_tree, this)); + factory.registerSimpleAction("reset_stop_tree", std::bind(&IriBTBasicNodes::reset_stop_tree, this)); +} + +BT::NodeStatus IriBTBasicNodes::NOP() +{ + return BT::NodeStatus::RUNNING; +} + +BT::NodeStatus IriBTBasicNodes::is_not_start_tree(void) +{ + if(this->tree_start) + return BT::NodeStatus::FAILURE; + else + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus IriBTBasicNodes::is_not_stop_tree(void) +{ + if(this->tree_stop) + return BT::NodeStatus::FAILURE; + else + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus IriBTBasicNodes::set_start_tree(void) +{ + this->tree_start=true; + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus IriBTBasicNodes::set_stop_tree(void) +{ + this->tree_stop=true; + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus IriBTBasicNodes::reset_start_tree(void) +{ + this->tree_start=false; + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus IriBTBasicNodes::reset_stop_tree(void) +{ + this->tree_stop=false; + return BT::NodeStatus::SUCCESS; +} + +void IriBTBasicNodes::start_tree(void) +{ + ROS_DEBUG("IriBTBasicNodes: start_tree()"); + this->tree_start=true; +} + +void IriBTBasicNodes::stop_tree(void) +{ + ROS_DEBUG("IriBTBasicNodes: stop_tree()"); + this->tree_stop=true; +} +