Commit faa006ff authored by abhagwan's avatar abhagwan
Browse files

Added BT layer

parent c3f11ad1
#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
#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);