diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10dcee40f9cdf65d287b1e9e35ffb64e2c664453..7298b9231152277ed4c899f3f0b39d8993a01dba 100644
--- a/CMakeLists.txt
+++ b/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,9 +122,14 @@ 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})
+target_link_libraries(iri_ana_nav_module_bt ${iriutils_LIBRARY})
##Link to other modules
##target_link_libraries(new_module <module path>/lib<module_name>.so)
@@ -131,12 +137,14 @@ 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)
target_link_libraries(iri_ana_nav_client ${catkin_LIBRARIES})
target_link_libraries(iri_ana_nav_client ${iriutils_LIBRARY})
target_link_libraries(iri_ana_nav_client iri_ana_nav_module)
+target_link_libraries(iri_ana_nav_module_bt iri_ana_nav_module)
add_dependencies(iri_ana_nav_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
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..65a869c0e6077bb4bf9266a0418cf6f497ef5f56
--- /dev/null
+++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
@@ -0,0 +1,529 @@
+#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:
+
+ // goal sent variables
+ bool orientation_goal_sent;
+ bool position_goal_sent;
+ bool pose_goal_sent;
+
+ // object of iri_ana_nav_module
+ CIriAnaNavModule nav;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CIriAnaNavModuleBT();
+
+ /**
+ * \brief Register all conditions and actions needed for navigation
+ *
+ * This function registers all conditions and actions needed for navigation
+ * to the factory provided including all input and output ports required
+ * by actions.
+ *
+ * \param factory (IriBehaviorTreeFactory)
+ *
+ */
+ void init(IriBehaviorTreeFactory &factory);
+
+ /**
+ * \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).
+ * If inputs are missing or incorrect it also returns 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).
+ * If inputs are missing or incorrect it also returns 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).
+ * If inputs are missing or incorrect it also returns 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). If inputs are missing or incorrect
+ * it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ 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). If inputs are missing or incorrect
+ * it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ 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). If inputs are missing or incorrect
+ * it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus async_go_to_pose(BT::TreeNode& self);
+ // Stop function
+ /**
+ * \brief Stops the current goal synchronously
+ *
+ * This function stops the motion of the robot, whatever the type of goal
+ * that has been given. This function 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();
+ /**
+ * \brief Sets goal sent variables to false
+ *
+ * This is a synchronous action that provides a way to reset the values of
+ * goal sent variables through the client node if needed.
+ *
+ * It will be needed in case of restart of the tree as it can interrupt an
+ * asynchronous go_to.
+ *
+ */
+ BT::NodeStatus reset_goal_variables();
+ // 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 function 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,yaw)
+ *
+ * This function calls get_current_pose of iri_ana_nav_module and converts
+ * from geometry_msgs::PoseStamped to 3 outputs (x,y,yaw).
+ *
+ * \param self node with the required output defined as follows:
+ *
+ * current_x (double): current X positon with respect to the /map frame.
+ *
+ * current_y (double): current Y positon with respect to the /map frame.
+ *
+ * current_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 function 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 function 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 if input is valid.
+ *
+ */
+ BT::NodeStatus costmaps_disable_auto_clear();
+ /**
+ * \brief Checks the status of the autoclear feature
+ *
+ * This function 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();
+
+ };
+
+ #endif
diff --git a/package.xml b/package.xml
index e730d1037ab86874da6d8c3a7014b72920d3b6e1..62e5858717827c1612ac8a9da343b1a59faf4b35 100644
--- a/package.xml
+++ b/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>-->
diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed96fc0a966e10bd936d73589b3685d83bb100a7
--- /dev/null
+++ b/src/iri_ana_nav_module_bt.cpp
@@ -0,0 +1,632 @@
+#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())
+{
+ orientation_goal_sent = false;
+ position_goal_sent = false;
+ pose_goal_sent = false;
+}
+
+void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
+{
+ // definition of ports
+ BT::PortsList sync_orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") };
+ BT::PortsList sync_position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol") };
+ BT::PortsList sync_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 async_orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::BidirectionalPort<bool>("update_goal") };
+ BT::PortsList async_position_port = { BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("update_goal")};
+ BT::PortsList async_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::BidirectionalPort<bool>("update_goal") };
+ 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
+ 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
+ factory.registerSimpleAction("sync_go_to_orientation", std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), sync_orientation_port);
+ factory.registerSimpleAction("sync_go_to_position", std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), sync_position_port);
+ factory.registerSimpleAction("sync_go_to_pose", std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), sync_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));
+ factory.registerSimpleAction("reset_goal_variables", std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this));
+ // registry of asynchronous actions
+ factory.registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port);
+ factory.registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port);
+ factory.registerIriAsyncAction("async_go_to_pose", std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), async_pose_port);
+ factory.registerIriAsyncAction("NOP", std::bind(&CIriAnaNavModuleBT::NOP, this));
+
+}
+
+CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void)
+{
+ // [free dynamic memory]
+}
+
+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 update_goal_value;
+
+ BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
+
+ if (!update_goal_input)
+ {
+ update_goal_value = false;
+ }
+ else
+ {
+ update_goal_value = update_goal_input.value();
+ }
+ if (!orientation_goal_sent || update_goal_value)
+ {
+ 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;
+ self.setOutput("update_goal", false);
+ }
+ 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 update_goal_value;
+
+ BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
+
+ if (!update_goal_input)
+ {
+ update_goal_value = false;
+ }
+ else
+ {
+ update_goal_value = update_goal_input.value();
+ }
+ if (!position_goal_sent || update_goal_value)
+ {
+ 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;
+ self.setOutput("update_goal", false);
+ }
+ 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 update_goal_value;
+
+ BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
+
+ if (!update_goal_input)
+ {
+ update_goal_value = false;
+ }
+ else
+ {
+ update_goal_value = update_goal_input.value();
+ }
+ if (!pose_goal_sent || update_goal_value)
+ {
+ 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;
+ self.setOutput("update_goal", false);
+ }
+ if (nav.is_finished())
+ {
+ pose_goal_sent = false;
+ return BT::NodeStatus::SUCCESS;
+ }
+ else return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
+{
+ ROS_INFO("-------STOP-------");
+ nav.stop();
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables()
+{
+ // goal sent variables
+ orientation_goal_sent = false;
+ position_goal_sent = false;
+ pose_goal_sent = false;
+ 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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()
+{
+ iri_ana_nav_module_status_t 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;
+}