Skip to content

Commit

Permalink
Revert "Adapt GoalUpdater to update goals as well (#4771)"
Browse files Browse the repository at this point in the history 
This reverts commit 55d7387.
  • Loading branch information
@SteveMacenski
SteveMacenski committed Feb 5, 2025
1 parent 2a848d9 commit d14589c
Show file tree
Hide file tree
Showing 4 changed files with 18 additions and 116 deletions.
20 changes: 3 additions & 17 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
@@ -1,6 +1,5 @@
// 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.
Expand All @@ -21,7 +20,6 @@
#include <string>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped_array.hpp"

#include "behaviortree_cpp/decorator_node.h"

Expand Down Expand Up @@ -54,11 +52,9 @@ class GoalUpdater : public BT::DecoratorNode
{
return {
BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals", "Original Goals"),
BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
"Received Goal by subscription"),
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
"Received Goals by subscription")
BT::OutputPort<geometry_msgs::msg::PoseStamped>(
"output_goal",
"Received Goal by subscription"),
};
}

Expand All @@ -75,19 +71,9 @@ 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<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStampedArray>::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_;
Expand Down
2 changes: 0 additions & 2 deletions nav2_behavior_tree/nav2_tree_nodes.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,9 +312,7 @@

<Decorator ID="GoalUpdater">
<input_port name="input_goal">Original goal in</input_port>
<input_port name="input_goals">Original goals in</input_port>
<output_port name="output_goal">Output goal set by subscription</output_port>
<output_port name="output_goals">Output goals set by subscription</output_port>
</Decorator>

<Decorator ID="SpeedController">
Expand Down
70 changes: 10 additions & 60 deletions nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
// 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.
Expand All @@ -17,6 +16,7 @@
#include <string>
#include <memory>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp/decorator_node.h"

#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
Expand All @@ -40,9 +40,7 @@ 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<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
node_->get_parameter_or<std::string>("goals_updater_topic", goals_updater_topic, "goals_update");

rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
Expand All @@ -51,87 +49,39 @@ GoalUpdater::GoalUpdater(
rclcpp::SystemDefaultsQoS(),
std::bind(&GoalUpdater::callback_updated_goal, this, _1),
sub_option);
goals_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStampedArray>(
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_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);
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 {
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 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);
}
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());
}
} else {
setOutput("output_goals", goals);
}

setOutput("output_goal", goal);
return child_node_->executeTick();
}

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
Expand Down
42 changes: 5 additions & 37 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 @@ -88,29 +88,26 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<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;
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)
Expand All @@ -120,7 +117,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
Expand All @@ -129,39 +126,26 @@ 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<geometry_msgs::msg::PoseStampedArray>("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)
Expand All @@ -171,7 +155,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
R"(
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
Expand All @@ -180,48 +164,32 @@ 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<geometry_msgs::msg::PoseStampedArray>("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)
Expand Down

0 comments on commit d14589c

Please sign in to comment.