Skip to content

Commit b416253

Browse files
tonynajjartonynajjar
committed
Make input goals be Goals again for compatibility
Signed-off-by: Tony Najjar <[email protected]>
1 parent cbd715d commit b416253

File tree

3 files changed

+56
-13
lines changed

3 files changed

+56
-13
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -86,12 +86,11 @@ class GoalUpdater : public BT::DecoratorNode
8686
rclcpp::Subscription<nav2_msgs::msg::PoseStampedArray>::SharedPtr goals_sub_;
8787

8888
geometry_msgs::msg::PoseStamped last_goal_received_;
89-
Goals last_goals_received_;
89+
nav2_msgs::msg::PoseStampedArray last_goals_received_;
9090

9191
rclcpp::Node::SharedPtr node_;
9292
rclcpp::CallbackGroup::SharedPtr callback_group_;
9393
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
94-
std::mutex mutex_;
9594
};
9695

9796
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

+18-6
Original file line numberDiff line numberDiff line change
@@ -83,28 +83,40 @@ inline BT::NodeStatus GoalUpdater::tick()
8383
last_goal_received_time.seconds(), goal_time.seconds());
8484
}
8585
}
86+
setOutput("output_goal", goal);
8687

87-
if (!last_goals_received_.empty()) {
88-
goals = last_goals_received_;
88+
if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) {
89+
auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp);
90+
rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type());
91+
for (const auto & g : goals) {
92+
if (rclcpp::Time(g.header.stamp) > most_recent_goal_time) {
93+
most_recent_goal_time = rclcpp::Time(g.header.stamp);
94+
}
95+
}
96+
if (last_goals_received_time > most_recent_goal_time) {
97+
goals = last_goals_received_.poses;
98+
} else {
99+
RCLCPP_WARN(
100+
node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the "
101+
"current goals (oldest: %f). Ignoring the received goals.",
102+
last_goals_received_time.seconds(), most_recent_goal_time.seconds());
103+
}
89104
}
90105

91-
setOutput("output_goal", goal);
92106
setOutput("output_goals", goals);
93107
return child_node_->executeTick();
94108
}
95109

96110
void
97111
GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
98112
{
99-
std::lock_guard<std::mutex> lock(mutex_);
100113
last_goal_received_ = *msg;
101114
}
102115

103116
void
104117
GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg)
105118
{
106-
std::lock_guard<std::mutex> lock(mutex_);
107-
last_goals_received_ = msg->poses;
119+
last_goals_received_ = *msg;
108120
}
109121

110122
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

+37-5
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include "nav2_behavior_tree/utils/test_action_server.hpp"
2727
#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
2828

29+
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
2930

3031
class GoalUpdaterTestFixture : public ::testing::Test
3132
{
@@ -88,26 +89,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
8889
R"(
8990
<root BTCPP_format="4">
9091
<BehaviorTree ID="MainTree">
91-
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
92+
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
9293
<AlwaysSuccess/>
9394
</GoalUpdater>
9495
</BehaviorTree>
9596
</root>)";
9697

9798
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
98-
auto goal_updater_pub =
99-
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
10099

101100
// create new goal and set it on blackboard
102101
geometry_msgs::msg::PoseStamped goal;
102+
Goals goals;
103103
goal.header.stamp = node_->now();
104104
goal.pose.position.x = 1.0;
105+
goals.push_back(goal);
105106
config_->blackboard->set("goal", goal);
107+
config_->blackboard->set("goals", goals);
106108

107109
// tick tree without publishing updated goal and get updated_goal
108110
tree_->rootNode()->executeTick();
109111
geometry_msgs::msg::PoseStamped updated_goal;
112+
Goals updated_goals;
110113
EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
114+
EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
111115
}
112116

113117
TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
@@ -117,7 +121,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
117121
R"(
118122
<root BTCPP_format="4">
119123
<BehaviorTree ID="MainTree">
120-
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
124+
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
121125
<AlwaysSuccess/>
122126
</GoalUpdater>
123127
</BehaviorTree>
@@ -126,26 +130,38 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
126130
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
127131
auto goal_updater_pub =
128132
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
133+
auto goals_updater_pub =
134+
node_->create_publisher<nav2_msgs::msg::PoseStampedArray>("goals_update", 10);
129135

