Skip to content

Add message retention options to subscriber action #42

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 4 commits into from
Closed

Add message retention options to subscriber action #42

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
dd8cea4
feat: Make subscriber reading configurable
tony-p Dec 28, 2023
402fe5d
fix: use template variable to configure due to static ports function
tony-p Dec 29, 2023
b22dc64
docs: add configuration documentation
tony-p Dec 29, 2023
d37ac4e
style: break long line
tony-p Dec 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 31 additions & 10 deletions behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -35,14 +42,17 @@ namespace BT
*
* The corresponding wrapper would be:
*
* class SubscriberNode: RosTopicSubNode<std_msgs::msg::String>
* class SubscriberNode: RosTopicSubNode<std_msgs::msg::String, SubscriberReadMode::READ_ONCE>
*
* 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 TopicT>
template<class TopicT, SubscriberReadMode read_mode = SubscriberReadMode::READ_ONCE>
class RosTopicSubNode : public BT::ConditionNode
{
public:
Expand Down Expand Up @@ -132,6 +142,13 @@ class RosTopicSubNode : public BT::ConditionNode
PortsList basic = {
InputPort<std::string>("topic_name", "__default__placeholder__", "Topic name")
};
if (read_mode == SubscriberReadMode::READ_CONFIGURABLE)
{
basic.insert(InputPort<bool>(
"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;
}
Expand Down Expand Up @@ -165,8 +182,8 @@ class RosTopicSubNode : public BT::ConditionNode
//---------------------- DEFINITIONS -----------------------------
//----------------------------------------------------------------

template<class T> inline
RosTopicSubNode<T>::RosTopicSubNode(const std::string & instance_name,
template<class T, SubscriberReadMode read_mode = SubscriberReadMode::READ_ONCE> inline
RosTopicSubNode<T, read_mode>::RosTopicSubNode(const std::string & instance_name,
const NodeConfig &conf,
const RosNodeParams& params)
: BT::ConditionNode(instance_name, conf),
Expand Down Expand Up @@ -211,8 +228,8 @@ template<class T> inline
}
}

template<class T> inline
bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name)
template<class T, SubscriberReadMode read_mode> inline
bool RosTopicSubNode<T, read_mode>::createSubscriber(const std::string& topic_name)
{
if(topic_name.empty())
{
Expand Down Expand Up @@ -247,8 +264,8 @@ template<class T> inline
}


template<class T> inline
NodeStatus RosTopicSubNode<T>::tick()
template<class T, SubscriberReadMode read_mode> inline
NodeStatus RosTopicSubNode<T, read_mode>::tick()
{
// First, check if the subscriber_ is valid and that the name of the
// topic_name in the port didn't change.
Expand All @@ -271,7 +288,11 @@ template<class T> 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<bool>("read_last").value()))
{
last_msg_ = nullptr;
}

return status;
}
Expand Down