From e7082812b8444d2001a361b034c69e74fa68aa41 Mon Sep 17 00:00:00 2001
From: abhagwan <abhagwan@upc.edu>
Date: Thu, 1 Aug 2019 12:28:48 +0200
Subject: [PATCH] Updated with changes made in iri_ana_nav_bt node
---
.../iri_ana_nav_module_bt.h | 8 +-
src/iri_ana_nav_module_bt.cpp | 200 ++++++++++++------
2 files changed, 137 insertions(+), 71 deletions(-)
diff --git a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
index bd2c57a..27e4a5b 100644
--- a/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
+++ b/include/iri_ana_nav_module/iri_ana_nav_module_bt.h
@@ -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;
diff --git a/src/iri_ana_nav_module_bt.cpp b/src/iri_ana_nav_module_bt.cpp
index 8022d1e..a8305fe 100644
--- a/src/iri_ana_nav_module_bt.cpp
+++ b/src/iri_ana_nav_module_bt.cpp
@@ -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()
{
--
GitLab