Skip to content

Commit eaa88e6

Browse files
tonynajjartonynajjar
authored
Adapt GoalUpdater to update goals as well (#4771)
* Add IsStoppedBTNode Signed-off-by: Tony Najjar <[email protected]> * add topic name + reformat Signed-off-by: Tony Najjar <[email protected]> * fix comment Signed-off-by: Tony Najjar <[email protected]> * fix abs Signed-off-by: Tony Najjar <[email protected]> * remove log Signed-off-by: Tony Najjar <[email protected]> * add getter functions for raw twist Signed-off-by: Tony Najjar <[email protected]> * remove unused code Signed-off-by: Tony Najjar <[email protected]> * use odomsmoother Signed-off-by: Tony Najjar <[email protected]> * fix formatting Signed-off-by: Tony Najjar <[email protected]> * update groot Signed-off-by: Tony Najjar <[email protected]> * Add test Signed-off-by: Tony Najjar <[email protected]> * reset at success Signed-off-by: Tony Najjar <[email protected]> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <[email protected]> * Fix stopped Node Signed-off-by: Tony Najjar <[email protected]> * Add tests to odometry_utils Signed-off-by: Tony Najjar <[email protected]> * fix linting Signed-off-by: Tony Najjar <[email protected]> * Adapt goalUpdater to modify goals as well Signed-off-by: Tony Najjar <[email protected]> * fix formatting Signed-off-by: Tony Najjar <[email protected]> * fixes Signed-off-by: Tony Najjar <[email protected]> * change name of msg Signed-off-by: Tony Najjar <[email protected]> * Make input goals be Goals again for compatibility Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * Revert "fix" This reverts commit 8303cdc. Signed-off-by: Tony Najjar <[email protected]> * refactoring Signed-off-by: Tony Najjar <[email protected]> * ament Signed-off-by: Tony Najjar <[email protected]> * ignore if no timestamps Signed-off-by: Tony Najjar <[email protected]> * facepalm Signed-off-by: Tony Najjar <[email protected]> * update groot nodes Signed-off-by: Tony Najjar <[email protected]> * Use PoseStampedArray Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fixes Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * use geometry_msgs Signed-off-by: Tony Najjar <[email protected]> * fix import Signed-off-by: Tony Najjar <[email protected]> * use geometry_msgs Signed-off-by: Tony Najjar <[email protected]> * more fixes Signed-off-by: Tony Najjar <[email protected]> * . Signed-off-by: Tony Najjar <[email protected]> * revert Signed-off-by: Tony Najjar <[email protected]> * . Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * add common_interfaces Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * use PoseStampedArray Signed-off-by: Tony Najjar <[email protected]> * reformat Signed-off-by: Tony Najjar <[email protected]> * revert Signed-off-by: Tony Najjar <[email protected]> * revert Signed-off-by: Tony Najjar <[email protected]> * fix warn msg Signed-off-by: Tony Najjar <[email protected]> * fix test Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> * improve Signed-off-by: Tony Najjar <[email protected]> * fix format Signed-off-by: Tony Najjar <[email protected]> * change to info Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
1 parent 408fbb8 commit eaa88e6

File tree

4 files changed

+116
-18
lines changed

4 files changed

+116
-18
lines changed

Diff for: ‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

+17-3
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
// Copyright (c) 2018 Intel Corporation
22
// Copyright (c) 2020 Francisco Martin Rico
3+
// Copyright (c) 2024 Angsa Robotics
34
//
45
// Licensed under the Apache License, Version 2.0 (the "License");
56
// you may not use this file except in compliance with the License.
@@ -20,6 +21,7 @@
2021
#include <string>
2122

2223
#include "geometry_msgs/msg/pose_stamped.hpp"
24+
#include "geometry_msgs/msg/pose_stamped_array.hpp"
2325

2426
#include "behaviortree_cpp/decorator_node.h"
2527

@@ -52,9 +54,11 @@ class GoalUpdater : public BT::DecoratorNode
5254
{
5355
return {
5456
BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
55-
BT::OutputPort<geometry_msgs::msg::PoseStamped>(
56-
"output_goal",
57-
"Received Goal by subscription"),
57+
BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals", "Original Goals"),
58+
BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
59+
"Received Goal by subscription"),
60+
BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
61+
"Received Goals by subscription")
5862
};
5963
}
6064

@@ -71,9 +75,19 @@ class GoalUpdater : public BT::DecoratorNode
7175
*/
7276
void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
7377

