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..65c2196 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,14 @@ 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 }; /** * @brief Abstract class to wrap a Topic subscriber. @@ -35,14 +42,17 @@ namespace BT * * 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 +template class RosTopicSubNode : public BT::ConditionNode { public: @@ -132,6 +142,13 @@ class RosTopicSubNode : public BT::ConditionNode PortsList basic = { InputPort("topic_name", "__default__placeholder__", "Topic name") }; + if (read_mode == SubscriberReadMode::READ_CONFIGURABLE) + { + 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.")); + } basic.insert(addition.begin(), addition.end()); return basic; } @@ -165,8 +182,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), @@ -211,8 +228,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()) { @@ -247,8 +264,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. @@ -271,7 +288,11 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; + if (read_mode == SubscriberReadMode::READ_ONCE || + (read_mode == SubscriberReadMode::READ_CONFIGURABLE && !getInput("read_last").value())) + { + last_msg_ = nullptr; + } return status; }