Skip to content

Commit b815d16

Browse files
zz990099zz990099
authored
Make NavigateThroughPoses navigator report waypoint statuses information. (fix #4846) (#4994)
* Add WaypointStatus message type & Remove MissedWaypoint message type Signed-off-by: zz990099 <[email protected]> * Add waypoint_statuses records in RemoveInCollisionGoals/RemovePassedGoals actions Signed-off-by: zz990099 <[email protected]> * Add validation tests Signed-off-by: zz990099 <[email protected]> * Make NavigateThroughPoses navigator report waypoint statuses information Signed-off-by: zz990099 <[email protected]> * Make WaypointFollower use WaypointStatus message type in action result Signed-off-by: zz990099 <[email protected]> * Split MISSED status into SKIPPED & FAILED Signed-off-by: zz990099 <[email protected]> * fix edge case handle in RemoveInCollision/RemovePassed BT actions Signed-off-by: zz990099 <[email protected]> * Add validation test cases for RemoveInCollision&RemovePassed BT actions Signed-off-by: zz990099 <[email protected]> * Update RemovePassedGoals/RemoveInCollisionGoals node in Groot XML Signed-off-by: zz990099 <[email protected]> --------- Signed-off-by: zz990099 <[email protected]>
1 parent d518dd3 commit b815d16

File tree

20 files changed

+442
-20
lines changed

20 files changed

+442
-20
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include "nav_msgs/msg/goals.hpp"
2424
#include "nav2_behavior_tree/bt_service_node.hpp"
2525
#include "nav2_msgs/srv/get_costs.hpp"
26+
#include "nav2_msgs/msg/waypoint_status.hpp"
2627

2728
namespace nav2_behavior_tree
2829
{
@@ -63,6 +64,10 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
6364
"Whether to consider unknown cost as obstacle"),
6465
BT::OutputPort<nav_msgs::msg::Goals>("output_goals",
6566
"Goals with in-collision goals removed"),
67+
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
68+
"Original waypoint_statuses to mark waypoint status from"),
69+
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
70+
"Waypoint_statuses with in-collision waypoints marked")
6671
});
6772
}
6873

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include "nav2_util/robot_utils.hpp"
2525
#include "behaviortree_cpp/action_node.h"
2626
#include "nav2_behavior_tree/bt_utils.hpp"
27+
#include "nav2_msgs/msg/waypoint_status.hpp"
2728

2829
namespace nav2_behavior_tree
2930
{
@@ -49,6 +50,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
4950
"Goals with passed viapoints removed"),
5051
BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
5152
BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),
53+
BT::InputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("input_waypoint_statuses",
54+
"Original waypoint_statuses to mark waypoint status from"),
55+
BT::OutputPort<std::vector<nav2_msgs::msg::WaypointStatus>>("output_waypoint_statuses",
56+
"Waypoint_statuses with passed waypoints marked")
5257
};
5358
}
5459

@@ -58,6 +63,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
5863

5964
double viapoint_achieved_radius_;
6065
double transform_tolerance_;
66+
rclcpp::Node::SharedPtr node_;
6167
std::shared_ptr<tf2_ros::Buffer> tf_;
6268
std::string robot_base_frame_;
6369
};

‎nav2_behavior_tree/nav2_tree_nodes.xml

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,9 @@
102102
<input_port name="input_goals">Input goals to remove if passed</input_port>
103103
<input_port name="radius">Radius tolerance on a goal to consider it passed</input_port>
104104
<input_port name="robot_base_frame">Robot base frame</input_port>
105+
<input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
105106
<output_port name="output_goals">Set of goals after removing any passed</output_port>
107+
<output_port name="output_waypoint_statuses">Waypoint_statuses with passed waypoints marked</output_port>
106108
</Action>
107109

108110
<Action ID="RemoveInCollisionGoals">
@@ -111,7 +113,9 @@
111113
<input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
112114
<input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
113115
<input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
116+
<input_port name="input_waypoint_statuses">Original waypoint_statuses to mark waypoint status from</input_port>
114117
<output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
118+
<output_port name="output_waypoint_statuses">Waypoint_statuses with in-collision waypoints marked</output_port>
115119
</Action>
116120

117121
<Action ID="SmoothPath">

‎nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "nav2_behavior_tree/bt_utils.hpp"
2121
#include "tf2/utils.hpp"
2222
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
23+
#include "nav2_util/geometry_utils.hpp"
2324

2425
namespace nav2_behavior_tree
2526
{
@@ -63,12 +64,29 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
6364
return BT::NodeStatus::FAILURE;
6465
}
6566