78+
/**
79+
* @brief Callback function for goals update topic
80+
* @param msg Shared pointer to geometry_msgs::msg::PoseStampedArray message
81+
*/
82+
void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg);
83+
7484
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
85+
rclcpp::Subscription<geometry_msgs::msg::PoseStampedArray>::SharedPtr goals_sub_;
7586

7687
geometry_msgs::msg::PoseStamped last_goal_received_;
88+
bool last_goal_received_set_{false};
89+
geometry_msgs::msg::PoseStampedArray last_goals_received_;
90+
bool last_goals_received_set_{false};
7791

7892
rclcpp::Node::SharedPtr node_;
7993
rclcpp::CallbackGroup::SharedPtr callback_group_;

Diff for: ‎nav2_behavior_tree/nav2_tree_nodes.xml

+2
Original file line numberDiff line numberDiff line change
@@ -328,7 +328,9 @@
328328

329329
<Decorator ID="GoalUpdater">
330330
<input_port name="input_goal">Original goal in</input_port>
331+
<input_port name="input_goals">Original goals in</input_port>
331332
<output_port name="output_goal">Output goal set by subscription</output_port>
333+
<output_port name="output_goals">Output goals set by subscription</output_port>
332334
</Decorator>
333335

334336
<Decorator ID="SpeedController">

Diff for: ‎nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

+60-10
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
// Copyright (c) 2018 Intel Corporation
22
// Copyright (c) 2020 Francisco Martin Rico
3+
// Copyright (c) 2024 Angsa Robotics
34
//
45
// Licensed under the Apache License, Version 2.0 (the "License");
56
// you may not use this file except in compliance with the License.
@@ -16,7 +17,6 @@
1617
#include <string>
1718
#include <memory>
1819

19-
#include "geometry_msgs/msg/pose_stamped.hpp"
2020
#include "behaviortree_cpp/decorator_node.h"
2121

2222
#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
@@ -40,7 +40,9 @@ GoalUpdater::GoalUpdater(
4040
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
4141

4242
std::string goal_updater_topic;
43+
std::string goals_updater_topic;
4344
node_->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
45+
node_->get_parameter_or<std::string>("goals_updater_topic", goals_updater_topic, "goals_update");
4446

4547
rclcpp::SubscriptionOptions sub_option;
4648
sub_option.callback_group = callback_group_;
@@ -49,39 +51,87 @@ GoalUpdater::GoalUpdater(
4951
rclcpp::SystemDefaultsQoS(),
5052
std::bind(&GoalUpdater::callback_updated_goal, this, _1),
5153
sub_option);
54+
goals_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStampedArray>(
55+
goals_updater_topic,
56+
rclcpp::SystemDefaultsQoS(),
57+
std::bind(&GoalUpdater::callback_updated_goals, this, _1),
58+
sub_option);
5259
}
5360

5461
inline BT::NodeStatus GoalUpdater::tick()
5562
{
5663
geometry_msgs::msg::PoseStamped goal;
64+
geometry_msgs::msg::PoseStampedArray goals;
5765

5866
getInput("input_goal", goal);
67+
getInput("input_goals", goals);
5968

6069
// Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
6170
callback_group_executor_.spin_all(std::chrono::milliseconds(1));
6271
callback_group_executor_.spin_all(std::chrono::milliseconds(49));
6372

64-
if (last_goal_received_.header.stamp != rclcpp::Time(0)) {
65-
auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
66-
auto goal_time = rclcpp::Time(goal.header.stamp);
67-
if (last_goal_received_time > goal_time) {
68-
goal = last_goal_received_;
73+
if (last_goal_received_set_) {
74+
if (last_goal_received_.header.stamp == rclcpp::Time(0)) {
75+
// if the goal doesn't have a timestamp, we reject it
76+
RCLCPP_WARN(
77+
node_->get_logger(), "The received goal has no timestamp. Ignoring.");
78+
setOutput("output_goal", goal);
6979
} else {
80+
auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
81+
auto goal_time = rclcpp::Time(goal.header.stamp);
82+
if (last_goal_received_time >= goal_time) {
83+
setOutput("output_goal", last_goal_received_);
84+
} else {
85+
RCLCPP_INFO(
86+
node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
87+
"current goal (%f). Ignoring the received goal.",
88+
last_goal_received_time.seconds(), goal_time.seconds());
89+
setOutput("output_goal", goal);
90+
}
91+
}
92+
} else {
93+
setOutput("output_goal", goal);
94+
}
95+
96+
if (last_goals_received_set_) {
97+
if (last_goals_received_.poses.empty()) {
98+
setOutput("output_goals", goals);
99+
} else if (last_goals_received_.header.stamp == rclcpp::Time(0)) {
70100
RCLCPP_WARN(
71-
node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
72-
"current goal (%f). Ignoring the received goal.",
73-
last_goal_received_time.seconds(), goal_time.seconds());
101+
node_->get_logger(), "The received goals array has no timestamp. Ignoring.");
102+
setOutput("output_goals", goals);
103+
} else {
104+
auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp);
105+
auto goals_time = rclcpp::Time(goals.header.stamp);
106+
if (last_goals_received_time >= goals_time) {
107+
setOutput("output_goals", last_goals_received_);
108+
} else {
109+
RCLCPP_INFO(
110+
node_->get_logger(), "The timestamp of the received goals (%f) is older than the "
111+
"current goals (%f). Ignoring the received goals.",
112+
last_goals_received_time.seconds(), goals_time.seconds());
113+
setOutput("output_goals", goals);
114+
}
74115
}
116+
} else {
117+
setOutput("output_goals", goals);
75118
}
76119

77-
setOutput("output_goal", goal);
78120
return child_node_->executeTick();
79121
}
80122

81123
void
82124
GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
83125
{
84126
last_goal_received_ = *msg;
127+
last_goal_received_set_ = true;
128+
}
129+
130+
void
131+
GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg)
132+
{
133+
last_goals_received_ = *msg;
134+
last_goals_received_set_ = true;
85135
}
86136

87137
} // namespace nav2_behavior_tree

Diff for: ‎nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

+37-5
Original file line numberDiff line numberDiff line change
@@ -88,26 +88,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
8888
R"(
8989
<root BTCPP_format="4">
9090
<BehaviorTree ID="MainTree">
91-
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
91+
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
9292
<AlwaysSuccess/>
9393
</GoalUpdater>
9494
</BehaviorTree>
9595
</root>)";
9696

9797
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);
10098

10199
// create new goal and set it on blackboard
102100
geometry_msgs::msg::PoseStamped goal;
101+
geometry_msgs::msg::PoseStampedArray goals;
103102
goal.header.stamp = node_->now();
104103
goal.pose.position.x = 1.0;
104+
goals.poses.push_back(goal);
105105
config_->blackboard->set("goal", goal);
106+
config_->blackboard->set("goals", goals);
106107

107108
// tick tree without publishing updated goal and get updated_goal
108109
tree_->rootNode()->executeTick();
109110
geometry_msgs::msg::PoseStamped updated_goal;
111+
geometry_msgs::msg::PoseStampedArray updated_goals;
110112
EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
113+
EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
111114
}
112115

113116
TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
@@ -117,7 +120,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
117120
R"(
118121
<root BTCPP_format="4">
119122
<BehaviorTree ID="MainTree">
120-
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
123+
<GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
121124
<AlwaysSuccess/>
122125
</GoalUpdater>
123126
</BehaviorTree>
@@ -126,26 +129,39 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
126129
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
127130
auto goal_updater_pub =
128131
node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
132+
auto goals_updater_pub =
133+
node_->create_publisher<geometry_msgs::msg::PoseStampedArray>("goals_update", 10);
129134

130135
// create new goal and set it on blackboard
131136
geometry_msgs::msg::PoseStamped goal;
137+
geometry_msgs::msg::PoseStampedArray goals;
132138
goal.header.stamp = node_->now();
133139
goal.pose.position.x = 1.0;
140+
goals.header.stamp = goal.header.stamp;
141+
goals.poses.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+
geometry_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+
geometry_msgs::msg::PoseStampedArray 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<geometry_msgs::msg::PoseStampedArray>("goals_update", 10);
167185

168186
// create new goal and set it on blackboard
169187
geometry_msgs::msg::PoseStamped goal;
188+
geometry_msgs::msg::PoseStampedArray goals;
170189
goal.header.stamp = node_->now();
171190
goal.pose.position.x = 1.0;
191+
goals.poses.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+
geometry_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+
geometry_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+
geometry_msgs::msg::PoseStampedArray 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);
193225
}
194226

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

0 commit comments

Comments
 (0)