Skip to content

Commit bf782d2

Browse files
alexanderjyuenSteveMacenski
alexanderjyuen
and
SteveMacenski
committed
outputting tracker feedback on ComputeAndTrack BT node (#5327)
* outputting tracker feedback on BT node Signed-off-by: Alexander Yuen <[email protected]> * initializing outputs Signed-off-by: Alexander Yuen <[email protected]> * outputting last state on success Signed-off-by: Alexander Yuen <[email protected]> * linting Signed-off-by: Alexander Yuen <[email protected]> * fixed nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <[email protected]> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: alexanderjyuen <[email protected]> * consolidated function to set outputs null, only setOutput with active feedback Signed-off-by: Alexander Yuen <[email protected]> * add class to method Signed-off-by: Alexander Yuen <[email protected]> * linting Signed-off-by: Alexander Yuen <[email protected]> --------- Signed-off-by: Alexander Yuen <[email protected]> Signed-off-by: alexanderjyuen <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
1 parent 7d9ba43 commit bf782d2

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
@@ -78,6 +78,11 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
7878
void on_wait_for_result(
7979
std::shared_ptr<const Action::Feedback> feedback) override;
8080

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

111138
} // 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
@@ -128,6 +128,12 @@
128128
<output_port name="execution_time">Duration to compute route</output_port>
129129
<output_port name="error_code_id">"Compute route to pose error code"</output_port>
130130
<output_port name="error_msg">"Compute route to pose error message"</output_port>
131+
<output_port name="last_node_id">"ID of the previous node"</output_port>
132+
<output_port name="next_node_id">"ID of the next node"</output_port>
133+
<output_port name="current_edge_id">"ID of the current edge"</output_port>
134+
<output_port name="route">"List of RouteNodes to go from start to end"</output_port>
135+
<output_port name="path">"Path created by ComputeAndTrackRoute node"</output_port>
136+
<output_port name="rerouted">"Whether the plan has rerouted"</output_port>
131137
</Action>
132138

133139
<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)