diff --git a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h index a0f08a45ad04bab2cad27b3ddfd4a2981735566d..bd2c57ae5216daac6d90f62f1b146600bb548113 100644 --- a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h +++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h @@ -24,9 +24,6 @@ class CIriAnaNavModuleBT // tree object to save the behavior tree created and execute its tick() BT::Tree tree; - // factory object to register actions, conditions, ... - IriBehaviorTreeFactory factory; - // loggers BT::StdCoutLogger *logger_cout; BT::MinitraceLogger *logger_minitrace; @@ -530,7 +527,7 @@ class CIriAnaNavModuleBT */ void restart(); - void create_tree(std::string path, std::string tree_xml_file); + void create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file); void enable_cout_logger(); diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp index d6db9453a1725d5add987c552d320b9a18b27037..8022d1e13aedf3e931975fe555f65c85f8cfd1c6 100644 --- a/src/iri_ana_nav_module_bt.cpp +++ b/src/iri_ana_nav_module_bt.cpp @@ -21,39 +21,39 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory) BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") }; // registry of conditions - this->factory.registerSimpleCondition("is_finished", std::bind(&CIriAnaNavModuleBT::is_finished, this)); - this->factory.registerSimpleCondition("is_succeded", std::bind(&CIriAnaNavModuleBT::is_succeded, this)); - this->factory.registerSimpleCondition("is_running", std::bind(&CIriAnaNavModuleBT::is_running, this)); - this->factory.registerSimpleCondition("is_action_start_failed", std::bind(&CIriAnaNavModuleBT::is_action_start_failed, this)); - this->factory.registerSimpleCondition("is_timeout", std::bind(&CIriAnaNavModuleBT::is_timeout, this)); - this->factory.registerSimpleCondition("is_watchdog", std::bind(&CIriAnaNavModuleBT::is_watchdog, this)); - this->factory.registerSimpleCondition("is_aborted", std::bind(&CIriAnaNavModuleBT::is_aborted, this)); - this->factory.registerSimpleCondition("is_preempted", std::bind(&CIriAnaNavModuleBT::is_preempted, this)); - this->factory.registerSimpleCondition("is_rejected", std::bind(&CIriAnaNavModuleBT::is_rejected, this)); - this->factory.registerSimpleCondition("is_set_param_failed", std::bind(&CIriAnaNavModuleBT::is_set_param_failed, this)); - this->factory.registerSimpleCondition("is_param_not_present", std::bind(&CIriAnaNavModuleBT::is_param_not_present, this)); - this->factory.registerSimpleCondition("is_no_odom", std::bind(&CIriAnaNavModuleBT::is_no_odom, this)); - this->factory.registerSimpleCondition("is_no_transform", std::bind(&CIriAnaNavModuleBT::is_no_transform, this)); - this->factory.registerSimpleCondition("costmap_is_auto_clear_enabled", std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled, this)); + factory->registerSimpleCondition("is_finished", std::bind(&CIriAnaNavModuleBT::is_finished, this)); + factory->registerSimpleCondition("is_succeded", std::bind(&CIriAnaNavModuleBT::is_succeded, this)); + factory->registerSimpleCondition("is_running", std::bind(&CIriAnaNavModuleBT::is_running, this)); + factory->registerSimpleCondition("is_action_start_failed", std::bind(&CIriAnaNavModuleBT::is_action_start_failed, this)); + factory->registerSimpleCondition("is_timeout", std::bind(&CIriAnaNavModuleBT::is_timeout, this)); + factory->registerSimpleCondition("is_watchdog", std::bind(&CIriAnaNavModuleBT::is_watchdog, this)); + factory->registerSimpleCondition("is_aborted", std::bind(&CIriAnaNavModuleBT::is_aborted, this)); + factory->registerSimpleCondition("is_preempted", std::bind(&CIriAnaNavModuleBT::is_preempted, this)); + factory->registerSimpleCondition("is_rejected", std::bind(&CIriAnaNavModuleBT::is_rejected, this)); + factory->registerSimpleCondition("is_set_param_failed", std::bind(&CIriAnaNavModuleBT::is_set_param_failed, this)); + factory->registerSimpleCondition("is_param_not_present", std::bind(&CIriAnaNavModuleBT::is_param_not_present, this)); + factory->registerSimpleCondition("is_no_odom", std::bind(&CIriAnaNavModuleBT::is_no_odom, this)); + factory->registerSimpleCondition("is_no_transform", std::bind(&CIriAnaNavModuleBT::is_no_transform, this)); + factory->registerSimpleCondition("costmap_is_auto_clear_enabled", std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled, this)); // registry of synchronous actions - this->factory.registerSimpleAction("sync_go_to_orientation", std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), orientation_port); - this->factory.registerSimpleAction("sync_go_to_position", std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), position_port); - this->factory.registerSimpleAction("sync_go_to_pose", std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), pose_port); - this->factory.registerSimpleAction("sync_stop", std::bind(&CIriAnaNavModuleBT::sync_stop, this)); - this->factory.registerSimpleAction("set_goal_frame", std::bind(&CIriAnaNavModuleBT::set_goal_frame, this, std::placeholders::_1), frame_port); - this->factory.registerSimpleAction("get_current_pose", std::bind(&CIriAnaNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_port); - this->factory.registerSimpleAction("costmaps_clear", std::bind(&CIriAnaNavModuleBT::costmaps_clear, this)); - this->factory.registerSimpleAction("costmaps_enable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port); - this->factory.registerSimpleAction("costmaps_disable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this)); + factory->registerSimpleAction("sync_go_to_orientation", std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), orientation_port); + factory->registerSimpleAction("sync_go_to_position", std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), position_port); + factory->registerSimpleAction("sync_go_to_pose", std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), pose_port); + factory->registerSimpleAction("sync_stop", std::bind(&CIriAnaNavModuleBT::sync_stop, this)); + factory->registerSimpleAction("set_goal_frame", std::bind(&CIriAnaNavModuleBT::set_goal_frame, this, std::placeholders::_1), frame_port); + factory->registerSimpleAction("get_current_pose", std::bind(&CIriAnaNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_port); + factory->registerSimpleAction("costmaps_clear", std::bind(&CIriAnaNavModuleBT::costmaps_clear, this)); + factory->registerSimpleAction("costmaps_enable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port); + factory->registerSimpleAction("costmaps_disable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this)); // registry of asynchronous actions - this->factory.registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), orientation_port); - this->factory.registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), position_port); - this->factory.registerIriAsyncAction("async_go_to_pose", std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), pose_port); - this->factory.registerIriAsyncAction("NOP", std::bind(&CIriAnaNavModuleBT::NOP, this)); + factory->registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), orientation_port); + factory->registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), position_port); + factory->registerIriAsyncAction("async_go_to_pose", std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), pose_port); + factory->registerIriAsyncAction("NOP", std::bind(&CIriAnaNavModuleBT::NOP, this)); // creation of tree from .xml std::cout << "create tree" << std::endl; - this->tree = this->factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml"); + this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml"); } CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void) @@ -624,7 +624,7 @@ void CIriAnaNavModuleBT::restart() } } -void CIriAnaNavModuleBT::create_tree(std::string path, std::string tree_xml_file) +void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file) { this->path=path; this->tree_xml_file=tree_xml_file; @@ -634,7 +634,7 @@ void CIriAnaNavModuleBT::create_tree(std::string path, std::string tree_xml_file try { - this->tree = this->factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml"); + this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml"); this->status = BT::NodeStatus::IDLE; // if logger exists delete previous one and create a new one