Skip to content
GitLab
Commit 8282143c authored by abhagwan's avatar abhagwan
Browse files

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

parent 57474c0f
Hide whitespace changes
Inline Side-by-side
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
View file @ 8282143c
......@@ -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
......
src/iri_ana_nav_module_bt.cpp
View file @ 8282143c
......@@ -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