130136
// create new goal and set it on blackboard
131137
geometry_msgs::msg::PoseStamped goal;
138+
Goals goals;
132139
goal.header.stamp = node_->now();
133140
goal.pose.position.x = 1.0;
141+
goals.push_back(goal);
134142
config_->blackboard->set("goal", goal);
143+
config_->blackboard->set("goals", goals);
135144

136145
// publish updated_goal older than goal
137146
geometry_msgs::msg::PoseStamped goal_to_update;
147+
nav2_msgs::msg::PoseStampedArray goals_to_update;
138148
goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0);
139149
goal_to_update.pose.position.x = 2.0;
150+
goals_to_update.header.stamp = goal_to_update.header.stamp;
151+
goals_to_update.poses.push_back(goal_to_update);
140152

141153
goal_updater_pub->publish(goal_to_update);
154+
goals_updater_pub->publish(goals_to_update);
142155
tree_->rootNode()->executeTick();
143156
geometry_msgs::msg::PoseStamped updated_goal;
157+
Goals updated_goals;
144158
EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
159+
EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
145160

146161
// expect to succeed and not update goal
147162
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
148163
EXPECT_EQ(updated_goal, goal);
164+
EXPECT_EQ(updated_goals, goals);
149165
}
150166

151167
TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
@@ -155,7 +171,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
155171
R"(
156172
<root BTCPP_format="4">
157173
<BehaviorTree ID="MainTree">
158-
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
174+
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
159175
<AlwaysSuccess/>
160176
</GoalUpdater>
161177
</BehaviorTree>
@@ -164,32 +180,48 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
164180
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
165181
auto goal_updater_pub =
166182
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
183+
auto goals_updater_pub =
184+
node_->create_publisher<nav2_msgs::msg::PoseStampedArray>("goals_update", 10);
167185

168186
// create new goal and set it on blackboard
169187
geometry_msgs::msg::PoseStamped goal;
188+
Goals goals;
170189
goal.header.stamp = node_->now();
171190
goal.pose.position.x = 1.0;
191+
goals.push_back(goal);
172192
config_->blackboard->set("goal", goal);
193+
config_->blackboard->set("goals", goals);
173194

174195
// publish updated_goal older than goal
175196
geometry_msgs::msg::PoseStamped goal_to_update_1;
197+
nav2_msgs::msg::PoseStampedArray goals_to_update_1;
176198
goal_to_update_1.header.stamp = node_->now();
177199
goal_to_update_1.pose.position.x = 2.0;
200+
goals_to_update_1.header.stamp = goal_to_update_1.header.stamp;
201+
goals_to_update_1.poses.push_back(goal_to_update_1);
178202

179203
geometry_msgs::msg::PoseStamped goal_to_update_2;
204+
nav2_msgs::msg::PoseStampedArray goals_to_update_2;
180205
goal_to_update_2.header.stamp = node_->now();
181206
goal_to_update_2.pose.position.x = 3.0;
207+
goals_to_update_2.header.stamp = goal_to_update_2.header.stamp;
208+
goals_to_update_2.poses.push_back(goal_to_update_2);
182209

183210
goal_updater_pub->publish(goal_to_update_1);
211+
goals_updater_pub->publish(goals_to_update_1);
184212
goal_updater_pub->publish(goal_to_update_2);
213+
goals_updater_pub->publish(goals_to_update_2);
185214
tree_->rootNode()->executeTick();
186215
geometry_msgs::msg::PoseStamped updated_goal;
216+
Goals updated_goals;
187217
EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
218+
EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
188219

189220
// expect to succeed
190221
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
191222
// expect to update goal with latest goal update
192223
EXPECT_EQ(updated_goal, goal_to_update_2);
224+
EXPECT_EQ(updated_goals, goals_to_update_2.poses);
193225
}
194226

195227
int main(int argc, char ** argv)

0 commit comments

Comments
 (0)