diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp index ac155a8f06b..ecdd0ecf0ce 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp @@ -79,6 +79,11 @@ class ComputeAndTrackRouteAction : public BtActionNode feedback) override; + /** + * @brief Function to set all feedbacks and output ports to be null values + */ + void resetFeedbackAndOutputPorts(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -99,14 +104,36 @@ class ComputeAndTrackRouteAction : public BtActionNode( "use_poses", false, "Whether to use poses or IDs for start and goal"), - BT::OutputPort("execution_duration", + BT::OutputPort( + "execution_duration", "Time taken to compute and track route"), BT::OutputPort( "error_code_id", "The compute route error code"), BT::OutputPort( "error_msg", "The compute route error msg"), + BT::OutputPort( + "last_node_id", + "ID of the previous node"), + BT::OutputPort( + "next_node_id", + "ID of the next node"), + BT::OutputPort( + "current_edge_id", + "ID of current edge"), + BT::OutputPort( + "route", + "List of RouteNodes to go from start to end"), + BT::OutputPort( + "path", + "Path created by ComputeAndTrackRoute node"), + BT::OutputPort( + "rerouted", + "Whether the plan has rerouted"), }); } + +protected: + Action::Feedback feedback_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a6a0dc012d1..8ee4ed2fd62 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -135,6 +135,12 @@ Duration to compute route "Compute route to pose error code" "Compute route to pose error message" + "ID of the previous node" + "ID of the next node" + "ID of the current edge" + "List of RouteNodes to go from start to end" + "Path created by ComputeAndTrackRoute node" + "Whether the plan has rerouted" diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp index eb2edc19b76..bddc186149c 100644 --- a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp @@ -26,6 +26,14 @@ ComputeAndTrackRouteAction::ComputeAndTrackRouteAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { + nav_msgs::msg::Path empty_path; + nav2_msgs::msg::Route empty_route; + feedback_.last_node_id = 0; + feedback_.next_node_id = 0; + feedback_.current_edge_id = 0; + feedback_.route = empty_route; + feedback_.path = empty_path; + feedback_.rerouted = false; } void ComputeAndTrackRouteAction::on_tick() @@ -52,6 +60,7 @@ void ComputeAndTrackRouteAction::on_tick() BT::NodeStatus ComputeAndTrackRouteAction::on_success() { + resetFeedbackAndOutputPorts(); setOutput("execution_duration", result_.result->execution_duration); setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); @@ -60,6 +69,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_success() BT::NodeStatus ComputeAndTrackRouteAction::on_aborted() { + resetFeedbackAndOutputPorts(); setOutput("execution_duration", builtin_interfaces::msg::Duration()); setOutput("error_code_id", result_.result->error_code); setOutput("error_msg", result_.result->error_msg); @@ -68,6 +78,7 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_aborted() BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled() { + resetFeedbackAndOutputPorts(); // Set empty error code, action was cancelled setOutput("execution_duration", builtin_interfaces::msg::Duration()); setOutput("error_code_id", ActionResult::NONE); @@ -82,7 +93,7 @@ void ComputeAndTrackRouteAction::on_timeout() } void ComputeAndTrackRouteAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr feedback) { // Check for request updates to the goal bool use_poses = false, use_start = false; @@ -129,6 +140,34 @@ void ComputeAndTrackRouteAction::on_wait_for_result( if (goal_updated_) { on_tick(); } + + if (feedback) { + feedback_ = *feedback; + setOutput("last_node_id", feedback_.last_node_id); + setOutput("next_node_id", feedback_.next_node_id); + setOutput("current_edge_id", feedback_.current_edge_id); + setOutput("route", feedback_.route); + setOutput("path", feedback_.path); + setOutput("rerouted", feedback_.rerouted); + } +} + +void ComputeAndTrackRouteAction::resetFeedbackAndOutputPorts() +{ + nav_msgs::msg::Path empty_path; + nav2_msgs::msg::Route empty_route; + feedback_.last_node_id = 0; + feedback_.next_node_id = 0; + feedback_.current_edge_id = 0; + feedback_.route = empty_route; + feedback_.path = empty_path; + feedback_.rerouted = false; + setOutput("last_node_id", feedback_.last_node_id); + setOutput("next_node_id", feedback_.next_node_id); + setOutput("current_edge_id", feedback_.current_edge_id); + setOutput("route", feedback_.route); + setOutput("path", feedback_.path); + setOutput("rerouted", feedback_.rerouted); } } // namespace nav2_behavior_tree