From f424900b7bf19ab87d01dd5a5268ea90bf6258f8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 2 Aug 2023 20:52:10 +0200 Subject: [PATCH] Fix Goal updater QoS (#3719) * Fix GoalUpdater QoS * Fixes --- .../plugins/decorator/goal_updater_node.cpp | 15 +++- .../decorator/test_goal_updater_node.cpp | 89 +++++++++++++++---- 2 files changed, 86 insertions(+), 18 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index febaa7de64..1f7226e6aa 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -46,7 +46,7 @@ GoalUpdater::GoalUpdater( sub_option.callback_group = callback_group_; goal_sub_ = node_->create_subscription( goal_updater_topic, - 10, + rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); } @@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_some(); - if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { - goal = last_goal_received_; + if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); + } } setOutput("output_goal", goal); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 9dc67914bd..cc6db706f2 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -95,6 +95,8 @@ TEST_F(GoalUpdaterTestFixture, test_tick) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -102,35 +104,92 @@ TEST_F(GoalUpdaterTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { - tree_->rootNode()->executeTick(); - } - + // tick tree without publishing updated goal and get updated_goal + tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); +} - EXPECT_EQ(updated_goal, goal); +TEST_F(GoalUpdaterTestFixture, test_older_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - goal_to_update.header.stamp = node_->now(); + goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goal_updater_pub->publish(goal_to_update); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + // expect to succeed and not update goal + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(updated_goal, goal); +} + +TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); - auto start = node_->now(); - while ((node_->now() - start).seconds() < 0.5) { - tree_->rootNode()->executeTick(); - goal_updater_pub->publish(goal_to_update); + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); - rclcpp::spin_some(node_); - } + // publish updated_goal older than goal + geometry_msgs::msg::PoseStamped goal_to_update_1; + goal_to_update_1.header.stamp = node_->now(); + goal_to_update_1.pose.position.x = 2.0; + + geometry_msgs::msg::PoseStamped goal_to_update_2; + goal_to_update_2.header.stamp = node_->now(); + goal_to_update_2.pose.position.x = 3.0; + goal_updater_pub->publish(goal_to_update_1); + goal_updater_pub->publish(goal_to_update_2); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); - EXPECT_NE(updated_goal, goal); - EXPECT_EQ(updated_goal, goal_to_update); + // expect to succeed + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + // expect to update goal with latest goal update + EXPECT_EQ(updated_goal, goal_to_update_2); } int main(int argc, char ** argv)