From 3c5bb9b485930773911b834943eed6e722c42eed Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Wed, 31 Jan 2024 11:06:39 +0100 Subject: [PATCH 1/7] feat: allows message clearing behaviour to be customised by the implementer --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 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 311594b..7ea9fc6 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -173,6 +173,16 @@ class RosTopicSubNode : public BT::ConditionNode */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + /** Clear the message that has been processed. If returns true and no new message is + * received, before next call there will be no message to process. If returns false, + * the next call will process the same message again, if no new message received. + * + * This can be equated with latched vs non-latched topics in ros 1. + * + * @return true will clear the message after ticking/processing. + */ + virtual bool clear_processed_message() { return true; } + private: bool createSubscriber(const std::string& topic_name); @@ -294,8 +304,10 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; - + if (clear_processed_message()) + { + last_msg_ = nullptr; + } return status; } From ac2e0ac6938ad97e23973541b6d32c0396fc571c Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Fri, 2 Feb 2024 13:53:23 +0100 Subject: [PATCH 2/7] style: match camel case function name --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 7ea9fc6..a2ed0d4 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -181,7 +181,7 @@ class RosTopicSubNode : public BT::ConditionNode * * @return true will clear the message after ticking/processing. */ - virtual bool clear_processed_message() { return true; } + virtual bool clearProcessedMessage() { return true; } private: @@ -304,7 +304,7 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (clear_processed_message()) + if (clearProcessedMessage()) { last_msg_ = nullptr; } From d31eb1fc2833d0bacbdecfb04206e5ff86273b92 Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 13:43:27 +0200 Subject: [PATCH 3/7] fix: Get last message on late subscription and ensure connection to correct subscriber on topic_name change --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 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 a2ed0d4..8d3667c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -66,6 +66,7 @@ class RosTopicSubNode : public BT::ConditionNode // The callback will broadcast to all the instances of RosTopicSubNode auto callback = [this](const std::shared_ptr msg) { + last_msg = msg; broadcaster(msg); }; subscriber = node->create_subscription(topic_name, 1, callback, option); @@ -75,6 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; boost::signals2::signal)> broadcaster; + std::shared_ptr last_msg = nullptr; }; @@ -270,6 +272,11 @@ template inline sub_instance_ = it->second; } + // Check if there was a message received before the creation of this subscriber action + if (sub_instance_.last_msg) + { + last_msg_ = sub_instance_.last_msg; + } // add "this" as received of the broadcaster signal_connection_ = sub_instance_->broadcaster.connect( @@ -286,12 +293,18 @@ template inline // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber + std::string topic_name; + getInput("topic_name", topic_name); + if(!sub_instance_) { - std::string topic_name; - getInput("topic_name", topic_name); createSubscriber(topic_name); } + else if(topic_name_ != topic_name) + { + sub_instance_.reset(); + createSubscriber(topic_name_); + } auto CheckStatus = [](NodeStatus status) { From 06a508e16f8c13e211d1d8b0d566c866043c0a8d Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 14:17:36 +0200 Subject: [PATCH 4/7] fix: pointer references --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 8d3667c..780b27e 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -273,9 +273,9 @@ template inline } // Check if there was a message received before the creation of this subscriber action - if (sub_instance_.last_msg) + if (sub_instance_->last_msg) { - last_msg_ = sub_instance_.last_msg; + last_msg_ = sub_instance_->last_msg; } // add "this" as received of the broadcaster From 421e5c03bb2e0350d5aecc7dd9e113e58ac93a0b Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Tue, 2 Apr 2024 15:55:17 +0200 Subject: [PATCH 5/7] fix: create subscriber with new topic name --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 2 +- 1 file changed, 1 insertion(+), 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 780b27e..77bde12 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -303,7 +303,7 @@ template inline else if(topic_name_ != topic_name) { sub_instance_.reset(); - createSubscriber(topic_name_); + createSubscriber(topic_name); } auto CheckStatus = [](NodeStatus status) From e85eb03b3663f153af8fcd005bf3abd04a95d737 Mon Sep 17 00:00:00 2001 From: tony-p Date: Fri, 5 Apr 2024 16:18:56 +0200 Subject: [PATCH 6/7] Apply suggestions from code review style: update from review Co-authored-by: Davide Faconti --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 77bde12..f732f1c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -76,7 +76,7 @@ class RosTopicSubNode : public BT::ConditionNode rclcpp::CallbackGroup::SharedPtr callback_group; rclcpp::executors::SingleThreadedExecutor callback_group_executor; boost::signals2::signal)> broadcaster; - std::shared_ptr last_msg = nullptr; + std::shared_ptr last_msg; }; @@ -319,7 +319,7 @@ template inline auto status = CheckStatus (onTick(last_msg_)); if (clearProcessedMessage()) { - last_msg_ = nullptr; + last_msg_.reset(); } return status; } From 47d60b9ed83c4fe7150e9d0a31b5c8aedd35c146 Mon Sep 17 00:00:00 2001 From: Tony Paulussen Date: Thu, 11 Apr 2024 21:59:59 +0200 Subject: [PATCH 7/7] refactor: change name of retention method to latchLastMessage --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 f732f1c..fa3014d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -175,15 +175,15 @@ class RosTopicSubNode : public BT::ConditionNode */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; - /** Clear the message that has been processed. If returns true and no new message is - * received, before next call there will be no message to process. If returns false, + /** latch the message that has been processed. If returns false and no new message is + * received, before next call there will be no message to process. If returns true, * the next call will process the same message again, if no new message received. * * This can be equated with latched vs non-latched topics in ros 1. * - * @return true will clear the message after ticking/processing. + * @return false will clear the message after ticking/processing. */ - virtual bool clearProcessedMessage() { return true; } + virtual bool latchLastMessage() const { return false; } private: @@ -317,7 +317,7 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - if (clearProcessedMessage()) + if (!latchLastMessage()) { last_msg_.reset(); }