67+
// get the `waypoint_statuses` vector
68+
std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
69+
auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
70+
if (!waypoint_statuses_get_res) {
71+
RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
72+
}
73+
6674
nav_msgs::msg::Goals valid_goal_poses;
6775
for (size_t i = 0; i < response->costs.size(); ++i) {
6876
if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
6977
response->costs[i] < cost_threshold_)
7078
{
7179
valid_goal_poses.goals.push_back(input_goals_.goals[i]);
80+
} else if (waypoint_statuses_get_res) {
81+
using namespace nav2_util::geometry_utils; // NOLINT
82+
auto cur_waypoint_index =
83+
find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, input_goals_.goals[i]);
84+
if (cur_waypoint_index == -1) {
85+
RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
86+
return BT::NodeStatus::FAILURE;
87+
}
88+
waypoint_statuses[cur_waypoint_index].waypoint_status =
89+
nav2_msgs::msg::WaypointStatus::SKIPPED;
7290
}
7391
}
7492
// Inform if all goals have been removed
@@ -78,6 +96,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
7896
"All goals are in collision and have been removed from the list");
7997
}
8098
setOutput("output_goals", valid_goal_poses);
99+
// set `waypoint_statuses` output
100+
setOutput("output_waypoint_statuses", waypoint_statuses);
101+
81102
return BT::NodeStatus::SUCCESS;
82103
}
83104

‎nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

Lines changed: 24 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -36,11 +36,11 @@ void RemovePassedGoals::initialize()
3636
getInput("radius", viapoint_achieved_radius_);
3737

3838
tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
39-
auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
40-
node->get_parameter("transform_tolerance", transform_tolerance_);
39+
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
40+
node_->get_parameter("transform_tolerance", transform_tolerance_);
4141

4242
robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
43-
node, "robot_base_frame", this);
43+
node_, "robot_base_frame", this);
4444
}
4545

4646
inline BT::NodeStatus RemovePassedGoals::tick()
@@ -67,6 +67,13 @@ inline BT::NodeStatus RemovePassedGoals::tick()
6767
return BT::NodeStatus::FAILURE;
6868
}
6969

70+
// get the `waypoint_statuses` vector
71+
std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
72+
auto waypoint_statuses_get_res = getInput("input_waypoint_statuses", waypoint_statuses);
73+
if (!waypoint_statuses_get_res) {
74+
RCLCPP_ERROR(node_->get_logger(), "Missing [input_waypoint_statuses] port input!");
75+
}
76+
7077
double dist_to_goal;
7178
while (goal_poses.goals.size() > 1) {
7279
dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);
@@ -75,10 +82,24 @@ inline BT::NodeStatus RemovePassedGoals::tick()
7582
break;
7683
}
7784

85+
// mark waypoint statuses before the goal is erased from goals
86+
if (waypoint_statuses_get_res) {
87+
auto cur_waypoint_index =
88+
find_next_matching_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]);
89+
if (cur_waypoint_index == -1) {
90+
RCLCPP_ERROR(node_->get_logger(), "Failed to find matching goal in waypoint_statuses");
91+
return BT::NodeStatus::FAILURE;
92+
}
93+
waypoint_statuses[cur_waypoint_index].waypoint_status =
94+
nav2_msgs::msg::WaypointStatus::COMPLETED;
95+
}
96+
7897
goal_poses.goals.erase(goal_poses.goals.begin());
7998
}
8099

81100
setOutput("output_goals", goal_poses);
101+
// set `waypoint_statuses` output
102+
setOutput("output_waypoint_statuses", waypoint_statuses);
82103

83104
return BT::NodeStatus::SUCCESS;
84105
}

‎nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -181,6 +181,139 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su
181181
EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
182182
}
183183

