From 8454ac46405669e6e74c0d181afb66fda2ced966 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Wed, 18 Sep 2024 21:05:54 -0400 Subject: [PATCH 1/7] add unit tests for RosTopicSubNode --- behaviortree_ros2/CMakeLists.txt | 3 + behaviortree_ros2/test/CMakeLists.txt | 7 + .../test/test_bt_topic_sub_node.cpp | 179 ++++++++++++++++++ 3 files changed, 189 insertions(+) create mode 100644 behaviortree_ros2/test/CMakeLists.txt create mode 100644 behaviortree_ros2/test/test_bt_topic_sub_node.cpp diff --git a/behaviortree_ros2/CMakeLists.txt b/behaviortree_ros2/CMakeLists.txt index edf1440..ad2d108 100644 --- a/behaviortree_ros2/CMakeLists.txt +++ b/behaviortree_ros2/CMakeLists.txt @@ -37,6 +37,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} bt_executor_parameters) +if (BUILD_TESTING) + add_subdirectory(test) +endif() ###################################################### # INSTALL diff --git a/behaviortree_ros2/test/CMakeLists.txt b/behaviortree_ros2/test/CMakeLists.txt new file mode 100644 index 0000000..dee5d2c --- /dev/null +++ b/behaviortree_ros2/test/CMakeLists.txt @@ -0,0 +1,7 @@ +find_package(ament_cmake_gmock REQUIRED) +find_package(std_msgs REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +ament_add_gmock(test_${PROJECT_NAME} test_bt_topic_sub_node.cpp) +target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +ament_target_dependencies(test_${PROJECT_NAME} std_msgs) diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp new file mode 100644 index 0000000..0fb6993 --- /dev/null +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -0,0 +1,179 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace +{ +constexpr auto kTopicName = "/my_topic"; +constexpr auto kHistoryDepth = 1; +constexpr auto kPortIdTopicName = "topic_name"; + +using Empty = std_msgs::msg::Empty; +} // namespace + +namespace BT +{ + +class SubNode : public RosTopicSubNode +{ +public: + SubNode(const std::string& instance_name, const BT::NodeConfig& conf, + const RosNodeParams& params) + : RosTopicSubNode(instance_name, conf, params) + {} + +private: + NodeStatus onTick(const std::shared_ptr& last_msg) override + { + // GIVEN if any message is received on the topic + if(last_msg != nullptr) + { + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; + } +}; + +class TestBtTopicSubNode : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + node_ = std::make_shared("ros_node"); + + BT::assignDefaultRemapping(config_); + config_.blackboard = Blackboard::create(); + + executor_thread_ = std::thread([this]() { rclcpp::spin(node_); }); + } + + void TearDown() override + { + // std::this_thread::sleep_for(std::chrono::seconds(15)); + + rclcpp::shutdown(); + executor_thread_.join(); + } + + void createPublisher(const rclcpp::QoS& qos) + { + publisher_ = node_->create_publisher(kTopicName, qos); + } + + std::shared_ptr node_; + std::shared_ptr> publisher_; + + NodeConfig config_; + +private: + std::thread executor_thread_; +}; + +TEST_F(TestBtTopicSubNode, TopicAsParam) +{ + // Problems: exception thrown upon creation + // The BT node's executor needs to be spun once to register its subscriber with the publisher, and then spun a second time to actually receive the message. + // This causes sort of weird behavior -- if the subscriber is created in the constructor, then it alwaus functions like a latched topic for its first tick, and then follows the setting for remaining ticks. + // Probably always want to create the subscriber in the first tick if possible, since this lets us check QoS too. + + // GIVEN the blackboard does not contain the topic name + + // GIVEN the input params contain the topic name + RosNodeParams params; + params.nh = node_; + params.default_port_value = kTopicName; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN we create publisher with default QoS settings after creating the BT node + createPublisher(rclcpp::QoS(kHistoryDepth)); + + // GIVEN the publisher has published a message + publisher_->publish(std_msgs::build()); + + // WHEN the BT node is ticked + // THEN it succeeds + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); +} + +TEST_F(TestBtTopicSubNode, TopicFromStaticStringInPort) +{ + // GIVEN the input port directly contains the topic name + config_.input_ports[kPortIdTopicName] = kTopicName; + + // GIVEN the input params do not contain the topic name + RosNodeParams params; + params.nh = node_; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN we create publisher with default QoS settings after creating the BT node + createPublisher(rclcpp::QoS(kHistoryDepth)); + + // GIVEN the publisher has published a message + publisher_->publish(std_msgs::build()); + + // WHEN the BT node is ticked + // THEN it succeeds + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); +} + +TEST_F(TestBtTopicSubNode, TopicFromBlackboard) +{ + // GIVEN the input port is remapped to point to a value on the blackboard + config_.blackboard->set(kPortIdTopicName, kTopicName); + + // GIVEN the input params do not contain the topic name + RosNodeParams params; + params.nh = node_; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN we create publisher with default QoS settings after creating the BT node + createPublisher(rclcpp::QoS(kHistoryDepth)); + + // GIVEN the publisher has published a message + publisher_->publish(std_msgs::build()); + + // WHEN the BT node is ticked + // THEN it succeeds + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); +} + +TEST_F(TestBtTopicSubNode, TopicAsParamQoSBestEffort) +{ + // GIVEN the blackboard does not contain the topic name + + // GIVEN the input params contain the topic name + RosNodeParams params; + params.nh = node_; + params.default_port_value = kTopicName; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node + createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); + + // GIVEN the publisher has published a message + publisher_->publish(std_msgs::build()); + + // WHEN the BT node is ticked + // THEN it succeeds + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); +} +} // namespace BT From deb7dc5e45ded057e541d0e4e17c0c96418f8137 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Wed, 18 Sep 2024 22:07:42 -0400 Subject: [PATCH 2/7] rework RosTopicSubNode as a StatefulActionNode --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 136 +++++++++--------- .../test/test_bt_topic_sub_node.cpp | 15 +- 2 files changed, 78 insertions(+), 73 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 211f41c..92a54b4 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -14,12 +14,12 @@ #pragma once +#include +#include #include #include #include #include -#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/ros_node_params.hpp" #include @@ -42,7 +42,7 @@ namespace BT * 2. Otherwise, use the value in RosNodeParams::default_port_value */ template -class RosTopicSubNode : public BT::ConditionNode +class RosTopicSubNode : public BT::StatefulActionNode { public: // Type definitions @@ -131,9 +131,13 @@ class RosTopicSubNode : public BT::ConditionNode return providedBasicPorts({}); } - NodeStatus tick() override final; + NodeStatus onStart() override final; - /** Callback invoked in the tick. You must return either SUCCESS of FAILURE + NodeStatus onRunning() override final; + + void onHalted() override final; + + /** Callback invoked in the tick. You must return SUCCESS, FAILURE, or RUNNING. * * @param last_msg the latest message received, since the last tick. * Will be empty if no new message received. @@ -157,6 +161,8 @@ class RosTopicSubNode : public BT::ConditionNode private: bool createSubscriber(const std::string& topic_name); + + NodeStatus checkStatus(const NodeStatus& status) const; }; //---------------------------------------------------------------- @@ -187,51 +193,15 @@ template inline RosTopicSubNode::RosTopicSubNode(const std::string& instance_name, const NodeConfig& conf, const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), node_(params.nh) + : BT::StatefulActionNode(instance_name, conf), node_(params.nh) { - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) + // Check if default_port_value was used to provide a topic name. + if(!params.default_port_value.empty()) { - const std::string& bb_topic_name = portIt->second; - - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createSubscriber(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_topic_name)) - { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createSubscriber(bb_topic_name); - } - else - { - // do nothing - // createSubscriber will be invoked in the first tick(). - } - } - else - { - if(params.default_port_value.empty()) - { - throw std::logic_error("Both [topic_name] in the InputPort and the RosNodeParams " - "are empty."); - } - else - { - createSubscriber(params.default_port_value); - } + topic_name_ = params.default_port_value; } + + // If no value was provided through the params, assume that the topic name will be set through a port when the node is first ticked. } template @@ -282,40 +252,65 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) signal_connection_ = sub_instance_->broadcaster.connect( [this](const std::shared_ptr msg) { last_msg_ = msg; }); - topic_name_ = topic_name; return true; } +template +inline NodeStatus RosTopicSubNode::checkStatus(const NodeStatus& status) const +{ + if(!isStatusActive(status)) + { + throw std::logic_error("RosTopicSubNode: the callback must return SUCCESS, FAILURE, " + "or RUNNING"); + } + return status; +}; template -inline NodeStatus RosTopicSubNode::tick() +inline NodeStatus RosTopicSubNode::onStart() { - // 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(!topic_name.empty() && topic_name != "__default__placeholder__" && - topic_name != topic_name_) + // If the topic name was provided through an input port, is a valid topic name, and is different from the stored topic name, update the stored topic name to the new string. + if(const auto topic_name = getInput("topic_name"); + topic_name.has_value() && !topic_name->empty() && + topic_name.value() != "__default__placeholder__" && topic_name != topic_name_) { - sub_instance_.reset(); + topic_name_ = topic_name.value(); } - if(!sub_instance_) + createSubscriber(topic_name_); + + // Check to see if the subscriber has received a message since we initially created it. + sub_instance_->callback_group_executor.spin_some(); + + // If no message was received, return RUNNING + if(last_msg_ == nullptr) { - createSubscriber(topic_name); + return NodeStatus::RUNNING; } - auto CheckStatus = [](NodeStatus status) { - if(!isStatusCompleted(status)) - { - throw std::logic_error("RosTopicSubNode: the callback must return" - "either SUCCESS or FAILURE"); - } - return status; - }; + auto status = checkStatus(onTick(last_msg_)); + if(!latchLastMessage()) + { + last_msg_.reset(); + } + return status; +} + +template +inline NodeStatus RosTopicSubNode::onRunning() +{ + // Spin the subscriber to process any new message that has been received since the last tick sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus(onTick(last_msg_)); + + // If no message was received, return RUNNING + if(last_msg_ == nullptr) + { + return NodeStatus::RUNNING; + } + + // TODO(schornakj): handle timeout here + + auto status = checkStatus(onTick(last_msg_)); + if(!latchLastMessage()) { last_msg_.reset(); @@ -323,4 +318,7 @@ inline NodeStatus RosTopicSubNode::tick() return status; } +template +inline void RosTopicSubNode::onHalted() +{} } // namespace BT diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp index 0fb6993..0544cd5 100644 --- a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -34,12 +34,11 @@ class SubNode : public RosTopicSubNode private: NodeStatus onTick(const std::shared_ptr& last_msg) override { - // GIVEN if any message is received on the topic - if(last_msg != nullptr) + if(last_msg == nullptr) { - return NodeStatus::SUCCESS; + return NodeStatus::RUNNING; } - return NodeStatus::FAILURE; + return NodeStatus::SUCCESS; } }; @@ -100,6 +99,8 @@ TEST_F(TestBtTopicSubNode, TopicAsParam) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -123,6 +124,8 @@ TEST_F(TestBtTopicSubNode, TopicFromStaticStringInPort) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -146,6 +149,8 @@ TEST_F(TestBtTopicSubNode, TopicFromBlackboard) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -169,6 +174,8 @@ TEST_F(TestBtTopicSubNode, TopicAsParamQoSBestEffort) // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); From f7c93232cacbd040c88c696d0390b9fb83ee8fa5 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Wed, 18 Sep 2024 22:41:48 -0400 Subject: [PATCH 3/7] handle QoS reliability settings and non-existent publisher --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 66 +++++++++++++++++-- .../test/test_bt_topic_sub_node.cpp | 40 +++++++++-- 2 files changed, 94 insertions(+), 12 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 92a54b4..41e2a54 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -51,7 +53,9 @@ class RosTopicSubNode : public BT::StatefulActionNode protected: struct SubscriberInstance { - SubscriberInstance(std::shared_ptr node, const std::string& topic_name); + SubscriberInstance(std::shared_ptr node, const std::string& topic_name, + const std::size_t history_depth, + const rclcpp::ReliabilityPolicy& reliability); std::shared_ptr subscriber; rclcpp::CallbackGroup::SharedPtr callback_group; @@ -170,7 +174,8 @@ class RosTopicSubNode : public BT::StatefulActionNode //---------------------------------------------------------------- template inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( - std::shared_ptr node, const std::string& topic_name) + std::shared_ptr node, const std::string& topic_name, + const std::size_t history_depth, const rclcpp::ReliabilityPolicy& reliability) { // create a callback group for this particular instance callback_group = @@ -186,7 +191,8 @@ inline RosTopicSubNode::SubscriberInstance::SubscriberInstance( last_msg = msg; broadcaster(msg); }; - subscriber = node->create_subscription(topic_name, 1, callback, option); + subscriber = node->create_subscription( + topic_name, rclcpp::QoS(history_depth).reliability(reliability), callback, option); } template @@ -201,7 +207,8 @@ inline RosTopicSubNode::RosTopicSubNode(const std::string& instance_name, topic_name_ = params.default_port_value; } - // If no value was provided through the params, assume that the topic name will be set through a port when the node is first ticked. + // If no value was provided through the params, assume that the topic name will be set + // through a port when the node is first ticked. } template @@ -225,13 +232,41 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) throw RuntimeError("The ROS node went out of scope. RosNodeParams doesn't take the " "ownership of the node."); } + + const auto publisher_info = node->get_publishers_info_by_topic(topic_name); + if(publisher_info.empty()) + { + RCLCPP_INFO_ONCE(logger(), + "No publisher found on topic [%s]. Deferring creation of subscriber " + "until publisher exists.", + topic_name_.c_str()); + return false; + } + + rclcpp::ReliabilityPolicy publisher_reliability = + publisher_info.at(0).qos_profile().reliability(); + for(std::size_t i = 1; i < publisher_info.size(); i++) + { + if(publisher_reliability != publisher_info.at(i).qos_profile().reliability()) + { + RCLCPP_WARN_ONCE(logger(), + "Multiple publishers were found on topic [%s] with different QoS " + "reliability policies. The subscriber will use the reliability " + "setting from the first publisher it found, but this may result " + "in undesirable behavior.", + topic_name_.c_str()); + break; + } + } + subscriber_key_ = std::string(node->get_fully_qualified_name()) + "/" + topic_name; auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); if(it == registry.end() || it->second.expired()) { - sub_instance_ = std::make_shared(node, topic_name); + sub_instance_ = + std::make_shared(node, topic_name, 1, publisher_reliability); registry.insert({ subscriber_key_, sub_instance_ }); RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), @@ -268,7 +303,8 @@ inline NodeStatus RosTopicSubNode::checkStatus(const NodeStatus& status) cons template inline NodeStatus RosTopicSubNode::onStart() { - // If the topic name was provided through an input port, is a valid topic name, and is different from the stored topic name, update the stored topic name to the new string. + // If the topic name was provided through an input port, is a valid topic name, and is different + // from the stored topic name, update the stored topic name to the new string. if(const auto topic_name = getInput("topic_name"); topic_name.has_value() && !topic_name->empty() && topic_name.value() != "__default__placeholder__" && topic_name != topic_name_) @@ -276,9 +312,17 @@ inline NodeStatus RosTopicSubNode::onStart() topic_name_ = topic_name.value(); } - createSubscriber(topic_name_); + if(!createSubscriber(topic_name_)) + { + return NodeStatus::RUNNING; + } // Check to see if the subscriber has received a message since we initially created it. + // NOTE(schornakj): The subscriber needs to be spun twice to receive a published message if it had + // never been spun before the message was published. I think the first spin_once handles the discovery + // interaction between the publisher and subscriber, and the second spin_once actually transmits the message. + // Therefore, this BT node will never receive a published message on the first tick. + // This might depend on the ROS middleware implementation. sub_instance_->callback_group_executor.spin_some(); // If no message was received, return RUNNING @@ -298,6 +342,14 @@ inline NodeStatus RosTopicSubNode::onStart() template inline NodeStatus RosTopicSubNode::onRunning() { + if(!sub_instance_) + { + if(!createSubscriber(topic_name_)) + { + return NodeStatus::RUNNING; + } + } + // Spin the subscriber to process any new message that has been received since the last tick sub_instance_->callback_group_executor.spin_some(); diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp index 0544cd5..d226d08 100644 --- a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -99,7 +99,8 @@ TEST_F(TestBtTopicSubNode, TopicAsParam) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); - EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the BT node is RUNNING before the publisher publishes a message + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -124,7 +125,8 @@ TEST_F(TestBtTopicSubNode, TopicFromStaticStringInPort) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); - EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the BT node is RUNNING before the publisher publishes a message + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -149,7 +151,8 @@ TEST_F(TestBtTopicSubNode, TopicFromBlackboard) // GIVEN we create publisher with default QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth)); - EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the BT node is RUNNING before the publisher publishes a message + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -159,7 +162,7 @@ TEST_F(TestBtTopicSubNode, TopicFromBlackboard) EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); } -TEST_F(TestBtTopicSubNode, TopicAsParamQoSBestEffort) +TEST_F(TestBtTopicSubNode, QoSBestEffort) { // GIVEN the blackboard does not contain the topic name @@ -174,7 +177,8 @@ TEST_F(TestBtTopicSubNode, TopicAsParamQoSBestEffort) // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); - EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + // GIVEN the BT node is RUNNING before the publisher publishes a message + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); @@ -183,4 +187,30 @@ TEST_F(TestBtTopicSubNode, TopicAsParamQoSBestEffort) // THEN it succeeds EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); } + +TEST_F(TestBtTopicSubNode, PublisherNotAvailableAtStart) +{ + // GIVEN the blackboard does not contain the topic name + + // GIVEN the input params contain the topic name + RosNodeParams params; + params.nh = node_; + params.default_port_value = kTopicName; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN the BT node is RUNNING before the publisher is created + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + + // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node and after the node starts ticking + createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); + // AND the publisher has published a message + publisher_->publish(std_msgs::build()); + + // WHEN the BT node is ticked + // THEN it succeeds + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); +} } // namespace BT From 84669de7d76a25b1e538fd7f3e5507c2cb5a6680 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Wed, 18 Sep 2024 23:12:40 -0400 Subject: [PATCH 4/7] add TODO about weird behavior when creating publisher and sending a message between ticks --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 32 +++++++++++-------- .../test/test_bt_topic_sub_node.cpp | 8 +++-- 2 files changed, 24 insertions(+), 16 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 41e2a54..40aba4b 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -223,9 +223,6 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) throw RuntimeError("Can't call createSubscriber more than once"); } - // find SubscriberInstance in the registry - std::unique_lock lk(registryMutex()); - auto node = node_.lock(); if(!node) { @@ -236,10 +233,10 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) const auto publisher_info = node->get_publishers_info_by_topic(topic_name); if(publisher_info.empty()) { - RCLCPP_INFO_ONCE(logger(), - "No publisher found on topic [%s]. Deferring creation of subscriber " - "until publisher exists.", - topic_name_.c_str()); + RCLCPP_INFO(logger(), + "No publisher found on topic [%s]. Deferring creation of subscriber " + "until publisher exists.", + topic_name_.c_str()); return false; } @@ -261,6 +258,9 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) subscriber_key_ = std::string(node->get_fully_qualified_name()) + "/" + topic_name; + // find SubscriberInstance in the registry + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); auto it = registry.find(subscriber_key_); if(it == registry.end() || it->second.expired()) @@ -317,12 +317,16 @@ inline NodeStatus RosTopicSubNode::onStart() return NodeStatus::RUNNING; } - // Check to see if the subscriber has received a message since we initially created it. - // NOTE(schornakj): The subscriber needs to be spun twice to receive a published message if it had - // never been spun before the message was published. I think the first spin_once handles the discovery - // interaction between the publisher and subscriber, and the second spin_once actually transmits the message. - // Therefore, this BT node will never receive a published message on the first tick. - // This might depend on the ROS middleware implementation. + // NOTE(schornakj): rclcpp's spin_some function handles all queued work on the executor. + // However, the discovery interaction between the publisher and subscriber is also handled through + // this queue, and the process of receiving a published message is added to the queue only after + // the publisher and subscriber are connected. + // This means we need to call spin_some twice to ensure all possible communication is handled between + // our subscriber and a publisher. It's important to do this to avoid failure to receive messages + // in situations where the publisher has both appeared and sent a message in between ticks. + // This behavior might depend on the ROS middleware implementation. + + sub_instance_->callback_group_executor.spin_some(); sub_instance_->callback_group_executor.spin_some(); // If no message was received, return RUNNING @@ -350,7 +354,7 @@ inline NodeStatus RosTopicSubNode::onRunning() } } - // Spin the subscriber to process any new message that has been received since the last tick + sub_instance_->callback_group_executor.spin_some(); sub_instance_->callback_group_executor.spin_some(); // If no message was received, return RUNNING diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp index d226d08..365ca24 100644 --- a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -9,6 +9,7 @@ #include #include +#include #include namespace @@ -205,12 +206,15 @@ TEST_F(TestBtTopicSubNode, PublisherNotAvailableAtStart) // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node and after the node starts ticking createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); - // AND the publisher has published a message + + // TODO(Joe): Why does the node need to be ticked in between the publisher appearing and sending a message for the message to be received? Seems highly suspicious! + ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); + + // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); // WHEN the BT node is ticked // THEN it succeeds - EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); } } // namespace BT From 7d6e2c2976c051e6d196af89df45375ffe504521 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Thu, 19 Sep 2024 20:09:14 -0400 Subject: [PATCH 5/7] revise comments and simplify test fixture --- .../behaviortree_ros2/bt_topic_sub_node.hpp | 18 ++++++------- .../test/test_bt_topic_sub_node.cpp | 25 ++++--------------- 2 files changed, 12 insertions(+), 31 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 40aba4b..e40308d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -220,7 +220,7 @@ inline bool RosTopicSubNode::createSubscriber(const std::string& topic_name) } if(sub_instance_) { - throw RuntimeError("Can't call createSubscriber more than once"); + throw RuntimeError("Subscriber already exists"); } auto node = node_.lock(); @@ -312,21 +312,16 @@ inline NodeStatus RosTopicSubNode::onStart() topic_name_ = topic_name.value(); } + // If we fail to create a subscriber, exit early and try again on the next tick if(!createSubscriber(topic_name_)) { return NodeStatus::RUNNING; } - // NOTE(schornakj): rclcpp's spin_some function handles all queued work on the executor. - // However, the discovery interaction between the publisher and subscriber is also handled through - // this queue, and the process of receiving a published message is added to the queue only after - // the publisher and subscriber are connected. - // This means we need to call spin_some twice to ensure all possible communication is handled between - // our subscriber and a publisher. It's important to do this to avoid failure to receive messages - // in situations where the publisher has both appeared and sent a message in between ticks. - // This behavior might depend on the ROS middleware implementation. + // NOTE(schornakj): Something weird is happening here that prevents the subscriber from receiving a message if the publisher both initializes and publishes a message in between ticks. + // I think it has to do with the discovery process between the publisher and subscriber and how that relates to events being handled by the executor. + // This might depend on the middleware implemenation too. - sub_instance_->callback_group_executor.spin_some(); sub_instance_->callback_group_executor.spin_some(); // If no message was received, return RUNNING @@ -346,15 +341,16 @@ inline NodeStatus RosTopicSubNode::onStart() template inline NodeStatus RosTopicSubNode::onRunning() { + // If the subscriber wasn't initialized during onStart(), attempt to create it here. if(!sub_instance_) { + // If the subscriber still cannot be created, return RUNNING and try again on the next tick. if(!createSubscriber(topic_name_)) { return NodeStatus::RUNNING; } } - sub_instance_->callback_group_executor.spin_some(); sub_instance_->callback_group_executor.spin_some(); // If no message was received, return RUNNING diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp index 365ca24..c8f7ba4 100644 --- a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -4,14 +4,13 @@ #include #include #include +#include #include +#include #include #include #include -#include -#include - namespace { constexpr auto kTopicName = "/my_topic"; @@ -54,16 +53,11 @@ class TestBtTopicSubNode : public ::testing::Test BT::assignDefaultRemapping(config_); config_.blackboard = Blackboard::create(); - - executor_thread_ = std::thread([this]() { rclcpp::spin(node_); }); } void TearDown() override { - // std::this_thread::sleep_for(std::chrono::seconds(15)); - rclcpp::shutdown(); - executor_thread_.join(); } void createPublisher(const rclcpp::QoS& qos) @@ -75,18 +69,10 @@ class TestBtTopicSubNode : public ::testing::Test std::shared_ptr> publisher_; NodeConfig config_; - -private: - std::thread executor_thread_; }; TEST_F(TestBtTopicSubNode, TopicAsParam) { - // Problems: exception thrown upon creation - // The BT node's executor needs to be spun once to register its subscriber with the publisher, and then spun a second time to actually receive the message. - // This causes sort of weird behavior -- if the subscriber is created in the constructor, then it alwaus functions like a latched topic for its first tick, and then follows the setting for remaining ticks. - // Probably always want to create the subscriber in the first tick if possible, since this lets us check QoS too. - // GIVEN the blackboard does not contain the topic name // GIVEN the input params contain the topic name @@ -189,7 +175,7 @@ TEST_F(TestBtTopicSubNode, QoSBestEffort) EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); } -TEST_F(TestBtTopicSubNode, PublisherNotAvailableAtStart) +TEST_F(TestBtTopicSubNode, PublisherCreatedAfterFirstTick) { // GIVEN the blackboard does not contain the topic name @@ -204,15 +190,14 @@ TEST_F(TestBtTopicSubNode, PublisherNotAvailableAtStart) // GIVEN the BT node is RUNNING before the publisher is created ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); - // GIVEN we create publisher with BestEffort reliability QoS settings after creating the BT node and after the node starts ticking - createPublisher(rclcpp::QoS(kHistoryDepth).best_effort()); + // GIVEN we create publisher with Reliable reliability QoS settings after creating the BT node and after the node starts ticking + createPublisher(rclcpp::QoS(kHistoryDepth).reliable()); // TODO(Joe): Why does the node need to be ticked in between the publisher appearing and sending a message for the message to be received? Seems highly suspicious! ASSERT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::RUNNING)); // GIVEN the publisher has published a message publisher_->publish(std_msgs::build()); - // WHEN the BT node is ticked // THEN it succeeds EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); From 7abb2c8533ba0ffdab9ae2055d1599328c591835 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Thu, 19 Sep 2024 20:40:20 -0400 Subject: [PATCH 6/7] move latching check to onStart --- .../include/behaviortree_ros2/bt_topic_sub_node.hpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 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 e40308d..a76dedd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -318,6 +318,13 @@ inline NodeStatus RosTopicSubNode::onStart() return NodeStatus::RUNNING; } + // Discard any previous message unless latching is enabled. + // This enforces that the message must have been received after the current call to onStart(), even if this node has been ticked to completion previously. + if(!latchLastMessage()) + { + last_msg_.reset(); + } + // NOTE(schornakj): Something weird is happening here that prevents the subscriber from receiving a message if the publisher both initializes and publishes a message in between ticks. // I think it has to do with the discovery process between the publisher and subscriber and how that relates to events being handled by the executor. // This might depend on the middleware implemenation too. @@ -363,10 +370,6 @@ inline NodeStatus RosTopicSubNode::onRunning() auto status = checkStatus(onTick(last_msg_)); - if(!latchLastMessage()) - { - last_msg_.reset(); - } return status; } From 846ab5d7e59fdf59cded2be373b39b8086a38d34 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Thu, 19 Sep 2024 21:07:14 -0400 Subject: [PATCH 7/7] add tests for exceptional error cases --- .../test/test_bt_topic_sub_node.cpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp index c8f7ba4..c37db63 100644 --- a/behaviortree_ros2/test/test_bt_topic_sub_node.cpp +++ b/behaviortree_ros2/test/test_bt_topic_sub_node.cpp @@ -202,4 +202,39 @@ TEST_F(TestBtTopicSubNode, PublisherCreatedAfterFirstTick) // THEN it succeeds EXPECT_THAT(bt_node.executeTick(), testing::Eq(NodeStatus::SUCCESS)); } + +TEST_F(TestBtTopicSubNode, ExceptionIfNoTopic) +{ + // GIVEN the blackboard does not contain the topic name + + // GIVEN the input params do not contain the topic name + RosNodeParams params; + params.nh = node_; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // WHEN the BT node is ticked + // THEN it throws an exception + EXPECT_THROW(bt_node.executeTick(), BT::RuntimeError); +} + +TEST_F(TestBtTopicSubNode, ExceptionIfRosNodeReset) +{ + // GIVEN the blackboard does not contain the topic name + + // GIVEN the input params do not contain the topic name + RosNodeParams params; + params.nh = node_; + + // GIVEN we pass in the params and the blackboard when creating the BT node + SubNode bt_node("test_node", config_, params); + + // GIVEN the ROS node is reset prior to the first tick + node_.reset(); + + // WHEN the BT node is ticked + // THEN it throws an exception + EXPECT_THROW(bt_node.executeTick(), BT::RuntimeError); +} } // namespace BT