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..fa3014d 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; }; @@ -173,6 +175,16 @@ class RosTopicSubNode : public BT::ConditionNode */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + /** 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 false will clear the message after ticking/processing. + */ + virtual bool latchLastMessage() const { return false; } + private: bool createSubscriber(const std::string& topic_name); @@ -260,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( @@ -276,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) { @@ -294,8 +317,10 @@ template inline }; sub_instance_->callback_group_executor.spin_some(); auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; - + if (!latchLastMessage()) + { + last_msg_.reset(); + } return status; }