Skip to content

Commit

Permalink
Make input goals be Goals again for compatibility
Browse files Browse the repository at this point in the history 
Signed-off-by: Tony Najjar <[email protected]>
  • Loading branch information
@tonynajjar
tonynajjar committed Dec 5, 2024
1 parent cbd715d commit b416253
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 13 deletions.
3 changes: 1 addition & 2 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,12 +86,11 @@ class GoalUpdater : public BT::DecoratorNode
rclcpp::Subscription<nav2_msgs::msg::PoseStampedArray>::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
Expand Down
24 changes: 18 additions & 6 deletions nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,28 +83,40 @@ 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();
}

void
GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
last_goal_received_ = *msg;
}

void
GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
last_goals_received_ = msg->poses;
last_goals_received_ = *msg;
}

} // namespace nav2_behavior_tree
Expand Down
42 changes: 37 additions & 5 deletions nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs::msg::PoseStamped> Goals;

class GoalUpdaterTestFixture : public ::testing::Test
{
Expand Down Expand Up @@ -88,26 +89,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
auto goal_updater_pub =
node_->create_publisher<geometry_msgs::msg::PoseStamped>("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)
Expand All @@ -117,7 +121,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
Expand All @@ -126,26 +130,38 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
auto goal_updater_pub =
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
auto goals_updater_pub =
node_->create_publisher<nav2_msgs::msg::PoseStampedArray>("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)
Expand All @@ -155,7 +171,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
Expand All @@ -164,32 +180,48 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
auto goal_updater_pub =
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
auto goals_updater_pub =
node_->create_publisher<nav2_msgs::msg::PoseStampedArray>("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)
Expand Down

0 comments on commit b416253

Please sign in to comment.