Commit e7082812 authored by abhagwan's avatar abhagwan
Browse files

Updated with changes made in iri_ana_nav_bt node

parent 0a4b8d64
...@@ -30,15 +30,17 @@ class CIriAnaNavModuleBT ...@@ -30,15 +30,17 @@ class CIriAnaNavModuleBT
BT::FileLogger *logger_file; BT::FileLogger *logger_file;
BT::PublisherZMQ *publisher_zmq; BT::PublisherZMQ *publisher_zmq;
// goal sent variables
bool orientation_goal_sent;
bool position_goal_sent;
bool pose_goal_sent;
// print tree // print tree
bool print_tree_enabled; bool print_tree_enabled;
// variable to save behavior tree status // variable to save behavior tree status
BT::NodeStatus 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 // object of iri_ana_nav_module
CIriAnaNavModule nav; CIriAnaNavModule nav;
......
...@@ -7,15 +7,27 @@ ...@@ -7,15 +7,27 @@
CIriAnaNavModuleBT::CIriAnaNavModuleBT() : CIriAnaNavModuleBT::CIriAnaNavModuleBT() :
nav("nav_module",ros::this_node::getName()) 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) void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
{ {
// definition of ports // definition of ports
BT::PortsList orientation_port = { BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol") }; BT::PortsList sync_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 sync_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_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 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 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") }; BT::PortsList costmap_port = { BT::InputPort<double>("rate_hz") };
...@@ -36,9 +48,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory) ...@@ -36,9 +48,9 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory *factory)
factory->registerSimpleCondition("is_no_transform", std::bind(&CIriAnaNavModuleBT::is_no_transform, 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)); factory->registerSimpleCondition("costmap_is_auto_clear_enabled", std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled, this));
// registry of synchronous actions // 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_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), position_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), pose_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("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("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("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) ...@@ -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_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("costmaps_disable_auto_clear", std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this));
// registry of asynchronous actions // 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_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), position_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), pose_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)); factory->registerIriAsyncAction("NOP", std::bind(&CIriAnaNavModuleBT::NOP, this));
// creation of tree from .xml // creation of tree from .xml
...@@ -195,9 +207,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self) ...@@ -195,9 +207,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
{ {
ROS_INFO("async_go_to_orientation"); 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> yaw_input = self.getInput<double>("yaw");
BT::Optional<double> heading_tol_input = self.getInput<double>("heading_tol"); 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) ...@@ -224,6 +246,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_orientation(BT::TreeNode& self)
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
orientation_goal_sent = true; orientation_goal_sent = true;
self.setOutput("update_goal", false);
} }
if (nav.is_finished()) if (nav.is_finished())
{ {
...@@ -237,9 +260,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self) ...@@ -237,9 +260,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
{ {
ROS_INFO("async_go_to_position"); 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> x_input = self.getInput<double>("x");
BT::Optional<double> y_input = self.getInput<double>("y"); BT::Optional<double> y_input = self.getInput<double>("y");
...@@ -273,6 +306,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self) ...@@ -273,6 +306,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_position(BT::TreeNode& self)
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
position_goal_sent = true; position_goal_sent = true;
self.setOutput("update_goal", false);
} }
if (nav.is_finished()) if (nav.is_finished())
{ {
...@@ -286,9 +320,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self) ...@@ -286,9 +320,19 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
{ {
ROS_INFO("async_go_to_pose"); 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> x_input = self.getInput<double>("x");
BT::Optional<double> y_input = self.getInput<double>("y"); BT::Optional<double> y_input = self.getInput<double>("y");
...@@ -336,6 +380,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self) ...@@ -336,6 +380,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
pose_goal_sent = true; pose_goal_sent = true;
self.setOutput("update_goal", false);
} }
if (nav.is_finished()) if (nav.is_finished())
{ {
...@@ -347,9 +392,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self) ...@@ -347,9 +392,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
BT::NodeStatus CIriAnaNavModuleBT::sync_stop() BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
{ {
ROS_INFO("sync_stop"); stop();
nav.stop();
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }
...@@ -367,7 +410,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_finished() ...@@ -367,7 +410,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_finished()
BT::NodeStatus CIriAnaNavModuleBT::is_succeded() 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) if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS)
{ {
...@@ -379,7 +422,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_succeded() ...@@ -379,7 +422,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
BT::NodeStatus CIriAnaNavModuleBT::is_running() 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) if (nav_status == IRI_ANA_NAV_MODULE_RUNNING)
{ {
...@@ -391,7 +434,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_running() ...@@ -391,7 +434,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_running()
BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed() 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) if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL)
{ {
...@@ -403,7 +446,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed() ...@@ -403,7 +446,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
BT::NodeStatus CIriAnaNavModuleBT::is_timeout() 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) if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT)
{ {
...@@ -415,7 +458,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_timeout() ...@@ -415,7 +458,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
BT::NodeStatus CIriAnaNavModuleBT::is_watchdog() 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) if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG)
{ {
...@@ -427,7 +470,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_watchdog() ...@@ -427,7 +470,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
BT::NodeStatus CIriAnaNavModuleBT::is_aborted() 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) if (nav_status == IRI_ANA_NAV_MODULE_ABORTED)
{ {
...@@ -440,7 +483,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_aborted() ...@@ -440,7 +483,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
BT::NodeStatus CIriAnaNavModuleBT::is_preempted() 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) if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED)
{ {
...@@ -452,7 +495,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_preempted() ...@@ -452,7 +495,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
BT::NodeStatus CIriAnaNavModuleBT::is_rejected() 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) if (nav_status == IRI_ANA_NAV_MODULE_REJECTED)
{ {
...@@ -464,7 +507,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_rejected() ...@@ -464,7 +507,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed() 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) if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL)
{ {
...@@ -476,7 +519,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed() ...@@ -476,7 +519,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present() 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) if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT)
{ {
...@@ -488,7 +531,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present() ...@@ -488,7 +531,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
BT::NodeStatus CIriAnaNavModuleBT::is_no_odom() 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) if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM)
{ {
...@@ -500,7 +543,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_odom() ...@@ -500,7 +543,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
BT::NodeStatus CIriAnaNavModuleBT::is_no_transform() 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) if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM)
{ {
...@@ -605,33 +648,41 @@ void CIriAnaNavModuleBT::stop() ...@@ -605,33 +648,41 @@ void CIriAnaNavModuleBT::stop()
void CIriAnaNavModuleBT::restart() void CIriAnaNavModuleBT::restart()
{ {
if (nav.is_finished()) ROS_INFO("-------RESTART-------");
if (!nav.is_finished())
{ {
ROS_INFO("-------RESTART-------"); stop();
for(auto& node : this->tree.nodes) while (!nav.is_finished());
{ orientation_goal_sent = false;
node.get()->halt(); position_goal_sent = false;
} pose_goal_sent = false;
this->status = BT::NodeStatus::IDLE;
if (print_tree_enabled)
{
BT::printTreeRecursively(this->tree.root_node);
}
} }
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) void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::string path, std::string tree_xml_file)
{ {
this->path=path; if (this->path != path || this->tree_xml_file != tree_xml_file)
this->tree_xml_file=tree_xml_file;
if (nav.is_finished())
{ {
ROS_INFO("-------CREATING NEW TREE-------"); 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 try
{ {
this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml"); this->tree = factory->createTreeFromFile(this->path + "/" + this->tree_xml_file + ".xml");
...@@ -669,36 +720,36 @@ void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::strin ...@@ -669,36 +720,36 @@ void CIriAnaNavModuleBT::create_tree(IriBehaviorTreeFactory *factory, std::strin
} }
catch (BT::RuntimeError &e) catch (BT::RuntimeError &e)
{ {
ROS_ERROR_STREAM("EXCEPTION " << e.what()); ROS_ERROR_STREAM("EXCEPTION: " << e.what());
} }
} }
else 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() void CIriAnaNavModuleBT::enable_cout_logger()
{ {
try if (this->logger_cout == NULL)
{ {
this->logger_cout = new BT::StdCoutLogger(this->tree); 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() 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()); 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() ...@@ -710,19 +761,19 @@ void CIriAnaNavModuleBT::enable_file_logger()
} }
else else
{ {
ROS_ERROR("EXCEPTION: FileLogger already enabled!"); ROS_WARN("FileLogger already enabled!");
} }
} }
void CIriAnaNavModuleBT::enable_zmq_publisher() void CIriAnaNavModuleBT::enable_zmq_publisher()
{ {
try if (this->publisher_zmq == NULL)
{ {
this->publisher_zmq = new BT::PublisherZMQ(this->tree); 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() ...@@ -735,30 +786,43 @@ void CIriAnaNavModuleBT::enable_print_tree()
BT::printTreeRecursively(this->tree.root_node); BT::printTreeRecursively(this->tree.root_node);
} }
print_tree_enabled = true; print_tree_enabled = true;
}} }
}
void CIriAnaNavModuleBT::disable_cout_logger() void CIriAnaNavModuleBT::disable_cout_logger()
{ {
delete this->logger_cout; if (this->logger_cout != NULL)
this->logger_cout = NULL; {
delete this->logger_cout;
this->logger_cout = NULL;
}
} }
void CIriAnaNavModuleBT::disable_minitrace_logger() void CIriAnaNavModuleBT::disable_minitrace_logger()
{ {
delete this->logger_minitrace; if (this->logger_minitrace != NULL)
this->logger_minitrace = NULL; {
delete this->logger_minitrace;
this->logger_minitrace = NULL;
}
} }
void CIriAnaNavModuleBT::disable_file_logger() void CIriAnaNavModuleBT::disable_file_logger()