Commit 8282143c authored by abhagwan's avatar abhagwan
Browse files

Externalised generic functions and creation of tree. Added extra functions needed.

parent 57474c0f
......@@ -17,23 +17,11 @@ class CIriAnaNavModuleBT
{
private:
// tree object to save the behavior tree created and execute its tick()
BT::Tree tree;
// loggers
BT::StdCoutLogger *logger_cout;
BT::MinitraceLogger *logger_minitrace;
BT::FileLogger *logger_file;
BT::PublisherZMQ *publisher_zmq;
// goal sent variables
bool orientation_goal_sent;
bool position_goal_sent;
bool pose_goal_sent;
// print tree
bool print_tree_enabled;
// object of iri_ana_nav_module
CIriAnaNavModule nav;
......@@ -44,50 +32,13 @@ class CIriAnaNavModuleBT
*/
CIriAnaNavModuleBT();
// tree file in xml format needed for creation of the behavior tree
std::string path;
std::string tree_xml_file;
// variable to save behavior tree status
BT::NodeStatus status;
void init(IriBehaviorTreeFactory &factory);
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(IriBehaviorTreeFactory &factory, 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();
bool is_finished_bool();
void disable_print_tree();
void init_goal_variables();
/**
* \brief Destructor
......
......@@ -7,18 +7,7 @@
CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
nav("nav_module",ros::this_node::getName())
{
// loggers
this->logger_cout = NULL;
this->logger_minitrace = NULL;
this->logger_file = NULL;
this->publisher_zmq = NULL;
// goal sent variables
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
this->status=BT::NodeStatus::IDLE;
init_goal_variables();
}
void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
......@@ -65,18 +54,11 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
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));
// creation of tree from .xml
std::cout << "create tree" << std::endl;
this->tree = 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)
......@@ -636,197 +618,21 @@ BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled()
return BT::NodeStatus::SUCCESS;
}
void CIriAnaNavModuleBT::tick()
{
ROS_INFO("-------Tick-------");
this->status = this->tree.root_node->executeTick();
}
void CIriAnaNavModuleBT::stop()
{
ROS_INFO("-------STOP-------");
nav.stop();
}
void CIriAnaNavModuleBT::restart()
{
ROS_INFO("-------RESTART-------");
if (!nav.is_finished())
{
stop();
while (!nav.is_finished());
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
}
for(auto& node : this->tree.nodes)
{
node.get()->halt();
}
this->status = BT::NodeStatus::IDLE;
if (print_tree_enabled)
{
BT::printTreeRecursively(this->tree.root_node);
}
}
void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory &factory, std::string path, std::string tree_xml_file)
{
if (this->path != path || this->tree_xml_file != tree_xml_file)
{
ROS_INFO("-------CREATING NEW TREE-------");
if (!nav.is_finished())
{
stop();
while (!nav.is_finished());
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
}
this->path=path;
this->tree_xml_file=tree_xml_file;
try
{
this->tree = factory.createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
this->status = BT::NodeStatus::IDLE;
// if logger exists delete previous one and create a new one
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_WARN("This tree has already been created, restart if you want to start again!");
}
}
void CIriAnaNavModuleBT::enable_cout_logger()
{
if (this->logger_cout == NULL)
{
this->logger_cout = new BT::StdCoutLogger(this->tree);
}
else
{
ROS_WARN("StdCoutLogger already enabled!");
}
}
void CIriAnaNavModuleBT::enable_minitrace_logger()
{
if (this->logger_minitrace == NULL)
{
this->logger_minitrace = new BT::MinitraceLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".json").c_str());
}
else
{
ROS_WARN("MinitraceLogger already enabled!");
}
}
void CIriAnaNavModuleBT::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_WARN("FileLogger already enabled!");
}
}
void CIriAnaNavModuleBT::enable_zmq_publisher()
bool CIriAnaNavModuleBT::is_finished_bool()
{
if (this->publisher_zmq == NULL)
{
this->publisher_zmq = new BT::PublisherZMQ(this->tree);
}
else
{
ROS_WARN("PublisherZMQ already enabled!");
}
}
void CIriAnaNavModuleBT::enable_print_tree()
{
if (!print_tree_enabled)
{
if (this->status == BT::NodeStatus::IDLE)
{
BT::printTreeRecursively(this->tree.root_node);
}
print_tree_enabled = true;
}
return nav.is_finished();
}
void CIriAnaNavModuleBT::disable_cout_logger()
void CIriAnaNavModuleBT::init_goal_variables()
{
if (this->logger_cout != NULL)
{
delete this->logger_cout;
this->logger_cout = NULL;
}
}
void CIriAnaNavModuleBT::disable_minitrace_logger()
{
if (this->logger_minitrace != NULL)
{
delete this->logger_minitrace;
this->logger_minitrace = NULL;
}
}
void CIriAnaNavModuleBT::disable_file_logger()
{
if (this->logger_file != NULL)
{
delete this->logger_file;
this->logger_file = NULL;
}
}
void CIriAnaNavModuleBT::disable_zmq_publisher()
{
if (this->publisher_zmq != NULL)
{
delete this->publisher_zmq;
this->publisher_zmq = NULL;
}
}
void CIriAnaNavModuleBT::disable_print_tree()
{
print_tree_enabled = false;
// goal sent variables
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment