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 ddce12cf02e..f9e12088541 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 @@ -1,5 +1,6 @@ // Copyright (c) 2018 Intel Corporation // Copyright (c) 2020 Francisco Martin Rico +// Copyright (c) 2024 Angsa Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,6 +21,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/decorator_node.h" @@ -52,9 +54,11 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::OutputPort( - "output_goal", - "Received Goal by subscription"), + BT::InputPort("input_goals", "Original Goals"), + BT::OutputPort("output_goal", + "Received Goal by subscription"), + BT::OutputPort("output_goals", + "Received Goals by subscription") }; } @@ -71,9 +75,19 @@ class GoalUpdater : public BT::DecoratorNode */ void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + /** + * @brief Callback function for goals update topic + * @param msg Shared pointer to geometry_msgs::msg::PoseStampedArray message + */ + void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg); + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; + bool last_goal_received_set_{false}; + geometry_msgs::msg::PoseStampedArray last_goals_received_; + bool last_goals_received_set_{false}; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index cf48b4c5e32..0afe09c4bdd 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -328,7 +328,9 @@ Original goal in + Original goals in Output goal set by subscription + Output goals set by subscription diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 4127bc68439..c40b712b937 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2018 Intel Corporation // Copyright (c) 2020 Francisco Martin Rico +// Copyright (c) 2024 Angsa Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,7 +17,6 @@ #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -40,7 +40,9 @@ GoalUpdater::GoalUpdater( callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); std::string goal_updater_topic; + std::string goals_updater_topic; node_->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + node_->get_parameter_or("goals_updater_topic", goals_updater_topic, "goals_update"); rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; @@ -49,32 +51,72 @@ GoalUpdater::GoalUpdater( rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); + goals_sub_ = node_->create_subscription( + goals_updater_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&GoalUpdater::callback_updated_goals, this, _1), + sub_option); } inline BT::NodeStatus GoalUpdater::tick() { geometry_msgs::msg::PoseStamped goal; + geometry_msgs::msg::PoseStampedArray goals; getInput("input_goal", goal); + getInput("input_goals", goals); // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin callback_group_executor_.spin_all(std::chrono::milliseconds(1)); callback_group_executor_.spin_all(std::chrono::milliseconds(49)); - 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_; + if (last_goal_received_set_) { + if (last_goal_received_.header.stamp == rclcpp::Time(0)) { + // if the goal doesn't have a timestamp, we reject it + RCLCPP_WARN( + node_->get_logger(), "The received goal has no timestamp. Ignoring."); + setOutput("output_goal", goal); } else { + 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) { + setOutput("output_goal", last_goal_received_); + } else { + RCLCPP_INFO( + 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); + } + } + } else { + setOutput("output_goal", goal); + } + + if (last_goals_received_set_) { + if (last_goals_received_.poses.empty()) { + setOutput("output_goals", goals); + } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { 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()); + node_->get_logger(), "The received goals array has no timestamp. Ignoring."); + setOutput("output_goals", goals); + } else { + auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); + auto goals_time = rclcpp::Time(goals.header.stamp); + if (last_goals_received_time >= goals_time) { + setOutput("output_goals", last_goals_received_); + } else { + RCLCPP_INFO( + node_->get_logger(), "The timestamp of the received goals (%f) is older than the " + "current goals (%f). Ignoring the received goals.", + last_goals_received_time.seconds(), goals_time.seconds()); + setOutput("output_goals", goals); + } } + } else { + setOutput("output_goals", goals); } - setOutput("output_goal", goal); return child_node_->executeTick(); } @@ -82,6 +124,14 @@ void GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { last_goal_received_ = *msg; + last_goal_received_set_ = true; +} + +void +GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg) +{ + last_goals_received_ = *msg; + last_goals_received_set_ = true; } } // 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..2f70061da7f 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 @@ -88,26 +88,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; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.poses.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; + geometry_msgs::msg::PoseStampedArray 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 +120,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) R"( - + @@ -126,26 +129,39 @@ 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; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.header.stamp = goal.header.stamp; + goals.poses.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; + geometry_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; + geometry_msgs::msg::PoseStampedArray 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; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.poses.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; + geometry_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; + geometry_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; + geometry_msgs::msg::PoseStampedArray 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); } int main(int argc, char ** argv)