diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index dda57080644..ebced08ed7a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -80,6 +80,11 @@ class FollowPathAction : public BtActionNode void on_wait_for_result( std::shared_ptr 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 @@ -96,6 +101,8 @@ class FollowPathAction : public BtActionNode BT::InputPort("goal_checker_id", ""), BT::InputPort("progress_checker_id", ""), BT::InputPort("path_handler_id", ""), + BT::OutputPort("tracking_feedback", + "Tracking feedback from controller server"), BT::OutputPort( "error_code_id", "The follow path error code"), BT::OutputPort( diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index b62ec4a487c..e43ea0172ee 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -189,6 +189,7 @@ Path handler Action server name Server timeout + Tracking feedback from controller server Follow Path error code Follow Path error message diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 4184e68bf4c..6db8b3b09ff 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -39,6 +39,7 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { + resetFeedbackAndOutputPorts(); setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); return BT::NodeStatus::SUCCESS; @@ -46,6 +47,7 @@ BT::NodeStatus FollowPathAction::on_success() BT::NodeStatus FollowPathAction::on_aborted() { + resetFeedbackAndOutputPorts(); setOutput("error_code_id", result_.result->error_code); setOutput("error_msg", result_.result->error_msg); return BT::NodeStatus::FAILURE; @@ -53,6 +55,7 @@ BT::NodeStatus FollowPathAction::on_aborted() BT::NodeStatus FollowPathAction::on_cancelled() { + resetFeedbackAndOutputPorts(); // Set empty error code, action was cancelled setOutput("error_code_id", ActionResult::NONE); setOutput("error_msg", ""); @@ -66,7 +69,7 @@ void FollowPathAction::on_timeout() } void FollowPathAction::on_wait_for_result( - std::shared_ptr/*feedback*/) + std::shared_ptr feedback) { // Grab the new path nav_msgs::msg::Path new_path; @@ -110,6 +113,16 @@ void FollowPathAction::on_wait_for_result( goal_.path_handler_id = new_path_handler_id; goal_updated_ = true; } + + if (feedback) { + setOutput("tracking_feedback", feedback->tracking_feedback); + } +} + +void FollowPathAction::resetFeedbackAndOutputPorts() +{ + nav2_msgs::msg::TrackingFeedback empty_feedback; + setOutput("tracking_feedback", empty_feedback); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 2deecf33b53..0ac8837b38e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -200,6 +200,7 @@ TEST(FollowPathAction, testProgressCheckerIdUpdate) config->blackboard->set("progress_checker_id", std::string("new_progress_checker")); auto feedback = std::make_shared(); auto follow_path_node = dynamic_cast(tree->rootNode()); + ASSERT_NE(follow_path_node, nullptr); follow_path_node->on_wait_for_result(feedback); } @@ -239,6 +240,7 @@ TEST(FollowPathAction, testGoalCheckerIdUpdate) config->blackboard->set("goal_checker_id", std::string("new_goal_checker")); auto feedback = std::make_shared(); auto follow_path_node = dynamic_cast(tree->rootNode()); + ASSERT_NE(follow_path_node, nullptr); follow_path_node->on_wait_for_result(feedback); } @@ -278,6 +280,7 @@ TEST(FollowPathAction, testControllerIdUpdate) config->blackboard->set("controller_id", std::string("new_controller")); auto feedback = std::make_shared(); auto follow_path_node = dynamic_cast(tree->rootNode()); + ASSERT_NE(follow_path_node, nullptr); follow_path_node->on_wait_for_result(feedback); } @@ -317,6 +320,7 @@ TEST(FollowPathAction, testPathHandlerUpdate) config->blackboard->set("path_handler_id", std::string("new_path_handler")); auto feedback = std::make_shared(); auto follow_path_node = dynamic_cast(tree->rootNode()); + ASSERT_NE(follow_path_node, nullptr); follow_path_node->on_wait_for_result(feedback); } diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index 5d5310f79cd..ab0ff781fbe 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -16,7 +16,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index f2b6589d927..54048f1c171 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml index 44662cb4656..6560dc4c441 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml @@ -65,7 +65,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 2e2a1b9c456..3293b52df45 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index f6a8e08e8f9..5a00c52a20f 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -23,7 +23,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index f79984b750e..7ddbf6297dd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -27,7 +27,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index c13b2629dea..139da7d9a93 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index fa39ff05283..c500a3ca2e9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index f64063bbdcb..d3735fa9447 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index 1607057c0e2..ad56f441325 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -17,7 +17,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 9c9342e8bbd..db512b294bd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 2e43016775d..645500fe482 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -10,7 +10,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml index 4692c9e17b0..e8cb406c8df 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -64,7 +64,7 @@ - +