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
new file mode 100644
index 0000000000000000000000000000000000000000..2e339fc212b6a410038e8d756fb8f8ce271a087f
--- /dev/null
+++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
@@ -0,0 +1,557 @@
+#ifndef _IRI_ANA_NAV_MODULE_BT_H
+#define _IRI_ANA_NAV_MODULE_BT_H
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include <behaviortree_cpp/bt_factory.h>
+#include <behaviortree_cpp/behavior_tree.h>
+
+#include "behaviortree_cpp/loggers/bt_cout_logger.h"
+#include "behaviortree_cpp/loggers/bt_zmq_publisher.h"
+#include "behaviortree_cpp/loggers/bt_minitrace_logger.h"
+#include "behaviortree_cpp/loggers/bt_file_logger.h"
+
+#include "iri_bt_factory.h"
+#include <iri_ana_nav_module/iri_ana_nav_module.h>
+
+class CIriAnaNavModuleBT
+{
+ private:
+
+ // tree file in xml format needed for creation of the behavior tree
+ std::string path;
+ std::string tree_xml_file;
+
+ // 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;
+ BT::FileLogger *logger_file;
+ BT::PublisherZMQ *publisher_zmq;
+
+ // print tree
+ bool print_tree_enabled;
+
+ // variable to save behavior tree status
+ BT::NodeStatus status;
+
+ // variable to save iri_ana_nav_module status
+ iri_ana_nav_module_status_t nav_status;
+
+ // object of iri_ana_nav_module
+ CIriAnaNavModule nav;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CIriAnaNavModuleBT();
+
+ void init(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file);
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~CIriAnaNavModuleBT();
+
+ protected:
+
+ // basic navigation synchronous functions
+ /**
+ * \brief Navigate to a given orientation (theta)
+ *
+ * This function calls go_to_orientation of iri_ana_nav_module which:
+ * rotates the robot about its axis to the given orientation with respect
+ * to the reference frame set by the set_goal_frame() function. As this is
+ * a synchronous action is_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * yaw (double): desired orientation with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * heading_tol (double): heading tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus sync_go_to_orientation(BT::TreeNode& self);
+ /**
+ * \brief Navigate to a given position (x,y)
+ *
+ * This function calls go_to_position of iri_ana_nav_module which:
+ * moves the robot to the given position preserving the final orientation
+ * with respect to the reference frame set by the set_goal_frame() function.
+ * The current orientation of the robot is taken from the odometry of the
+ * robot. As this is a synchronous action is_finished() must be used to
+ * know when the action has actually finished.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * x (double): desired X positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * y (double): desired Y positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * x_y_pos_tol (double): position tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus sync_go_to_position(BT::TreeNode& self);
+ /**
+ * \brief Navigate to a given pose (x,y,theta)
+ *
+ * This function calls go_to_pose of iri_ana_nav_module which:
+ * moves the robot to the given pose preserving with respect to the
+ * reference frame set by the set_goal_frame() function. As this is a
+ * synchronous action is_finished() must be used to know when the action
+ * has actually finished.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * x (double): desired X positon with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * y (double): desired Y positon with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * yaw (double): desired orientation with respect to the goal frame set
+ * by the set_goal_frame() function.
+ *
+ * heading_tol (double): heading tolerance for the goal.
+ *
+ * x_y_pos_tol (double): position tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus sync_go_to_pose(BT::TreeNode& self);
+ // basic navigation asynchronous functions
+ /**
+ * \brief Navigate to a given orientation (theta)
+ *
+ * This function calls go_to_orientation of iri_ana_nav_module which:
+ * rotates the robot about its axis to the given orientation with respect
+ * to the reference frame set by the set_goal_frame() function. As this is
+ * an asynchronous action is_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * yaw (double): desired orientation with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * heading_tol (double): heading tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+ * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+ * in progress (BT:NodeStatus::RUNNING).
+ *
+ */
+ BT::NodeStatus async_go_to_orientation(BT::TreeNode& self);
+ /**
+ * \brief Navigate to a given position (x,y)
+ *
+ * This function calls go_to_position of iri_ana_nav_module which:
+ * moves the robot to the given position preserving the final orientation
+ * with respect to the reference frame set by the set_goal_frame() function.
+ * The current orientation of the robot is taken from the odometry of the
+ * robot. As this is an asynchronous action is_finished() is checked to
+ * know when the action has actually finished.
+ *
+ * \param self node with the required inputs defined as follows:
+ *
+ * x (double): desired X positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+ *
+ * y (double): desired Y positon with respect to the goal frame set by the
+ * set_goal_frame() function.
+
+ * x_y_pos_tol (double): position tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+ * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+ * in progress (BT:NodeStatus::RUNNING).
+ *
+ */
+ BT::NodeStatus async_go_to_position(BT::TreeNode& self);
+ /**
+ * \brief Navigate to a given pose (x,y,theta)
+ *
+ * This function calls go_to_pose of iri_ana_nav_module which:
+ * moves the robot to the given pose preserving with respect to the
+ * reference frame set by the set_goal_frame() function. As this is an
+ * asynchronous action is_finished() is checked to know when the action
+ * has actually finished.
+ *
+ * \param self node with the required input defined as follows:
+ *
+ * goal (Pose_with_tol): the user must write the goal as an string with
+ * all the required inputs separated by semicolons like this:
+ *
+ * "x;y;yaw;heading_tol;x_y_pos_tol"
+ *
+ * These inputs define the goal as follows:
+ *
+ * - x (double): desired X positon with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * - y (double): desired Y positon with respect to the goal frame set by
+ * the set_goal_frame() function.
+ *
+ * - yaw (double): desired orientation with respect to the goal frame set
+ * by the set_goal_frame() function.
+ *
+ * - heading_tol (double): heading tolerance for the goal.
+
+ * - x_y_pos_tol (double): position tolerance for the goal.
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
+ * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
+ * in progress (BT:NodeStatus::RUNNING).
+ *
+ */
+ BT::NodeStatus async_go_to_pose(BT::TreeNode& self);
+ // Stop functions
+ /**
+ * \brief Stops the current goal synchronously
+ *
+ * This functions stops the motion of the robot, whatever the type of goal
+ * that has been given. This functions signals the stop to the navigation
+ * module, but the is_finished() function must be used to know when the
+ * navigation is actually stopped as it is a synchronous action.
+ *
+ * \return a BT:NodeStatus::SUCCESS always.
+ *
+ */
+ BT::NodeStatus sync_stop();
+ // Conditions to check if the last goal has finished or not and how
+ /**
+ * \brief Checks if the last goal has finished or not.
+ *
+ * This function must be used to know when the last navigation goal has
+ * ended in the case of synchronous actions, either successfully or by any
+ * error. In the case of asynchronous actions it is already called by
+ * these.
+ *
+ * \return a BT:NodeStatus indicating whether the last navigation goal has
+ * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE),
+ * regardless of its success or not.
+ *
+ */
+ BT::NodeStatus is_finished();
+ /**
+ * \brief Checks if the last navigation goal has been reached successfully.
+ *
+ * This function must be used to know whether the last navigation goal has
+ * been reached successfully or not, once it has finished.
+ *
+ * \return a BT:NodeStatus indicating whether the last navigation goal has
+ * been reached successfully (BT:NodeStatus::SUCCESS) or not
+ * (BT:NodeStatus::FAILURE), regardless of how it has failed.
+ *
+ */
+ BT::NodeStatus is_succeded();
+ /**
+ * \brief Checks if the module is active and operating properly.
+ *
+ * This function must be used to know whether the module is active and
+ * operating properly, or not.
+ *
+ * \return a BT:NodeStatus indicating whether the module is active and
+ * operating properly (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_running();
+ /**
+ * \brief Checks if the goal could not be started.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * goal could not be started.
+ *
+ * \return a BT:NodeStatus indicating whether the goal could not be started
+ * (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+ * (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_action_start_failed();
+ /**
+ * \brief Checks if the goal did not complete in the allowed time.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * goal did not complete in the allowed time.
+ *
+ * \return a BT:NodeStatus indicating whether the goal did not complete in
+ * the allowed time (BT:NodeStatus::SUCCESS) or if it failed due to another
+ * reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_timeout();
+ /**
+ * \brief Checks if the feedback has not been received for a long time.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * feedback has not been received for a long time.
+ *
+ * \return a BT:NodeStatus indicating whether the feedback has not been
+ * received for a long time (BT:NodeStatus::SUCCESS) or if it failed due
+ * to another reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_watchdog();
+ /**
+ * \brief Checks if the navigation goal could not be reached.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * navigation goal could not be reached.
+ *
+ * \return a BT:NodeStatus indicating whether the navigation goal could not
+ * be reached (BT:NodeStatus::SUCCESS) or if it failed due to another
+ * reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_aborted();
+ /**
+ * \brief Checks if the current navigation goal has been cancelled by
+ * another goal.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * current navigation goal has been cancelled by another goal.
+ *
+ * \return a BT:NodeStatus indicating whether the current navigation goal
+ * has been cancelled by another goal (BT:NodeStatus::SUCCESS) or if it
+ * failed due to another reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_preempted();
+ /**
+ * \brief Checks if the goal information is not valid.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * goal information is not valid and the goal will not be executed.
+ *
+ * \return a BT:NodeStatus indicating whether the goal information is not
+ * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+ * (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_rejected();
+ /**
+ * \brief Checks if it was impossible to set the value of a parameter.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because it was
+ * impossible to set the value of a parameter.
+ *
+ * \return a BT:NodeStatus indicating whether it was impossible to set the
+ * value of a parameter (BT:NodeStatus::SUCCESS) or if it failed due to
+ * another reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_set_param_failed();
+ /**
+ * \brief Checks if the parameter to be changed is not present in the
+ * remote node.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because the
+ * parameter to be changed is not present in the remote node.
+ *
+ * \return a BT:NodeStatus indicating whether the parameter to be changed
+ * is not present in the remote node (BT:NodeStatus::SUCCESS) or if it
+ * failed due to another reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_param_not_present();
+ /**
+ * \brief Checks if no odometry information is being received.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because no
+ * odometry information is being received, so it is not possible to get
+ * the current position of the robot.
+ *
+ * \return a BT:NodeStatus indicating whether no odometry information is
+ * being received (BT:NodeStatus::SUCCESS) or if it failed due to another
+ * reason (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_no_odom();
+ /**
+ * \brief Checks if there exists no transform between the desired goal
+ * frame and the rest of the TF tree.
+ *
+ * This function must be used to know the type of failure, once the last
+ * goal has finished. Specifically, it checks if it failed because there
+ * exists no transform between the desired goal frame and the rest of the
+ * TF tree.
+ *
+ * \return a BT:NodeStatus indicating whether there exists no transform
+ * between the desired goal frame and the rest of the TF tree
+ * (BT:NodeStatus::SUCCESS) or if it failed due to another reason
+ * (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus is_no_transform();
+ // Other useful functions
+ /**
+ * \brief Set the goal reference frame
+ *
+ * This functions sets the reference with used for all the position,
+ * orientation and pose goals.
+ *
+ * \param self node with the required input defined as follows:
+ *
+ * frame_id (std::string): name of the new reference frame for the future
+ * goals. This frame can be any that exist inside
+ * the TF tree of the robot.
+ *
+ * \return a BT:NodeStatus::SUCCESS if the input is valid.
+ *
+ */
+ BT::NodeStatus set_goal_frame(BT::TreeNode& self);
+ /**
+ * \brief Get current pose (x,y,theta)
+ *
+ * This function calls get_current_pose of iri_ana_nav_module and converts
+ * from geometry_msgs::PoseStamped to a structure defined as Pose.
+ *
+ * \param self node with the required output defined as follows:
+ *
+ * x (double): current X positon with respect to the /map frame.
+ *
+ * y (double): current Y positon with respect to the /map frame.
+ *
+ * yaw (double): current orientation with respect the /map frame.
+ *
+ * \return a BT:NodeStatus::SUCCESS always.
+ *
+ */
+ BT::NodeStatus get_current_pose(BT::TreeNode& self);
+ /**
+ * \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();
+ // Costmaps functions
+ /**
+ * \brief Clears the costmaps
+ *
+ * This functions can be used to clear the navigation costmaps at any time.
+ * On success, this function will remove any ghost obstacles that remained
+ * from previous detections. Calling this function when the auto-clearing
+ * feature is enabled will have no effect.
+ *
+ * Clearing the costmaps may improve the robot's navigation in narrow spaces.
+ *
+ * \return a BT:NodeStatus indicating whether the costmaps have been
+ * cleared (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus costmaps_clear();
+ /**
+ * \brief Enable the autoclear of the costmaps
+ *
+ * This functions enables a periodic clearing of the navigation costmaps at a
+ * desired rate, which in general should be lower than 1 Hz. When this feature
+ * is enabled, it will be no longer possible to clear the costmaps manually
+ * with the costmaps_clear() function.
+ *
+ * \param self node with the required input defined as follows:
+ *
+ * rate_hz (double): the desired clearing rate in Hz. This value should be
+ * less than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
+ *
+ * \return a BT:NodeStatus::SUCCESS always
+ *
+ */
+ BT::NodeStatus costmaps_enable_auto_clear(BT::TreeNode& self);
+ /**
+ * \brief Disable the autoclear feature
+ *
+ * This function disables the periodic clearing of the navigation costmaps.
+ * After disabling this feature, the costmaps_clear() function may be used
+ * to do so.
+ *
+ * \return a BT:NodeStatus::SUCCESS always
+ *
+ */
+ BT::NodeStatus costmaps_disable_auto_clear();
+ /**
+ * \brief Checks the status of the autoclear feature
+ *
+ * This functions checks whether the costmaps autoclear feature is enabled
+ * or not.
+ *
+ * \return a BT:NodeStatus indicating whether the costmaps autoclear feature
+ * is enabled (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ *
+ */
+ BT::NodeStatus costmap_is_auto_clear_enabled();
+
+ void tick();
+
+ void stop();
+
+ /**
+ * \brief Restart behavior tree function
+ *
+ * This functions halts all nodes, so their status changes to IDLE. ATENTION:
+ * It does not reset the parameters values changed during the execution of the
+ * behavior tree and stop() should be called before restarting.
+ *
+ */
+ void restart();
+
+ void create_tree(std::string path, std::string tree_xml_file);
+
+ void enable_cout_logger();
+
+ void enable_minitrace_logger();
+
+ void enable_file_logger();
+
+ void enable_zmq_publisher();
+
+ void enable_print_tree();
+
+ void disable_cout_logger();
+
+ void disable_minitrace_logger();
+
+ void disable_file_logger();
+
+ void disable_zmq_publisher();
+
+ void disable_print_tree();
+
+ };
+
+ #endif
diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b2e98f5f7b1fc7204b9df1fd04e2bbed7d61b87
--- /dev/null
+++ b/src/iri_ana_nav_module_bt.cpp
@@ -0,0 +1,766 @@
+#include <iri_ana_nav_module/iri_ana_nav_module_bt.h>
+#include <iri_ana_nav_module/iri_ana_nav_module.h>
+#include "behaviortree_cpp/blackboard.h"
+#include <sstream>
+#include <stdlib.h>
+
+CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
+ nav("nav_module",ros::this_node::getName())
+{
+
+}
+
+void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
+{
+ // definition of ports
+ BT::PortsList orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
+ BT::PortsList position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol") };
+ BT::PortsList pose_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol") };
+ BT::PortsList frame_port = { BT::InputPort<std::string>("frame_id") };
+ BT::PortsList current_pose_port = { BT::OutputPort<double>("current_x"), BT::OutputPort<double>("current_y"), BT::OutputPort<double>("current_yaw") };
+ 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));
+ // 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));
+ // 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));
+
+ // creation of tree from .xml
+ std::cout << "create tree" << std::endl;
+ this->tree = this->factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
+}
+
+CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void)
+{
+ // [free dynamic memory]
+ disable_cout_logger();
+ disable_minitrace_logger();
+ disable_file_logger();
+ disable_zmq_publisher();
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_orientation(BT::TreeNode& self)
+{
+ ROS_INFO("sync_go_to_orientation");
+ BT::Optional<double> yaw_input = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
+
+ if (!yaw_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [yaw]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!heading_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double yaw_goal = yaw_input.value();
+ double heading_tol_goal = heading_tol_input.value();
+ ROS_INFO("The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
+ if(!this->nav.go_to_orientation(yaw_goal, heading_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_position(BT::TreeNode& self)
+{
+ ROS_INFO("sync_go_to_position");
+ BT::Optional<double> x_input = self.getInput<double>("x");
+ BT::Optional<double> y_input = self.getInput<double>("y");
+ BT::Optional<double> x_y_pos_tol_input = self.getInput<double>("x_y_pos_tol");
+
+ if (!x_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!y_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [y]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!x_y_pos_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double x_goal = x_input.value();
+ double y_goal = y_input.value();
+ double x_y_pos_tol_goal = x_y_pos_tol_input.value();
+ ROS_INFO("The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
+ if(!this->nav.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::sync_go_to_pose(BT::TreeNode& self)
+{
+
+ ROS_INFO("sync_go_to_pose");
+ BT::Optional<double> x_input = self.getInput<double>("x");
+ BT::Optional<double> y_input = self.getInput<double>("y");
+ BT::Optional<double> yaw_input = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
+ BT::Optional<double> x_y_pos_tol_input = self.getInput<double>("x_y_pos_tol");
+
+ if (!x_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!y_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [y]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!yaw_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [yaw]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!heading_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!x_y_pos_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double x_goal = x_input.value();
+ double y_goal = y_input.value();
+ double yaw_goal = yaw_input.value();
+ double heading_tol_goal = heading_tol_input.value();
+ double x_y_pos_tol_goal = x_y_pos_tol_input.value();
+ ROS_INFO("The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
+ if(!this->nav.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ else
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
+{
+ ROS_INFO("async_go_to_orientation");
+
+ static bool orientation_goal_sent = false;
+
+ if (!orientation_goal_sent)
+ {
+ BT::Optional<double> yaw_input = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
+
+ if (!yaw_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [yaw]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!heading_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double yaw_goal = yaw_input.value();
+ double heading_tol_goal = heading_tol_input.value();
+ ROS_INFO("The goal is yaw: [%f] with heading_tol: [%f]", yaw_goal, heading_tol_goal);
+
+ if(!this->nav.go_to_orientation(yaw_goal, heading_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ orientation_goal_sent = true;
+ }
+ if (nav.is_finished())
+ {
+ orientation_goal_sent = false;
+ return BT::NodeStatus::SUCCESS;
+ }
+ else return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
+{
+ ROS_INFO("async_go_to_position");
+
+ static bool position_goal_sent = false;
+
+ if (!position_goal_sent)
+ {
+ BT::Optional<double> x_input = self.getInput<double>("x");
+ BT::Optional<double> y_input = self.getInput<double>("y");
+ BT::Optional<double> x_y_pos_tol_input = self.getInput<double>("x_y_pos_tol");
+
+ if (!x_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!y_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [y]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!x_y_pos_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double x_goal = x_input.value();
+ double y_goal = y_input.value();
+ double x_y_pos_tol_goal = x_y_pos_tol_input.value();
+ ROS_INFO("The goal is x: [%f], y: [%f] with x_y_pos_tol: [%f]", x_goal, y_goal, x_y_pos_tol_goal);
+
+ if(!this->nav.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ position_goal_sent = true;
+ }
+ if (nav.is_finished())
+ {
+ position_goal_sent = false;
+ return BT::NodeStatus::SUCCESS;
+ }
+ else return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
+{
+ ROS_INFO("async_go_to_pose");
+
+ static bool pose_goal_sent = false;
+
+ if (!pose_goal_sent)
+ {
+ BT::Optional<double> x_input = self.getInput<double>("x");
+ BT::Optional<double> y_input = self.getInput<double>("y");
+ BT::Optional<double> yaw_input = self.getInput<double>("yaw");
+ BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol");
+ BT::Optional<double> x_y_pos_tol_input = self.getInput<double>("x_y_pos_tol");
+
+ if (!x_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!y_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [y]");
+ return BT::NodeStatus::FAILURE;
+ }
+ if (!yaw_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [yaw]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!heading_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [heading_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ if (!x_y_pos_tol_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [x_y_pos_tol]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double x_goal = x_input.value();
+ double y_goal = y_input.value();
+ double yaw_goal = yaw_input.value();
+ double heading_tol_goal = heading_tol_input.value();
+ double x_y_pos_tol_goal = x_y_pos_tol_input.value();
+ ROS_INFO("The goal is x: [%f], y: [%f], yaw: [%f] with heading_tol: [%f] and x_y_pos_tol: [%f]", x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal);
+ if(!this->nav.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
+ {
+ ROS_ERROR("Impossible to start goal");
+ return BT::NodeStatus::FAILURE;
+ }
+ pose_goal_sent = true;
+ }
+ if (nav.is_finished())
+ {
+ pose_goal_sent = false;
+ return BT::NodeStatus::SUCCESS;
+ }
+ else return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
+{
+ ROS_INFO("sync_stop");
+
+ nav.stop();
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_finished()
+{
+ if (nav.is_finished())
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ {
+ return BT::NodeStatus::FAILURE;
+ }
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_running()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_RUNNING)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_ABORTED)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+
+BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_REJECTED)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::is_no_transform()
+{
+ nav_status = nav.get_status();
+
+ if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
+{
+ ROS_INFO("set_goal_frame");
+ BT::Optional<std::string> frame_id_input = self.getInput<std::string>("frame_id");
+
+
+ if (!frame_id_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [frame_id]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ std::string frame_id_goal = frame_id_input.value();
+ ROS_INFO_STREAM("The new frame is: " << frame_id_goal);
+ nav.set_goal_frame(frame_id_goal);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::get_current_pose(BT::TreeNode& self)
+{
+ ROS_INFO("get_current_pose");
+ geometry_msgs::Pose current_pose = nav.getCurrentPose();
+
+ self.setOutput("current_x", current_pose.position.x);
+ self.setOutput("current_y", current_pose.position.y);
+ self.setOutput("current_yaw", tf::getYaw(current_pose.orientation));
+ ROS_INFO("The current pose is: x-> [%f], y-> [%f], yaw-> [%f]", current_pose.position.x, current_pose.position.y, tf::getYaw(current_pose.orientation));
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::NOP()
+{
+ return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::costmaps_clear()
+{
+ ROS_INFO("costmaps_clear");
+ if (!this->nav.costmaps_clear())
+ {
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::costmaps_enable_auto_clear(BT::TreeNode& self)
+{
+ ROS_INFO("costmaps_enable_auto_clear");
+ BT::Optional<double> rate_hz_input = self.getInput<double>("rate_hz");
+
+
+ if (!rate_hz_input)
+ {
+ ROS_ERROR("Incorrect or missing required input [rate_hz]");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double rate_hz_goal = rate_hz_input.value();
+ ROS_INFO_STREAM("The rate in [hz] is: " << rate_hz_goal);
+ nav.costmaps_enable_auto_clear(rate_hz_goal);
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::costmaps_disable_auto_clear()
+{
+ ROS_INFO("costmaps_disable_auto_clear");
+ nav.costmaps_disable_auto_clear();
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled()
+{
+ ROS_INFO("costmap_is_auto_clear_enabled");
+ if (!this->nav.costmap_is_auto_clear_enabled())
+ {
+ return BT::NodeStatus::FAILURE;
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+void AnaNavBtAlgNode::tick()
+{
+ ROS_INFO("-------Tick-------");
+ this->status = this->tree.root_node->executeTick();
+}
+
+void AnaNavBtAlgNode::stop()
+{
+ ROS_INFO("-------STOP-------");
+ nav.stop();
+}
+
+void AnaNavBtAlgNode::restart()
+{
+ if (nav.is_finished())
+ {
+ ROS_INFO("-------RESTART-------");
+ for(auto& node : this->tree.nodes)
+ {
+ node.get()->halt();
+ }
+ this->status = BT::NodeStatus::IDLE;
+ if (print_tree_enabled)
+ {
+ BT::printTreeRecursively(this->tree.root_node);
+ }
+ }
+ else
+ {
+ ROS_ERROR("Could not restart, stop movement first!");
+ }
+}
+
+void AnaNavBtAlgNode::create_tree(std::string path, std::string tree_xml_file)
+{
+ this->path=path;
+ this->tree_xml_file=tree_xml_file;
+ if (nav.is_finished())
+ {
+ ROS_INFO("-------CREATING NEW TREE-------");
+
+ try
+ {
+ this->tree = this->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
+ if (this->logger_cout != NULL)
+ {
+ disable_cout_logger();
+ this->logger_cout = new BT::StdCoutLogger(this->tree);
+ }
+
+ if (this->logger_minitrace != NULL)
+ {
+ disable_minitrace_logger();
+ this->logger_minitrace = new BT::MinitraceLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".json").c_str());
+ }
+
+ if (this->logger_file != NULL)
+ {
+ disable_file_logger();
+ this->logger_file = new BT::FileLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".fbl").c_str());
+ }
+
+ if (this->publisher_zmq != NULL)
+ {
+ disable_zmq_publisher();
+ this->publisher_zmq = new BT::PublisherZMQ(this->tree);
+ }
+
+ if (print_tree_enabled)
+ {
+ BT::printTreeRecursively(this->tree.root_node);
+ }
+ }
+ catch (BT::RuntimeError &e)
+ {
+ ROS_ERROR_STREAM("EXCEPTION " << e.what());
+ }
+ }
+ else
+ {
+ ROS_ERROR("Could not create new tree, stop movement first!");
+ }
+}
+
+void AnaNavBtAlgNode::enable_cout_logger()
+{
+ try
+ {
+ this->logger_cout = new BT::StdCoutLogger(this->tree);
+ }
+ catch (BT::LogicError &e)
+ {
+ ROS_ERROR("EXCEPTION: StdCoutLogger already enabled!");
+ }
+}
+
+void AnaNavBtAlgNode::enable_minitrace_logger()
+{
+ try
+ {
+ this->logger_minitrace = new BT::MinitraceLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".json").c_str());
+ }
+ catch (BT::LogicError)
+ {
+ ROS_ERROR("EXCEPTION: MinitraceLogger already enabled!");
+ }
+}
+
+void AnaNavBtAlgNode::enable_file_logger()
+{
+ if (this->logger_file == NULL)
+ {
+ this->logger_file = new BT::FileLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".fbl").c_str());
+ }
+ else
+ {
+ ROS_ERROR("EXCEPTION: FileLogger already enabled!");
+ }
+}
+
+void AnaNavBtAlgNode::enable_zmq_publisher()
+{
+ try
+ {
+ this->publisher_zmq = new BT::PublisherZMQ(this->tree);
+ }
+ catch (BT::LogicError &e)
+ {
+ ROS_ERROR("EXCEPTION: PublisherZMQ already enabled!");
+ }
+}
+
+void AnaNavBtAlgNode::enable_print_tree()
+{
+ if (!print_tree_enabled)
+ {
+ if (this->status == BT::NodeStatus::IDLE)
+ {
+ BT::printTreeRecursively(this->tree.root_node);
+ }
+ print_tree_enabled = true;
+ }}
+
+void AnaNavBtAlgNode::disable_cout_logger()
+{
+ delete this->logger_cout;
+ this->logger_cout = NULL;
+}
+
+void AnaNavBtAlgNode::disable_minitrace_logger()
+{
+ delete this->logger_minitrace;
+ this->logger_minitrace = NULL;
+}
+
+void AnaNavBtAlgNode::disable_file_logger()
+{
+ delete this->logger_file;
+ this->logger_file = NULL;
+}
+
+void AnaNavBtAlgNode::disable_zmq_publisher()
+{
+ delete this->publisher_zmq;
+ this->publisher_zmq = NULL;
+}
+void AnaNavBtAlgNode::disable_print_tree()
+{
+ print_tree_enabled = false;
+}