From d0a4f01a5d046aeb1bb26c159ef05cd5b050f2ad Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Mon, 18 Dec 2023 13:12:11 +0100
Subject: [PATCH] Added timeout functions
---
CMakeLists.txt | 3 +-
include/iri_bt_basic_nodes.h | 72 ++++++++++++++++++++++
package.xml | 2 +-
src/iri_bt_basic_nodes.cpp | 113 +++++++++++++++++++++++++++++++++++
src/xml/bt_definitions.xml | 19 ++++++
5 files changed, 207 insertions(+), 2 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dc74094..c4fb859 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
tf2_ros
geometry_msgs
sensor_msgs
+ iri_ros_tools
)
## System dependencies are found with CMake's conventions
@@ -98,7 +99,7 @@ set(head_helpers_name head_helpers)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} ${head_helpers_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs
+ CATKIN_DEPENDS roscpp dynamic_reconfigure diagnostic_updater behaviortree_cpp_v3 iri_behaviortree tf2_ros geometry_msgs sensor_msgs iri_ros_tools
# DEPENDS system_lib
DEPENDS Boost
)
diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h
index 820043f..9968ef5 100644
--- a/include/iri_bt_basic_nodes.h
+++ b/include/iri_bt_basic_nodes.h
@@ -4,6 +4,7 @@
#include "iri_bt_factory.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
+#include <iri_ros_tools/timeout.h>
class IriBTBasicNodes
{
@@ -25,6 +26,7 @@ class IriBTBasicNodes
// variables to know whether start or stop have been demanded by the user
tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener;
+ std::map<std::string, CROSTimeout> timeouts;
protected:
/**
@@ -151,6 +153,76 @@ class IriBTBasicNodes
*
*/
BT::NodeStatus print_msg(BT::TreeNode& _self);
+
+ /**
+ * \brief Action to create (if necessary) and to start a timeout
+ *
+ * It has the following input ports:
+ *
+ * tout_name (std::string): Timeout name.
+ *
+ * duration (double): Duration time in seconds.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus start_timeout(BT::TreeNode& _self);
+
+ /**
+ * \brief Action to stop a timeout
+ *
+ * It has the following input ports:
+ *
+ * tout_name (std::string): Timeout name.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus stop_timeout(BT::TreeNode& _self);
+
+ /**
+ * \brief Action to stop a timeout
+ *
+ * It has the following input ports:
+ *
+ * tout_name (std::string): Timeout name.
+ *
+ * It also has the following output ports:
+ *
+ * timed_out (bool): If is timed out.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus get_timed_out(BT::TreeNode& _self);
+
+ /**
+ * \brief Action to erase a timeout
+ *
+ * It has the following input ports:
+ *
+ * tout_name (std::string): Timeout name.
+ *
+ * \param _self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus erase_timeout(BT::TreeNode& _self);
};
#endif
diff --git a/package.xml b/package.xml
index b45eb3e..e74fd57 100644
--- a/package.xml
+++ b/package.xml
@@ -21,7 +21,7 @@
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
-
+ <depend>iri_ros_tools</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index 5955797..58780d4 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -13,6 +13,9 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
BT::PortsList compute_distance_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose1"),BT::InputPort<geometry_msgs::PoseStamped>("pose2"),BT::OutputPort<double>("distance")};
BT::PortsList compare_ports = {BT::InputPort<double>("value1"),BT::InputPort<double>("value2")};
BT::PortsList print_msg_ports = {BT::InputPort<std::string>("msg"), BT::InputPort<std::string>("type")};
+ BT::PortsList start_timeout_ports = {BT::InputPort<std::string>("tout_name"), BT::InputPort<double>("duration")};
+ BT::PortsList stop_timeout_ports = {BT::InputPort<std::string>("tout_name")};
+ BT::PortsList get_timed_out_ports = {BT::InputPort<std::string>("tout_name"), BT::OutputPort<bool>("timed_out")};
//Registration of conditions
factory.registerSimpleCondition("is_variable_true", std::bind(&IriBTBasicNodes::is_variable_true, this, std::placeholders::_1),check_variable_ports);
@@ -25,6 +28,10 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("compare_bigger", std::bind(&IriBTBasicNodes::compare_bigger, this, std::placeholders::_1),compare_ports);
factory.registerSimpleAction("compare_smaller", std::bind(&IriBTBasicNodes::compare_smaller, this, std::placeholders::_1),compare_ports);
factory.registerSimpleAction("print_msg", std::bind(&IriBTBasicNodes::print_msg, this, std::placeholders::_1), print_msg_ports);
+ factory.registerSimpleAction("start_timeout", std::bind(&IriBTBasicNodes::start_timeout, this, std::placeholders::_1), start_timeout_ports);
+ factory.registerSimpleAction("stop_timeout", std::bind(&IriBTBasicNodes::stop_timeout, this, std::placeholders::_1), stop_timeout_ports);
+ factory.registerSimpleAction("get_timed_out", std::bind(&IriBTBasicNodes::get_timed_out, this, std::placeholders::_1), get_timed_out_ports);
+ factory.registerSimpleAction("erase_timeout", std::bind(&IriBTBasicNodes::erase_timeout, this, std::placeholders::_1), stop_timeout_ports);
}
BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
@@ -237,3 +244,109 @@ BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
ROS_INFO_STREAM(msg);
return BT::NodeStatus::SUCCESS;
}
+
+BT::NodeStatus IriBTBasicNodes::start_timeout(BT::TreeNode& _self)
+{
+ BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+ BT::Optional<double> duration_p = _self.getInput<double>("duration");
+ std::string name;
+ double duration;
+ std::map<std::string, CROSTimeout>::iterator it;
+
+ if (!name_p || !duration_p)
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::start_timeout-> Incorrect or missing input. It needs the following input ports:"
+ << " tout_name (sdt::string) and duration (double)");
+ return BT::NodeStatus::FAILURE;
+ }
+ name = name_p.value();
+ duration = duration_p.value();
+
+ it = this->timeouts.find(name);
+ if (it == this->timeouts.end())
+ {
+ CROSTimeout tout;
+ this->timeouts[name] = tout;
+ it = this->timeouts.find(name);
+ }
+ it->second.start(ros::Duration(duration));
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::stop_timeout(BT::TreeNode& _self)
+{
+ BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+ std::string name;
+ std::map<std::string, CROSTimeout>::iterator it;
+
+ if (!name_p)
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout-> Incorrect or missing input. It needs the following input ports:"
+ << " tout_name (sdt::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+ name = name_p.value();
+
+ it = this->timeouts.find(name);
+ if (it == this->timeouts.end())
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout-> No timeout found with name " << name);
+ return BT::NodeStatus::FAILURE;
+ }
+ it->second.stop();
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::get_timed_out(BT::TreeNode& _self)
+{
+ BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+ std::string name;
+ std::map<std::string, CROSTimeout>::iterator it;
+ bool timed_out = false;
+
+ _self.setOutput("timed_out", timed_out);
+ if (!name_p)
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out-> Incorrect or missing input. It needs the following input ports:"
+ << " tout_name (sdt::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+ name = name_p.value();
+
+ it = this->timeouts.find(name);
+ if (it == this->timeouts.end())
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out-> No timeout found with name " << name);
+ return BT::NodeStatus::FAILURE;
+ }
+ timed_out = it->second.timed_out();
+ _self.setOutput("timed_out", timed_out);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus IriBTBasicNodes::erase_timeout(BT::TreeNode& _self)
+{
+ BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name");
+ std::string name;
+ std::map<std::string, CROSTimeout>::iterator it;
+
+ if (!name_p)
+ {
+ ROS_ERROR_STREAM("IriBTBasicNodes::erase_timeout-> Incorrect or missing input. It needs the following input ports:"
+ << " tout_name (sdt::string)");
+ return BT::NodeStatus::FAILURE;
+ }
+ name = name_p.value();
+
+ it = this->timeouts.find(name);
+ if (it == this->timeouts.end())
+ {
+ ROS_WARN_STREAM("IriBTBasicNodes::erase_timeout-> No timeout found with name " << name);
+ return BT::NodeStatus::SUCCESS;
+ }
+ this->timeouts.erase(it);
+
+ return BT::NodeStatus::SUCCESS;
+}
\ No newline at end of file
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index c96154b..8471bf2 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -24,6 +24,11 @@
<input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
<output_port name="distance"> (double): Distance.</output_port>
</Action>
+ <Action ID="compute_y_distance">
+ <input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
+ <input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
+ <output_port name="distance"> (double): Distance.</output_port>
+ </Action>
<Action ID="compare_bigger">
<input_port name="value1"> (double): Value 1.</input_port>
<input_port name="value2"> (double): Value 2.</input_port>
@@ -69,6 +74,20 @@
<input_port name="param_name"> (std::string): Parameter name.</input_port>
<output_port name="param_value"> (bool): Parameter value.</output_port>
</Action>
+ <Action ID="start_timeout">
+ <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+ <input_port name="duration"> (double): Duration time in second.</input_port>
+ </Action>
+ <Action ID="stop_timeout">
+ <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+ </Action>
+ <Action ID="get_timed_out">
+ <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+ <output_port name="timed_out"> (bool): If is timed out.</output_port>
+ </Action>
+ <Action ID="erase_timeout">
+ <input_port name="tout_name"> (std::string): Timeout name.</input_port>
+ </Action>
<!-- Head helepers -->
<Action ID="from_img_point_to_pan_tilt">
<input_port name="x"> Image x coordinate</input_port>
--
GitLab