diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index b40b8f825b8..0552bc9203d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -86,12 +86,11 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; - Goals last_goals_received_; + nav2_msgs::msg::PoseStampedArray last_goals_received_; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::mutex mutex_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index fd32e4c8c4d..2df8fb4c4da 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -83,12 +83,26 @@ inline BT::NodeStatus GoalUpdater::tick() last_goal_received_time.seconds(), goal_time.seconds()); } } + setOutput("output_goal", goal); - if (!last_goals_received_.empty()) { - goals = last_goals_received_; + if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) { + auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); + rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); + for (const auto & g : goals) { + if (rclcpp::Time(g.header.stamp) > most_recent_goal_time) { + most_recent_goal_time = rclcpp::Time(g.header.stamp); + } + } + if (last_goals_received_time > most_recent_goal_time) { + goals = last_goals_received_.poses; + } else { + RCLCPP_WARN( + node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " + "current goals (oldest: %f). Ignoring the received goals.", + last_goals_received_time.seconds(), most_recent_goal_time.seconds()); + } } - setOutput("output_goal", goal); setOutput("output_goals", goals); return child_node_->executeTick(); } @@ -96,15 +110,13 @@ inline BT::NodeStatus GoalUpdater::tick() void GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::lock_guard lock(mutex_); last_goal_received_ = *msg; } void GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg) { - std::lock_guard lock(mutex_); - last_goals_received_ = msg->poses; + last_goals_received_ = *msg; } } // namespace nav2_behavior_tree 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 821b2a64492..15296f7b4e9 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 @@ -26,6 +26,7 @@ #include "nav2_behavior_tree/utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" +typedef std::vector Goals; class GoalUpdaterTestFixture : public ::testing::Test { @@ -88,26 +89,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick) 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; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); } TEST_F(GoalUpdaterTestFixture, test_older_goal_update) @@ -117,7 +121,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) R"( - + @@ -126,26 +130,38 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); + auto goals_updater_pub = + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; + nav2_msgs::msg::PoseStampedArray goals_to_update; goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goals_to_update.header.stamp = goal_to_update.header.stamp; + goals_to_update.poses.push_back(goal_to_update); goal_updater_pub->publish(goal_to_update); + goals_updater_pub->publish(goals_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); // expect to succeed and not update goal EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(updated_goal, goal); + EXPECT_EQ(updated_goals, goals); } TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) @@ -155,7 +171,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) R"( - + @@ -164,32 +180,48 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); + auto goals_updater_pub = + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update_1; + nav2_msgs::msg::PoseStampedArray goals_to_update_1; goal_to_update_1.header.stamp = node_->now(); goal_to_update_1.pose.position.x = 2.0; + goals_to_update_1.header.stamp = goal_to_update_1.header.stamp; + goals_to_update_1.poses.push_back(goal_to_update_1); geometry_msgs::msg::PoseStamped goal_to_update_2; + nav2_msgs::msg::PoseStampedArray goals_to_update_2; goal_to_update_2.header.stamp = node_->now(); goal_to_update_2.pose.position.x = 3.0; + goals_to_update_2.header.stamp = goal_to_update_2.header.stamp; + goals_to_update_2.poses.push_back(goal_to_update_2); goal_updater_pub->publish(goal_to_update_1); + goals_updater_pub->publish(goals_to_update_1); goal_updater_pub->publish(goal_to_update_2); + goals_updater_pub->publish(goals_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); // 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); + EXPECT_EQ(updated_goals, goals_to_update_2.poses); } int main(int argc, char ** argv)