From dd8cea4bc25918262e3bf1adb35c750816dcfe3c Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Thu, 28 Dec 2023 23:58:42 +0100 Subject: [PATCH 1/4] feat: Make subscriber reading configurable --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 4a97079..4cd4d75 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -50,6 +50,13 @@ class RosTopicSubNode : public BT::ConditionNode using Subscriber = typename rclcpp::Subscription; protected: + enum class ReadMode { READ_ONCE, READ_LATCH, READ_CONFIGURABLE }; + + virtual ReadMode getReadMode(void) + { + return ReadMode::READ_ONCE; + } + struct SubscriberInstance { void init(std::shared_ptr node, const std::string& topic_name) @@ -132,6 +139,13 @@ class RosTopicSubNode : public BT::ConditionNode PortsList basic = { InputPort("topic_name", "__default__placeholder__", "Topic name") }; + if (getReadMode() == ReadMode::READ_CONFIGURABLE) + { + basic.push_back(InputPort( + "read_last", + false, + "Read mode. True if read last message, even if it has already been seen. False if read only new messages.")); + } basic.insert(addition.begin(), addition.end()); return basic; } @@ -271,7 +285,10 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; + if (getReadMode() == ReadMode::READ_ONCE || (getReadMode() == ReadMode::READ_CONFIGURABLE && !config().input_ports.get("read_last"))) + { + last_msg_ = nullptr; + } return status; } From 402fe5d31a05cab6e168c67eb7fd4e870c1953ba Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Fri, 29 Dec 2023 12:10:10 +0100 Subject: [PATCH 2/4] fix: use template variable to configure due to static ports function --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 29 +++++++------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 4cd4d75..d21bdda 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -26,7 +26,7 @@ namespace BT { - +enum class SubscriberReadMode { READ_ONCE, READ_LATCH, READ_CONFIGURABLE }; /** * @brief Abstract class to wrap a Topic subscriber. @@ -42,7 +42,7 @@ namespace BT * 1. If a value is passes in the InputPort "topic_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value */ -template +template class RosTopicSubNode : public BT::ConditionNode { public: @@ -50,13 +50,6 @@ class RosTopicSubNode : public BT::ConditionNode using Subscriber = typename rclcpp::Subscription; protected: - enum class ReadMode { READ_ONCE, READ_LATCH, READ_CONFIGURABLE }; - - virtual ReadMode getReadMode(void) - { - return ReadMode::READ_ONCE; - } - struct SubscriberInstance { void init(std::shared_ptr node, const std::string& topic_name) @@ -139,9 +132,9 @@ class RosTopicSubNode : public BT::ConditionNode PortsList basic = { InputPort("topic_name", "__default__placeholder__", "Topic name") }; - if (getReadMode() == ReadMode::READ_CONFIGURABLE) + if (read_mode == SubscriberReadMode::READ_CONFIGURABLE) { - basic.push_back(InputPort( + basic.insert(InputPort( "read_last", false, "Read mode. True if read last message, even if it has already been seen. False if read only new messages.")); @@ -179,8 +172,8 @@ class RosTopicSubNode : public BT::ConditionNode //---------------------- DEFINITIONS ----------------------------- //---------------------------------------------------------------- -template inline - RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, +template inline + RosTopicSubNode::RosTopicSubNode(const std::string & instance_name, const NodeConfig &conf, const RosNodeParams& params) : BT::ConditionNode(instance_name, conf), @@ -225,8 +218,8 @@ template inline } } -template inline - bool RosTopicSubNode::createSubscriber(const std::string& topic_name) +template inline + bool RosTopicSubNode::createSubscriber(const std::string& topic_name) { if(topic_name.empty()) { @@ -261,8 +254,8 @@ template inline } -template inline - NodeStatus RosTopicSubNode::tick() +template inline + NodeStatus RosTopicSubNode::tick() { // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. @@ -285,7 +278,7 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (getReadMode() == ReadMode::READ_ONCE || (getReadMode() == ReadMode::READ_CONFIGURABLE && !config().input_ports.get("read_last"))) + if (read_mode == SubscriberReadMode::READ_ONCE || (read_mode == SubscriberReadMode::READ_CONFIGURABLE && !getInput("read_last").value())) { last_msg_ = nullptr; } From b22dc64d89043cfa069972174497b0849285ae56 Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Fri, 29 Dec 2023 14:14:17 +0100 Subject: [PATCH 3/4] docs: add configuration documentation --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index d21bdda..6d2e98a 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -26,6 +26,13 @@ namespace BT { +/** + * @brief Settings for how the subscriber should read messages + * + * READ_ONCE: Read the message once then clear it + * READ_LATCH: Keep reading the same message until a new one is received + * READ_CONFIGURABLE: Read the message once then clear it, unless the "read_last" port is set to true + */ enum class SubscriberReadMode { READ_ONCE, READ_LATCH, READ_CONFIGURABLE }; /** @@ -35,12 +42,15 @@ enum class SubscriberReadMode { READ_ONCE, READ_LATCH, READ_CONFIGURABLE }; * * The corresponding wrapper would be: * - * class SubscriberNode: RosTopicSubNode + * class SubscriberNode: RosTopicSubNode * * The name of the topic will be determined as follows: * * 1. If a value is passes in the InputPort "topic_name", use that * 2. Otherwise, use the value in RosNodeParams::default_port_value + * + * To configure the reading mode, set the 2nd class template parameter to one of the options in SubscriberReadMode. + * If not set READ_ONCE will be used by default for backwards compatibility. */ template class RosTopicSubNode : public BT::ConditionNode From d37ac4ebed7be0ac2ef7520f2989e429e2617f3d Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Fri, 29 Dec 2023 14:30:55 +0100 Subject: [PATCH 4/4] style: break long line --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 6d2e98a..65c2196 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -288,7 +288,8 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (read_mode == SubscriberReadMode::READ_ONCE || (read_mode == SubscriberReadMode::READ_CONFIGURABLE && !getInput("read_last").value())) + if (read_mode == SubscriberReadMode::READ_ONCE || + (read_mode == SubscriberReadMode::READ_CONFIGURABLE && !getInput("read_last").value())) { last_msg_ = nullptr; }