Skip to content

Commit dc92138

Browse files
alexanderjyuenSteveMacenski
alexanderjyuen
and
SteveMacenski
authored
outputting tracker feedback on ComputeAndTrack BT node (#5327)
* outputting tracker feedback on BT node Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * initializing outputs Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * outputting last state on success Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * fixed nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> * consolidated function to set outputs null, only setOutput with active feedback Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * add class to method Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> --------- Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent 12ceabe commit dc92138

File tree

3 files changed

+74
-2
lines changed

3 files changed

+74
-2
lines changed

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

Lines changed: 28 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,11 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
7979
void on_wait_for_result(
8080
std::shared_ptr<const Action::Feedback> feedback) override;
8181

82+
/**
83+
* @brief Function to set all feedbacks and output ports to be null values
84+
*/
85+
void resetFeedbackAndOutputPorts();
86+
8287
/**
8388
* @brief Creates list of BT ports
8489
* @return BT::PortsList Containing basic ports along with node-specific ports
@@ -99,14 +104,36 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
99104
"Whether to use the start pose or the robot's current pose"),
100105
BT::InputPort<bool>(
101106
"use_poses", false, "Whether to use poses or IDs for start and goal"),
102-
BT::OutputPort<builtin_interfaces::msg::Duration>("execution_duration",
107+
BT::OutputPort<builtin_interfaces::msg::Duration>(
108+
"execution_duration",
103109
"Time taken to compute and track route"),
104110
BT::OutputPort<ActionResult::_error_code_type>(
105111
"error_code_id", "The compute route error code"),
106112
BT::OutputPort<std::string>(
107113
"error_msg", "The compute route error msg"),
114+
BT::OutputPort<uint16_t>(
115+
"last_node_id",
116+
"ID of the previous node"),
117+
BT::OutputPort<uint16_t>(
118+
"next_node_id",
119+
"ID of the next node"),
120+
BT::OutputPort<uint16_t>(
121+
"current_edge_id",
122+
"ID of current edge"),
123+
BT::OutputPort<nav2_msgs::msg::Route>(
124+
"route",
125+
"List of RouteNodes to go from start to end"),
126+
BT::OutputPort<nav_msgs::msg::Path>(
127+
"path",
128+
"Path created by ComputeAndTrackRoute node"),
129+
BT::OutputPort<bool>(
130+
"rerouted",
131+
"Whether the plan has rerouted"),
108132
});
109133
}
134+
135+
protected:
136+
Action::Feedback feedback_;
110137
};
111138

112139
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/nav2_tree_nodes.xml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,12 @@
135135
<output_port name="execution_time">Duration to compute route</output_port>
136136
<output_port name="error_code_id">"Compute route to pose error code"</output_port>
137137
<output_port name="error_msg">"Compute route to pose error message"</output_port>
138+
<output_port name="last_node_id">"ID of the previous node"</output_port>
139+
<output_port name="next_node_id">"ID of the next node"</output_port>
140+
<output_port name="current_edge_id">"ID of the current edge"</output_port>
141+
<output_port name="route">"List of RouteNodes to go from start to end"</output_port>
142+
<output_port name="path">"Path created by ComputeAndTrackRoute node"</output_port>
143+
<output_port name="rerouted">"Whether the plan has rerouted"</output_port>
138144
</Action>
139145

140146
<Action ID="RemovePassedGoals">

‎nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp

Lines changed: 40 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,14 @@ ComputeAndTrackRouteAction::ComputeAndTrackRouteAction(
2626
const BT::NodeConfiguration & conf)
2727
: BtActionNode<Action>(xml_tag_name, action_name, conf)
2828
{
29+
nav_msgs::msg::Path empty_path;
30+
nav2_msgs::msg::Route empty_route;
31+
feedback_.last_node_id = 0;
32+
feedback_.next_node_id = 0;
33+
feedback_.current_edge_id = 0;
34+
feedback_.route = empty_route;
35+
feedback_.path = empty_path;
36+
feedback_.rerouted = false;
2937
}
3038

3139
void ComputeAndTrackRouteAction::on_tick()
@@ -52,6 +60,7 @@ void ComputeAndTrackRouteAction::on_tick()
5260

5361
BT::NodeStatus ComputeAndTrackRouteAction::on_success()
5462
{
63+
resetFeedbackAndOutputPorts();
5564
setOutput("execution_duration", result_.result->execution_duration);
5665
setOutput("error_code_id", ActionResult::NONE);
5766
setOutput("error_msg", "");
@@ -60,6 +69,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_success()
6069

6170
BT::NodeStatus ComputeAndTrackRouteAction::on_aborted()
6271
{
72+
resetFeedbackAndOutputPorts();
6373
setOutput("execution_duration", builtin_interfaces::msg::Duration());
6474
setOutput("error_code_id", result_.result->error_code);
6575
setOutput("error_msg", result_.result->error_msg);
@@ -68,6 +78,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_aborted()
6878

6979
BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled()
7080
{
81+
resetFeedbackAndOutputPorts();
7182
// Set empty error code, action was cancelled
7283
setOutput("execution_duration", builtin_interfaces::msg::Duration());
7384
setOutput("error_code_id", ActionResult::NONE);
@@ -82,7 +93,7 @@ void ComputeAndTrackRouteAction::on_timeout()
8293
}
8394

8495
void ComputeAndTrackRouteAction::on_wait_for_result(
85-
std::shared_ptr<const Action::Feedback>/*feedback*/)
96+
std::shared_ptr<const Action::Feedback> feedback)
8697
{
8798
// Check for request updates to the goal
8899
bool use_poses = false, use_start = false;
@@ -129,6 +140,34 @@ void ComputeAndTrackRouteAction::on_wait_for_result(
129140
if (goal_updated_) {
130141
on_tick();
131142
}
143+
144+
if (feedback) {
145+
feedback_ = *feedback;
146+
setOutput("last_node_id", feedback_.last_node_id);
147+
setOutput("next_node_id", feedback_.next_node_id);
148+
setOutput("current_edge_id", feedback_.current_edge_id);
149+
setOutput("route", feedback_.route);
150+
setOutput("path", feedback_.path);
151+
setOutput("rerouted", feedback_.rerouted);
152+
}
153+
}
154+
155+
void ComputeAndTrackRouteAction::resetFeedbackAndOutputPorts()
156+
{
157+
nav_msgs::msg::Path empty_path;
158+
nav2_msgs::msg::Route empty_route;
159+
feedback_.last_node_id = 0;
160+
feedback_.next_node_id = 0;
161+
feedback_.current_edge_id = 0;
162+
feedback_.route = empty_route;
163+
feedback_.path = empty_path;
164+
feedback_.rerouted = false;
165+
setOutput("last_node_id", feedback_.last_node_id);
166+
setOutput("next_node_id", feedback_.next_node_id);
167+
setOutput("current_edge_id", feedback_.current_edge_id);
168+
setOutput("route", feedback_.route);
169+
setOutput("path", feedback_.path);
170+
setOutput("rerouted", feedback_.rerouted);
132171
}
133172

134173
} // namespace nav2_behavior_tree

0 commit comments

Comments
 (0)