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

Updated with changes made in iri_ana_nav_bt node

parent 0a4b8d64
Hide whitespace changes
Inline Side-by-side
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
View file @ e7082812
......@@ -30,15 +30,17 @@ class CIriAnaNavModuleBT
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;
// 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;
......
src/iri_ana_nav_module_bt.cpp
View file @ e7082812
......@@ -7,15 +7,27 @@
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;
}
void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
{
// 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 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") };
......@@ -36,9 +48,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
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), orientation_port);
factory->registerSimpleAction("sync_go_to_position", std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), position_port);
factory->registerSimpleAction("sync_go_to_pose", std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), pose_port);
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);
......@@ -46,9 +58,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
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));
// registry of asynchronous actions
factory->registerIriAsyncAction("async_go_to_orientation", std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), orientation_port);
factory->registerIriAsyncAction("async_go_to_position", std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), position_port);
factory->registerIriAsyncAction("async_go_to_pose", std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), pose_port);
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));
// creation of tree from .xml
......@@ -195,9 +207,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
{
ROS_INFO("async_go_to_orientation");
static bool orientation_goal_sent = false;
static bool update_goal_value;
if (!orientation_goal_sent)
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");
......@@ -224,6 +246,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
return BT::NodeStatus::FAILURE;
}
orientation_goal_sent = true;
self.setOutput("update_goal", false);
}
if (nav.is_finished())
{
......@@ -237,9 +260,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
{
ROS_INFO("async_go_to_position");
static bool position_goal_sent = false;
static bool update_goal_value;
if (!position_goal_sent)
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");
......@@ -273,6 +306,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
return BT::NodeStatus::FAILURE;
}
position_goal_sent = true;
self.setOutput("update_goal", false);
}
if (nav.is_finished())
{
......@@ -286,9 +320,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
{
ROS_INFO("async_go_to_pose");
static bool pose_goal_sent = false;
static bool update_goal_value;
BT::Optional<bool> update_goal_input = self.getInput<bool>("update_goal");
if (!pose_goal_sent)
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");
......@@ -336,6 +380,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
return BT::NodeStatus::FAILURE;
}
pose_goal_sent = true;
self.setOutput("update_goal", false);
}
if (nav.is_finished())
{
......@@ -347,9 +392,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
{
ROS_INFO("sync_stop");
nav.stop();
stop();
return BT::NodeStatus::SUCCESS;
}
......@@ -367,7 +410,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_finished()
BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS)
{
......@@ -379,7 +422,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
BT::NodeStatus CIriAnaNavModuleBT::is_running()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_RUNNING)
{
......@@ -391,7 +434,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_running()
BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL)
{
......@@ -403,7 +446,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT)
{
......@@ -415,7 +458,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG)
{
......@@ -427,7 +470,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_ABORTED)
{
......@@ -440,7 +483,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED)
{
......@@ -452,7 +495,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_REJECTED)
{
......@@ -464,7 +507,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL)
{
......@@ -476,7 +519,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT)
{
......@@ -488,7 +531,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM)
{
......@@ -500,7 +543,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
BT::NodeStatus CIriAnaNavModuleBT::is_no_transform()
{
nav_status = nav.get_status();
iri_ana_nav_module_status_t nav_status = nav.get_status();
if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM)
{
......@@ -605,33 +648,41 @@ void CIriAnaNavModuleBT::stop()
void CIriAnaNavModuleBT::restart()
{
if (nav.is_finished())
ROS_INFO("-------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);
}
stop();
while (!nav.is_finished());
orientation_goal_sent = false;
position_goal_sent = false;
pose_goal_sent = false;
}
else
for(auto& node : this->tree.nodes)
{
node.get()->halt();
}
this->status = BT::NodeStatus::IDLE;
if (print_tree_enabled)
{
ROS_ERROR("Could not restart, stop movement first!");
BT::printTreeRecursively(this->tree.root_node);
}
}
void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
{
this->path=path;
this->tree_xml_file=tree_xml_file;
if (nav.is_finished())
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");
......@@ -669,36 +720,36 @@ void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::strin
}
catch (BT::RuntimeError &e)
{
ROS_ERROR_STREAM("EXCEPTION " << e.what());
ROS_ERROR_STREAM("EXCEPTION: " << e.what());
}
}
else
{
ROS_ERROR("Could not create new tree, stop movement first!");
ROS_WARN("This tree has already been created, restart if you want to start again!");
}
}
void CIriAnaNavModuleBT::enable_cout_logger()
{
try
if (this->logger_cout == NULL)
{
this->logger_cout = new BT::StdCoutLogger(this->tree);
}
catch (BT::LogicError &e)
else
{
ROS_ERROR("EXCEPTION: StdCoutLogger already enabled!");
ROS_WARN("StdCoutLogger already enabled!");
}
}
void CIriAnaNavModuleBT::enable_minitrace_logger()
{
try
if (this->logger_minitrace == NULL)
{
this->logger_minitrace = new BT::MinitraceLogger(this->tree, (this->path + "/logs/" + this->tree_xml_file + ".json").c_str());
}
catch (BT::LogicError)
else
{
ROS_ERROR("EXCEPTION: MinitraceLogger already enabled!");
ROS_WARN("MinitraceLogger already enabled!");
}
}
......@@ -710,19 +761,19 @@ void CIriAnaNavModuleBT::enable_file_logger()
}
else
{
ROS_ERROR("EXCEPTION: FileLogger already enabled!");
ROS_WARN("FileLogger already enabled!");
}
}
void CIriAnaNavModuleBT::enable_zmq_publisher()
{
try
if (this->publisher_zmq == NULL)
{
this->publisher_zmq = new BT::PublisherZMQ(this->tree);
}
catch (BT::LogicError &e)
else
{
ROS_ERROR("EXCEPTION: PublisherZMQ already enabled!");
ROS_WARN("PublisherZMQ already enabled!");
}
}
......@@ -735,30 +786,43 @@ void CIriAnaNavModuleBT::enable_print_tree()
BT::printTreeRecursively(this->tree.root_node);
}
print_tree_enabled = true;
}}
}
}
void CIriAnaNavModuleBT::disable_cout_logger()
{
delete this->logger_cout;
this->logger_cout = NULL;
if (this->logger_cout != NULL)
{
delete this->logger_cout;
this->logger_cout = NULL;
}
}
void CIriAnaNavModuleBT::disable_minitrace_logger()
{
delete this->logger_minitrace;
this->logger_minitrace = NULL;
if (this->logger_minitrace != NULL)
{
delete this->logger_minitrace;
this->logger_minitrace = NULL;
}
}
void CIriAnaNavModuleBT::disable_file_logger()
{
delete this->logger_file;
this->logger_file = NULL;
if (this->logger_file != NULL)
{
delete this->logger_file;
this->logger_file = NULL;
}
}
void CIriAnaNavModuleBT::disable_zmq_publisher()
{
delete this->publisher_zmq;
this->publisher_zmq = NULL;
if (this->publisher_zmq != NULL)
{
delete this->publisher_zmq;
this->publisher_zmq = NULL;
}
}
void CIriAnaNavModuleBT::disable_print_tree()
{
......
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