184+
TEST_F(RemoveInCollisionGoalsTestFixture,
185+
test_tick_remove_in_collision_goals_success_and_output_waypoint_statues)
186+
{
187+
// create tree
188+
std::string xml_txt =
189+
R"(
190+
<root BTCPP_format="4">
191+
<BehaviorTree ID="MainTree">
192+
<RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
193+
input_goals="{goals}" output_goals="{goals}"
194+
cost_threshold="253"
195+
input_waypoint_statuses="{waypoint_statuses}"
196+
output_waypoint_statuses="{waypoint_statuses}"/>
197+
</BehaviorTree>
198+
</root>)";
199+
200+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
201+
202+
// create new goal and set it on blackboard
203+
nav_msgs::msg::Goals poses;
204+
poses.goals.resize(4);
205+
poses.goals[0].pose.position.x = 0.0;
206+
poses.goals[0].pose.position.y = 0.0;
207+
208+
poses.goals[1].pose.position.x = 0.5;
209+
poses.goals[1].pose.position.y = 0.0;
210+
211+
poses.goals[2].pose.position.x = 1.0;
212+
poses.goals[2].pose.position.y = 0.0;
213+
214+
poses.goals[3].pose.position.x = 2.0;
215+
poses.goals[3].pose.position.y = 0.0;
216+
217+
config_->blackboard->set("goals", poses);
218+
219+
// create waypoint_statuses and set it on blackboard
220+
std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
221+
for (size_t i = 0 ; i < waypoint_statuses.size() ; ++i) {
222+
waypoint_statuses[i].waypoint_pose = poses.goals[i];
223+
waypoint_statuses[i].waypoint_index = i;
224+
}
225+
config_->blackboard->set("waypoint_statuses", waypoint_statuses);
226+
227+
// tick until node is not running
228+
tree_->rootNode()->executeTick();
229+
while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
230+
tree_->rootNode()->executeTick();
231+
}
232+
233+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
234+
// check that it removed the point in range
235+
nav_msgs::msg::Goals output_poses;
236+
EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
237+
238+
EXPECT_EQ(output_poses.goals.size(), 3u);
239+
EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
240+
EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
241+
EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
242+
243+
// check the waypoint_statuses
244+
std::vector<nav2_msgs::msg::WaypointStatus> output_waypoint_statuses;
245+
EXPECT_TRUE(config_->blackboard->get("waypoint_statuses", output_waypoint_statuses));
246+
EXPECT_EQ(output_waypoint_statuses.size(), 4u);
247+
EXPECT_EQ(output_waypoint_statuses[0].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
248+
EXPECT_EQ(output_waypoint_statuses[1].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
249+
EXPECT_EQ(output_waypoint_statuses[2].waypoint_status, nav2_msgs::msg::WaypointStatus::PENDING);
250+
EXPECT_EQ(output_waypoint_statuses[3].waypoint_status, nav2_msgs::msg::WaypointStatus::SKIPPED);
251+
}
252+
253+
TEST_F(RemoveInCollisionGoalsTestFixture,
254+
test_tick_remove_in_collision_goals_find_matching_waypoint_fail)
255+
{
256+
// create tree
257+
std::string xml_txt =
258+
R"(
259+
<root BTCPP_format="4">
260+
<BehaviorTree ID="MainTree">
261+
<RemoveInCollisionGoals service_name="/global_costmap/get_cost_global_costmap"
262+
input_goals="{goals}" output_goals="{goals}"
263+
cost_threshold="253"
264+
input_waypoint_statuses="{waypoint_statuses}"
265+
output_waypoint_statuses="{waypoint_statuses}"/>
266+
</BehaviorTree>
267+
</root>)";
268+
269+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
270+
271+
// create new goal and set it on blackboard
272+
nav_msgs::msg::Goals poses;
273+
poses.goals.resize(4);
274+
poses.goals[0].pose.position.x = 0.0;
275+
poses.goals[0].pose.position.y = 0.0;
276+
277+
poses.goals[1].pose.position.x = 0.5;
278+
poses.goals[1].pose.position.y = 0.0;
279+
280+
poses.goals[2].pose.position.x = 1.0;
281+
poses.goals[2].pose.position.y = 0.0;
282+
283+
poses.goals[3].pose.position.x = 2.0;
284+
poses.goals[3].pose.position.y = 0.0;
285+
286+
config_->blackboard->set("goals", poses);
287+
288+
// create waypoint_statuses and set it on blackboard
289+
std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses(poses.goals.size());
290+
for (size_t i = 0 ; i < waypoint_statuses.size() ; ++i) {
291+
waypoint_statuses[i].waypoint_pose = poses.goals[i];
292+
waypoint_statuses[i].waypoint_index = i;
293+
}
294+
// inconsistency between waypoint_statuses and poses
295+
waypoint_statuses[3].waypoint_pose.pose.position.x = 0.0;
296+
297+
config_->blackboard->set("waypoint_statuses", waypoint_statuses);
298+
299+
// tick until node is not running
300+
tree_->rootNode()->executeTick();
301+
while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) {
302+
tree_->rootNode()->executeTick();
303+
}
304+
305+
// check that it failed and returned the original goals
306+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
307+
nav_msgs::msg::Goals output_poses;
308+
EXPECT_TRUE(config_->blackboard->get("goals", output_poses));
309+
310+
EXPECT_EQ(output_poses.goals.size(), 4u);
311+
EXPECT_EQ(output_poses.goals[0], poses.goals[0]);
312+
EXPECT_EQ(output_poses.goals[1], poses.goals[1]);
313+
EXPECT_EQ(output_poses.goals[2], poses.goals[2]);
314+
EXPECT_EQ(output_poses.goals[3], poses.goals[3]);
315+
}
316+
184317
TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail)
185318
{
186319
// create tree

0 commit comments

Comments
 (0)